Switch Missions 'on the fly'

I coded a script that is just a little like that of @hwurzburg ardupilot/MissionSelector.lua at master · ArduPilot/ardupilot · GitHub
but it is able to switch between three Missions ‘on the fly’ - e.g. in mode-FBWA.
It checks the mode and mode_reason to prevent a loading e.g. in mode-Thermal, who’s switching back to mode-AUTO if no thermals are detected.
It would be trivial to increase the amount of Missions by using e.g. an analog RC-input or a multi-position-switch, but that’s been not my aim.

I tested that script in SITL Plane and used it in some real flights with no problems. It should work on all vehicles.

I would be happy if some ‘lua-masters’ like @WickedShell or @hwurzburg or everybody else could have a look on it and give me feedback on dangerous things or constructs I could not see yet or things I could improve.

ThreeMissions(1.1).lua (3.7 KB)

1 Like

Can you more specifically protect against the thermal case with SOARING_THERMAL_DETECTED (or are the other edge cases that you guard against with present logic)?

Do you want to be able to switch missions even after mission.MISSION_COMPLETE?

Do you want to be able to switch missions while disarmed when arming in AUTO is allowed?

May want to wrap scripting_rc:get_aux_switch_pos() in an assert statement to avoid script crashes if the user has not set an appropriate scripting function.

It might be smart to include an “enable” switch for this, since perhaps a user would want to avoid accidentally overwriting a mission that is uploaded via GCS. Alternatively, if you use a multi-position switch, one position could be a “user” position that reloads the last uploaded mission (you’d have to store the present mission in memory for that to occur, which might be more problematic than useful, but maybe worth a try).

Minor nitpicks:
In the interest of code readability (or maintenance), it may be nice to include a function like is_mission_change_allowed rather than the lengthy logic statement.

Again, just for readability, you might have a look at string.format() rather than concatenating multiple values. Such as:
local file_name = string.format('%s/Mission#%d.waypoints', SUB_DIR, sw_pos)

(incidentally, you have declared file and file_name as global in your script, and your notes in the comments section are missing the “#” that you use in the expected filename in the code, and you can declare sw_pos as local to the update_switch function)

1 Like

Thank you for your feedback and hints @Yuri_Rage
I will have a closer look on that things you mentioned and will improve the script.

Another possibility:

Use a single momentary button to cycle through missions in sequence. Short press to increment the file number. Long press to execute.

You could implement by polling sw_pos twice across a period of 250-500ms. If it has been released, increment. If it is still depressed, load and execute the selected mission. In that case,you may want to increase the loop rate and use millis() to determine button press timeout in order to avoid perceived ambiguous timing.

This has the added benefit of a confirmation-like user experience without the constraint of only 3 missions from which to select.

The downside is that you’d want passthrough telemetry on the transmitter or a view of a GCS messages screen in order to see which mission has been selected (via gcs:send_text()). If the intent is to operate without those things, it would be difficult for a user to discern which mission has been selected.

If you wanted to get a little fancier, you could add a custom parameter to allow a user to choose between a momentary button or a multi-position switch.

Just a thought!

2 Likes

I would prefer to make the basic-function waterproof and the script easy readable - that’s duty.

To expand the amount of missions or the way you select them is freestyle and depends on the personal preference.

Just for grins, here’s an example that adds a couple of user parameters and allows for using a multi-position switch or a momentary switch as I described above.

Set MLT_FUNCTION to whatever scripting option you intend to use for the RC channel (300).
MLT_POSITIONS = 0 disables switch functionality.
MLT_POSITIONS = 1 uses momentary switch logic.
Otherwise, set MLT_POSITIONS to the number of discrete switch positions available.

Tested in SITL with momentary logic, 2, 3, and 6 positions.

multi-select.lua (3.9 KB)

No offense taken if you don’t wish to incorporate such logic - it was an interesting exercise, regardless!

1 Like

Yes, I had to grin…
That’s a great example with all possibilities used, but for me as lua starter it’s a little overloaded.
It’s very helpful for me to see how some things are managed in an easy and stringent way.
It’s the eternal conflict of scripting - should it be smart and self tailored or should it be complex with no need to edit.

That’s the improved script - with many thanks to @Yuri_Rage
ThreeMissions(1.2.1).lua (4.7 KB)

Feedback and hints are welcome

I see you incorporated nearly all of the feedback I gave above. I wasn’t suggesting that you absolutely shouldn’t allow for a mission switch while disarmed or after mission complete. I was simply posing questions as to the intent. I can envision use cases where a user might want to load a new mission in nearly any state other than some sort of failsafe/RTL scenario. It’s of course up to you to decide the implementation and where you want to guard against accidental mission uploads.

I haven’t done a very deep dive on all of your logic, but mission.MISSION_COMPLETE is already an available enumeration, so you do not need to declare a variable for it.

Also, you have an extremely long embedded series of assert statements that should likely be broken out into a couple of lines for better readability.

If you wanted to use my example, you’d only have to flesh out the file loading function and safeguards. The rest of the logic should be reasonably complete, though you’d be well advised to thoroughly test and make sure I haven’t introduced an error of my own!

This is making me feel lua envy. Questions:

  • If I had multiple missions with different DO_LAND_START commands set up could that be used for storing different auto-RTL landings on my regular flying sites? ie a west landing or east landing depending on the day?
  • How do you load these different mission files at one time? Or where do the .waypoint files need to be stored to be loaded?

I too - it would be possible to: switch to FBWB - change Mission - switch to Auto.
But my script shouldn’t be overloaded.

Maybe I will realize that in combination with my “RelocateMission” script.
Here an example with an Eight-, a Square- and a Triangle-Mission that I can place where I want to (real flight):

Yes, just copy your Mission and change the WPs after DO_LAND_START

In real flight the three Missions are stored on SD-Card “/APM/scripts/Mission#0.waypoints”, “/APM/scripts/Mission#1.waypoints”, “/APM/scripts/Mission#2.waypoints”, that means, the Missions and the lua-script are in the same subdir.

(uups - I just recognized to have a writing error in the headlines of my script - will be corrected immediatelly)
ThreeMissions(1.2.1).lua (4.7 KB)

2 Likes

Thanks! Weather this weekend is looking pretty bad so I may have to try some SITL testing.

Hi @Quaxwilly @Yuri_Rage . I tried to use your script. I haven 't used scripts on Ardupilot before . I have some questions. As I understood from the instructions, I assigned a channel (RC15) and assigned it the value 300 in RC15_OPTION. Then I created 3 missions and saved them to a folder along with the script.
When the autopilot starts, messages come out:
7.11.2022 14:53:18 : r empty
17.11.2022 14:53:18 : ons: ./scripts//Mission#0.waypoints not existing o
17.11.2022 14:53:18 : Lua: /APM/scripts/ThreeMissions.lua:60: ThreeMissi
17.11.2022 14:53:08 : r empty
17.11.2022 14:53:08 : ons: ./scripts//Mission#0.waypoints not existing o
17.11.2022 14:53:08 : Lua: /APM/scripts/ThreeMissions.lua:60: ThreeMissi
17.11.2022 14:52:58 : r empty
17.11.2022 14:52:58 : ons: ./scripts//Mission#0.waypoints not existing o
17.11.2022 14:52:58 : Lua: /APM/scripts/ThreeMissions.lua:60: ThreeMissi
17.11.2022 14:52:52 : IMU0: fast sampling enabled 8.0kHz/1.0kHz
17.11.2022 14:52:52 : RCOut: PWM:1-14
17.11.2022 14:52:52 : fmuv3 00180028 3430510E 37343438
17.11.2022 14:52:52 : ChibiOS: 38022f4f
17.11.2022 14:52:52 : ArduRover V4.2.3 (2172cfb3)
17.11.2022 14:52:52 : IMU0: fast sampling enabled 8.0kHz/1.0kHz
17.11.2022 14:52:52 : RCOut: PWM:1-14
17.11.2022 14:52:52 : fmuv3 00180028 3430510E 37343438
17.11.2022 14:52:52 : ChibiOS: 38022f4f
17.11.2022 14:52:52 : ArduRover V4.2.3 (2172cfb3)
17.11.2022 14:52:51 : IMU0: fast sampling enabled 8.0kHz/1.0kHz
17.11.2022 14:52:51 : RCOut: PWM:1-14
17.11.2022 14:52:51 : fmuv3 00180028 3430510E 37343438
17.11.2022 14:52:51 : ChibiOS: 38022f4f
17.11.2022 14:52:51 : ArduRover V4.2.3 (2172cfb3)
17.11.2022 14:52:50 : u-blox 1 HW: 00190000 SW: EXT CORE 4.04 (7f89f7)
17.11.2022 14:52:50 : r empty
17.11.2022 14:52:50 : ons: ./scripts//Mission#0.waypoints not existing o
17.11.2022 14:52:50 : Lua: /APM/scripts/ThreeMissions.lua:60: ThreeMissi
Changing the position on channel 15 (I have it at 6 positions), nothing happens. What could be the problem? And another question is in what condition should the autopilot be - AUTO, ARMED, MANUAL or another?
UPD
I didn’t pay attention that this is for a planer, and I have a boat, could there be a reason for this?
I want previously saved missions to load when the position changes on the assigned channel.

@McKey
Are you testing on SITL or on real FC?
If you are on real FC, you have to change accordingly

-------- USER 'CONSTANTS' - change here, if necessary --------
local SUB_DIR               = './scripts/' -- that's where to store the Missions - for SITL: './scripts/' - for SD-Card: '/APM/scripts/'

So change to
local SUB_DIR = '/APM/scripts'

(Btw the slash at the end of the SUB_DIR is no problem, but it’s not absolutely correct)

As a simplyfication: Arming is not necessary, but you shouldn’t be in AUTO

Here is the decision code block:

  if ((mode ~= MODE_AUTO) 
   and
   ((mode_reason == REASON_RC) or (mode_reason == REASON_GCS) or (mode_reason == REASON_INITIALIZED) or (not arming:is_armed())))
   or
   (mission:state() == MISSION_COMPLETE)
  then
    return true

Would be nice if you could give a short feedback of your test.

Edit: The lua should run on every vehicle

@Quaxwilly, it worked out! Thank you very much I fixed it, I tried on Pixhawk 2.4.8.


I have a switch for 6 positions, is it possible to redo the script for 5 missions instead of 3?

Yes, I reworked the script to be more flexible on number of missions and find automatically the correct subdir.
But be aware that some things changed like subdir where to store the missions and the parameters SCR_USER1 and …2 where you have to store your preferences.
Just have a look at the headlines of the lua.

SwitchMission(1.3.2).lua (8.0 KB)

1 Like

I tested, the script works. The only thing I will need to change the curves for the RC channel.

Hi @Quaxwilly. I’ve changed your script a bit. Reduced the length of the file path (as in the previous version) and the name of the missions. I did this so that the mission number could be seen (photo attached). Previously it was written that you can switch mission numbers, that is, you can not limit yourself to the number of switch positions Can this be done in your script, it would be great for those who have similar transmitters with Yaapu Widget installed?
Thanks again so much for your script!

SM.lua (8.0 KB)