Smarter flight waypoints

I thought about the task of more reasonable performance of the flight task.
The figure below shows the flight plan.
If, for some reason, the RTL command (let’s call this point X) triggered when the drone moved from point A to point B, then the flight will continue from point A, but I need the flight to continue from point X.
How do I imagine this task:
When performing a mission when the RTL command is triggered, the current coordinates must be overwritten into the memory of the position of point A.
It looks quite simple =), yet I know how to do it. Does anyone have any thoughts on this? Is it possible to perform such operations with memory during the flight?

1 Like

Look at how Rally Points work

1 Like

thank you for that more food for thought and more fun to be had with missions

Hello to all.
I made some progress in my search, but I bumped into a wall. I want to share the knowledge gained and look for help.

I found out that in the general cycle with a frequency of 100Hz, the update_flight_mode () function is started. This function is performed in the file flight_mode.cpp;

It checks which current mode is running. I need RTL mode.
If the RTL mode is executed, the rtl_run () function is started;

case RTL:
rtl_run ();

Next, in the control_rtl.cpp file, the rtl_run () function is executed
In it, I inserted the following code, overwriting the position of the last point.

// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
void Copter::rtl_run()
        uint16_t  my_nav_index ;
        AP_Mission::Mission_Command my_cmd;
        static int my_counter = 0 ;

        my_nav_index = mission.get_current_nav_index() ;
        if (! mission.read_cmd_from_storage (my_nav_index, my_cmd) && my_counter==0){
            gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "FAILED TO READ FROM STORAGE.");
        else if ( my_counter == 0 ) {
            gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "SUCCES TO READ FROM STORAGE.");
        my_cmd.content.location = gps.location();
        if (!mission.replace_cmd(my_nav_index, my_cmd) ) {
            gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "FAILED TO REPLACE COMMAND.");
        } else if ( my_counter == 0 ){
        my_counter = 1;


I see the code being executed,
but the next time you start the copter flies along the old route and I cannot understand why.
I attach a record of the execution of the algorithm on the simulator.
I will be very grateful if someone will share their advice with me.
Ardupilot Version 3.5.4

I also want to ask. Is it possible to perform the functions of working with memory when the copter is in the air? How to more correctly implement the intended algorithm?

1 Like

Hello to all. I have progress. I cite the code that is used to overwrite the previous waypoint in the place where the RTL command worked.
It now remains to refine the code and find the right place in the project where to insert it.
// rtl_run - runs the return-to-launch controller
// should be called at 100hz or more
void Copter::rtl_run()

        static int my_counter = 0 ;
        if (my_counter == 0){
            uint16_t  my_nav_index ;
            AP_Mission::Mission_Command my_cmd;
            my_cmd = mission.get_current_nav_cmd() ;
             my_nav_index = mission.get_current_nav_index() ;
             my_cmd.content.location = current_loc ;
             mission.replace_cmd( (my_nav_index -1 ) , my_cmd) ;
             mission.set_current_cmd((my_nav_index -1));           
             my_counter = 1;

You also need to visualize this new point in QGC.
Now the autopilot behaves as follows:

1 Like

I misunderstood when I first read your post. You’re resuming interrupted missions (for battery changes etc). Nice!

1 Like

I would rather implement it in the Mission Planner side, look for mode change to RTL and save the point in the GCS, then update the mission once the copter landed and disarmed. Needs less testing, no fiddle with FC code, can be implemented as a Mission Planner plugin, and can use any other input to trigger the point save (such a flow meter from a sprayer drone).

Once the onboard scripting is done, this will be more easier to do it onboard.

Good day,
Whether prompt there are commands of updating of mission which are sent from the UAV to QGC?
I made a function for resuming interrupted missions from the last point. The position of the UAV is recorded in memory and the drone returns to the take-off point.
Then QGC offers to resume the mission and overwrites the coordinates from its memory.
This removes my coordinate.

  1. Any ideas how to overcome the task of c resuming interrupted missions?
  2. How safe is it to re-record a mission point during a flight? SITL gives the error “crash …” when landing, it worries me. The rewrite function is called in three_hz_loop ()

At first, I thought that this option should be implemented on the QGC side. But personally, I have an unstable telemetry connection. And the QGC base station can just skip the moment of switching modes. The best option in my opinion is that the UAV remembers the moment of switching the mode from AUTO to RTL / LAND and writes the current point to the address of the mission memory.
Further, the QGC, offering to continue the mission, must read all the coordinates from the UAV memory.

Can you tell me how to implement a mission update on the QGC side?

Dunno, I’m a Mission Planner guy.
I’m using a Mission Planner plugin for spray planning and controlling the return point when pesticide tank empty. You can check the code here : It’s a quick hack and needs polishing, but works. (Although it needs a stable telemetry link, like the rfd868x)

Hi @Eosbandi, what flow sensor are you using and how do you get a reading from it onto the FC?

Also, is there any documentation on how to write and use a Mission Planner plugin?

Hi ! A simple impulse sensor, from banggood and an arduino nano to convert it to analog signal to connect to the RSSI analog input.

Documentation is the code itself, there is no manual, sorry.

1 Like