Until now, You’ve made a custom flight mode and a custom mavlink message. There may come a time when you have to trigger this flight mode in an auto mission. For this, you have to trigger the flight mode using the mavlink command. The following steps as given below will help you trigger flight mode by mavlink command while being in a mission.
Triggering flight mode by mavlink command
Make 4 functions in ModeAuto class in mode.h . They are
- do_<command_name>( )
- <command_name>_start( )
- <command_name>_run( )
- verify_<command_name( )
And while adding these functions on mode.h make sure to add these functions where they belong in the class definition by searching where similar functions belong. According to my mavlink command MAV_CMD_NAV_PAYLOAD_RELEASE, the functions became
- do_payload_release( )
- payload_release_start( )
- payload_release_run( )
- verify_payload_release( )
Now since you added these functions in ModeAuto class, open mode_auto.cpp to add functions definition.
- do_payload_release( ) gets called once when mavlink command arrives. For example, if waypoint 3 has a mavlink command MAV_CMD_NAV_PAYLOAD_RELEASE then, this function gets called once after entering the function start_command( ) of mode_auto.cpp file. This function has a case statement which checks which mavlink command has arrived and calls function do_payload_release( ).
The definition of do_payload_release( ) is shown below. You can add additional lines too according to your needs
2. payload_release_start( ) gets called inside the do_payload_release( ). We can directly call payload_release_start( ) inside start_command( ) function above, but it is a convention to do it this way. payload_release_start( ) function gets called once and it calls the init( ) function of the mode_payloadrelease.cpp file or ModePayloadRelease class.
At this step you have to add Auto_<command_name> in AutoMode enum in the file defines.h
So now, you can assign _mode = Auto_ just like I’ve done in the starting of the payload_release_start( ) function. This is important in the run( ) function of ModeAuto class since case statement checks _mode.
Now you can see copter.mode_payloadrelease.init(false) at the bottom of payload_release_start( ) function. This calls the init( ) function of ModePayloadRelease class or mode_payloadrelease.cpp file. All the lines in between serve my purpose of flight mode so you don’t have to understand that. You can remove those lines and add your lines of code.
3. payload_release_run( ) function gets called from the run( ) function of ModeAuto class. This run( ) function gets called every 100Hz after entering mission mode or auto mode. From the run( ) function, when _mode is equal to Auto_PayloadRelease which we set in payload_release_start( ) function then payload_release_run( ) function gets called. Below is run( ) function of the ModeAuto class with _mode switch statement.
The definition of payload_release_run( ) function is shown below
This payload_release_run( ) function calls run( ) function of ModePayloadRelease class or mode_payloadrelease.cpp file.
4. verify_payload_release( ) function gets called inside verify_command( ) function of ModeAuto class or mode_auto.cpp file.
verify_payload_release( ) function gets called every time to verify that payload release task has completed or not. This function returns true if the task is completed and the drone begins to run the next mavlink command.
So what I’ve added is, if a waypoint is reached to where I’ve set the destination (inside run( ) function of ModePayloadRelease class) , then payload is released. You can set your own verify checking statements inside this function.
So now, all the steps needed to trigger flight mode from mavlink command while being in mission is completed. You just need to fill the 4 functions mentioned above and run( ) and init( ) functions of mode_(your_flight_mode).cpp file for Mode(your_flight_mode) class inside mode.h file.
Running the code.
To run the code, you should send MAV_CMD_NAV_PAYLOAD_RELEASE command in mission from ground control station (gcs). So most of the time you cannot make your custom command in gcs. This will need separate programming in the gcs software side. So for the time being we can use MAV_CMD_NAV_RETURN_TO_LAUNCH or any other command available in gcs mission mode to trigger our command. You just need to interchange the command value of your mavlink command and one other command which is available in gcs.
I’ve exchanged the mavlink command value of MAV_CMD_NAV_RETURN_TO_LAUNCH and MAV_CMD_NAV_PAYLOAD_RELEASE. So whenever i set any waypoint command to MAV_CMD_NAV_RETURN_TO_LAUNCH in the mission, then in the ardupilot codebase, MAV_CMD_NAV_PAYLOAD_RELEASE will be called.
Original value of MAV_CMD_NAV_RETURN_TO_LAUNCH = 20
Original value of MAV_CMD_NAV_PAYLAOD_RELEASE = 31015
Changed value of MAV_CMD_NAV_RETURN_TO_LAUNCH = 31015
Changed value of MAV_CMD_NAV_PAYLOAD_RELEASE = 20
You will have to interchange the value in ardupilotmega.h file of ardupilot codebase.
For those who can add custom mavlink commands in gcs, interchanging of values isn’t needed. Just make sure that your ‘NAV’ cmd has value less than *100.
Above you can see that MAV_CMD_NAV_RETURN_TO_LAUNCH is set to 31015 and below you can see MAV_CMD_NAV_PAYLOAD_RELEASE is set to 20.
So now, let’s set a mission in GCS.
You can see that on waypoint 4, RTL is called. RTL has a mavlink command value of 20 in gcs so it calls payload release in Ardupilot codebase.
My flight mode calculates a position from where payload should be dropped for payload to reach coordinates given in param5,param6,param7 of mission. The copter navigates to the calculated coordinate, drops the payload ending flight mode task, and continues its automatic mission to waypoint 5 and lands.
You can see that the copter went to the position calculated to drop payload from waypoint 4. And after the payload was dropped, it returned to waypoint 5 and landed.
- Add a mavlink command in the gcs rather than exchanging with MAV_CMD_NAV_RETURN_TO_LAUNCH.