Mission handler

[mdouglas90]

Fixes Issue #306.

This is still a work in progress but there have been several major changes to the Mavlink_waypoint_handler class as well as the creation of several other classes.

Changes that have been made already:

• The waypoint class
  • A Waypoint class has been created and both other waypoint structures have been removed.
  • The waypoint stores most of its parameters as member fields. The current field is no longer a part of the waypoint. It must be passed to the waypoint when sending the message to the ground control station. The reason for this is that the waypoint should not need to know if it is the current waypoint, just the physical properties as well as some options set by the user. In addition, the x, y, and z fields are no longer stored by themselves as they are stored as a global position.
  • The waypoint can send itself as a mavlink message by calling the Waypoint::send() function and passing in the appropriate arguments.
  • The waypoint class can set and retrieve the local position of the waypoint. This will convert its global position to the local position.
• The Mavlink_waypoint_handler has been greatly reduced.
  • This class is now only responsible for the creation, deletion, reception, transmission, and retrieval of the waypoints.
  • Various redundant variables have been removed.
  • The waypoints can be retrieved via the public functions Mavlink_waypoint_handler::current_waypoint(), Mavlink_waypoint_handler::next_waypoint(), and Mavlink_waypoint_handler::waypoint_from_index(int i).
  • The waypoint index can be advanced by calling Mavlink_waypoint_handler::advance_to_next_waypoint().
  • The sending and receiving waypoint function logic has been largely unchanged.
• The mission planner class
  • A Mission_planner class has been created that is responsible for planning the mission. It determines which handler to call based off of the state of the drone. It also contains the callback functions from the old Mavlink_waypoint_handler that were related to the mission planning.
  • The critical handler has been largely unchanged
  • The state machine now calls various Mission_planner_handler classes based off the drone state.
  • The mission planner no longer contains waypoint_next_, next_waypoint, current_waypoint_, etc. Instead, these waypoints should be obtained by requesting them from the Mavlink_waypoint_handler.
• A mission planner handler class has been created.
  • An abstract Mission_planner_handler class has been created that is called by the mission planner state machine function. The child classes should determine what to do with the waypoint goal based on the current state of the drone.
  • The current child classes are Mission_planner_handler_hold_position, Mission_planner_handler_landing, Mission_planner_handler_manual_control, Mission_planner_handler_navigating, Mission_planner_handler_on_ground, Mission_planner_handler_stop_on_position, Mission_planner_handler_stop_there, and Mission_planner_handler_takeoff.
  • If a custom mission planner handler needs to be made, the handle and init functions need to be defined.
  • This class contains private static members hold_waypoint_and hold_waypoint_set_ that should be set whenever the hold position needs to be changed. As they are private, these subclasses will therefore not have access to it but can manipulate them with the functions reset_hold_waypoint(), hold_waypoint_set(), set_hold_waypoint(local_position_t hold_position), set_hold_waypoint(local_position_t hold_position, float heading), set_hold_waypoint(Waypoint wpt), and hold_waypoint(). These functions will automatically control both private variables. If something tries to access the hold_waypoint before it has been set (or after it has been reset), it will return the local position of the drone and set the hold waypoint.
• The navigation class has been modified
  • The goal waypoint has been made private, it is now accessed by the functions goal() and set_goal(Waypoint wpt).
  • The dubin control has been moved to the navigation class.
  • The dubin structure does not need to be reinitialized manually, the navigation class will reinitialize the goal if there is a slight change in the goal (slight as the parameters are mainly floats).
  • The internal state has been made private. It can be accessed by the functions internal_state() and set_internal_state(internal_state_t new_internal_state). The set internal state will automatically output a debug message of a state change.
  • A waiting_at_waypoint_ boolean has been created. This is used for the various methods that need to know if the drone is en route or if the drone has arrived at the waypoint.

Things that still need to be changed:

• The critical state handler has been largely unchanged. This should probably be changed to a method similar to that of the regular mission planner state machine.
• The navigating handler is a little complex and should probably be changed
• There seems to be issues with the navigation path towards the goals. See issues Overshoot during navigation #275 and weird navigation path #297. I do not believe that it is caused by an incorrect goal location.
• I think something should be done with the nav_plan_active boolean.
• Fix the landing for the fixed wing/dubin landing procedure.
• General code cleanup.

Some recommendations/comments would be very helpful.

[GregoireH]

This is useless since waypoint.radius() is already setting the radius to zero in case no radius correspond to that type of waypoint

[GregoireH]

I am still not sure I understood why we have this check on autocontinue to determine if mission is finished or not !?

[jlecoeur]

Sounds like copy paste error. I would even think the mission is finished if we landed and the waypoint is not autocontinue

[mdouglas90]

When you land through the landing handler, the drone will disarm automatically, which means that the next iteration, the drone will switch to standby.

Autocontinue is used to determine if the drone should start to handle the next waypoint. Because when we land through the button, I assumed that we want the mission to be over so I made the button not be autocontinue. That means that the handler will not change to the interrupted waypoint in the list and will just switch to standby later. The handlers don't have to finish, they can be interrupted.

If autocontinue is set to on, then the drone will switch to standby in two methods based on how the landing waypoint was created. If the landing was a part of the mission, it will just switch to standby. If the mission was interrupted through a button or something, it will setup the interrupted waypoint again and then overwrite it with standby next iteration (this due to the drone resuming the mission after an inserted waypoint has been completed).

I left the autocontinue in there as I could see the mission plan requiring more than one landing and takeoff (e.g. delivery of goods). In that case, you would have the drone land with autocontinue on into some new mission item such as release goods and then takeoff or something. Of course, the mission planner will need to be modified a bit to allow mission items to be handled when the drone has disarmed and to not force the drone into standby after landing.

If you'd prefer, the entire if (autocontinue && is_landed_) { return MISSION_FINISHED; } part can be removed and then it would just go to standby next iteration with no check of autocontinue.

[GregoireH]

something weird here, isn't it ?

[GregoireH]

this is weird as well !
0.9f should be a parameter at least,
But what happen if you are upper than this limit for some reason ?
Don't you want to use maths_fabs(ins_.position_lf()[Z] - wpt_pos[Z]) <= takeoff_acceptance_range_ ?

[GregoireH]

idem here , why autocontinue ?

[jlecoeur]

It is needed because when the waypoint is not autocontinue, we want to hover on the waypoint after take-off. By returning MISSION_IN_PROGRESS, the handler will be called next time to keep hovering on waypoint.

However I still believe the autocontinue field should be tested outside handlers... That would make them much simpler, and less error-prone.

[jlecoeur]

We could check the autocontinue field in Mission_planner.

update current handler
if current handler is finished
|    if waypoint is autocontinue
|    |    fetch next waypoint and next handler
|    else
|    |    insert hover waypoint and fetch handler
|    endif
endif

[mdouglas90]

If autocontinue is off, you don't want the drone to advance to the next mission item.

[jlecoeur]

yes my bad, I fixed my comment
(comment still valid though)

[mdouglas90]

If we check the autocontinue in Mission_planner, it would reduce some flexibility. For example, if we want to ignore the autocontinue flag for some reason (like a timeout if no commands have been received within a set time), the handler could advance anyways.

Though as you said, autocontinue checks within the mission planner would make it less error prone.

[GregoireH]

don't you want to add a default case ?

[GregoireH]

this 4.0f should be a parameter

[GregoireH]

idem autocontinue
& why do you check if there is more than 1 waypoint to know if mission is finished ?

[mdouglas90]

The check for the waypoint count is to not uselessly advance to the same waypoint that you're at. If there's only 1 waypoint and we just arrived at it, should we be advancing to that same waypoint.

[jlecoeur]

sounds like a very common thing to check, IMHO it would make more sense in the planner

[jlecoeur]

else we just crash ?

[mdouglas90]

Is guided is set only during hold position and auto. If it's not guided we are in attitude or velocity which has the pilot controlling the drone.

[jlecoeur]

I am not really comfortable with this middle ground approach.
The two tasks mission_planner and main_task are both writing into the controller, but only in certain modes.

• It is very difficult to be sure at one point who wrote into the controller
• we have to make sure that these modes do not overlap, in which case the behaviour is undefined and depend on which task is run first.
• we also have to make sure there is no gap between these sets of modes, in which case the drone would simply do nothing and crash.

It would be much safer and easy to read if we had a single point where commands are written to the controller.

I also fear -- that because of all the edge cases in on ground, paused, critical, and manual states -- mission_planner is growing more than expected, and that we do not gain much in readability compared to our beloved waypoint_handler_state_navigation_t.

Solution A: Mission planner is responsible

The two tasks would be sensing_plus_cascade_controller_update() and mission_planner.update(). (best to keep the two separate to allow different update freqs)
Mission_planner would check which mode we are in, then call the appropriate handler.
A new Manual_handler would contain the code we have now in main task.

Solution B: Main task is responsible

Still two tasks: Mission_planner::update() and LEQuad::main_task()
Mission_planner would be yet another source of command. It does not directly write into the controller. But it has a method to get its desired command. In auto mode, LEQuad::main_task() reads the desired command from Mission_planner, and writes it into the controller. In other modes, LEQuad::main_task() reads from remote and writes into the controller.

Solution C: Big brother

This is kind of a mix of A and B.
The scope of Mission_planner is reduced to autonomous mode only, and mission only. No critical handler, no paused state, no position_hold mode. Handlers do not write into a controller by themselves, but they have a method to return the desired command.
A new Critical_handler class is created, it is responsible for everything related to critical state. It can contains a small list of waypoints that it fills based on the desired behaviour. It can use the same Mission_handlers as Mission_planner. Same as Mission_planner, it does not directly know the controller, but can give its desired command.
A new Critical_detector class is created. Its only purpose is to check various aspects of the drone (battery, fence, connexion to GCS...), and indicate which critical state we are in, or if everything is fine.
A Manual_handler is created as for solution A
The Big_brother module contains all of them. It can be configured by filling it with a list of mission_handlers, by specifying the type of critical handler to use, the type of critical detector, the type of manual handler, and the type of controller to use.
In its update loop, it will check the current mav mode. And check the critical state using its Critical_detector member. It will call the update of either Manual_handler, Mission_planner or Critical_handler. Ask them the desired command and give it to the controller (single entry point of the controller).
The two tasks would be big_brother_update (relatively slow) and sensing_plus_cascade_controller_update (high speed)

I tend towards C. But if you ask short term solution, I think solution B would be the easier to debug, and later easier to bend towards solution C. What is your opinion @GregoireH @basil-huber ?

[basil-huber]

I also prefer solution C

[mdouglas90]

Speaking of this, I took a look at the code again and we are guided if in velocity control as well. I did add a check in the critical handler to make sure we are in auto or hold position, but if that was not there, there would be two modules competing for the control command. This is a good argument for your Solution C as well, putting the decision for who has control within only one module.

[jlecoeur]

• yaw not alined
• super aggressive maneuvers during sharp turns, maybe limit angle commands

Other than that,

• I'll try to clean controllers before merge -> nop
• critical behavior kept as is -> until ?

[jlecoeur]

Will merge as is, as soon as travis is happy with it.