Arduino Mavlink Library – Changing flight modes
Problem: Cannot change flight controller to auto on startup without GPS or EKF lock
The Pixhawk 1 flight controller running the latest 4.0 release of Ardurover is unable to enter auto mode despite setting the parameter ‘initial_mode’ to 10. The problem seems to stem from the autopilot trying to switch to the specified initial_mode setting before a GPS fix has stabilized on boot. For initial driving modes which require a valid and stable GPS lock, a “flight mode change failed” warning will display in the status window and the driving mode will be set to manual instead of auto.
This problem is further compounded when despite having a 3DGPS lock (GPS fix of 3) and a satellite count as high as 10, if the Extended Kalman Filter has not stabilized, the setting of auto mode will still be prohibited.
Dead-end workarounds:
- The Arduino Mavlink 1 library does not include any EKF specific status modules (mavlink_ekf_status_report_t) so monitoring EKF status using mavlink commands did not seem to be an option.
- Monitoring # of satellites GPS has acquired – Sat count as high as 10 have been observed with EKF still not ready.
- GPS Fix of 3 or 4 – It seems EKF stabilization corresponds to a GPS fix of 4, however I cannot be certain that EKF cannot be normalized with a GPS fix of 3.
Workaround: Send flight/driving mode change command till successful, then stop messaging (used in conjunction with a heartbeat monitor function)
For the time being I skipped any Ardupilot/Pixhawk configuration and instead focused changing the driving/flight mode using the Arduino companion computer with Mavlink commands.
Below is my implementation using the Arduino Mavlink protocol:
I call the function below while checking whether or not I also have a GPS lock that is greater than or equal to 3, and if the mode has changed successfully, start the motor. It is used in conjunction with a heartbeat monitor function which determines if the mode has successfully changed to 10 (auto), otherwise keep running the function below every few seconds. This ensures the auto mode is set before commencing its journey.
The function below will send a mode change Mavlink message to the Pixhawk:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 |
void setmode_Auto() { //Set message variables uint8_t _system_id = 255; // id of computer which is sending the command (ground control software has id of 255) uint8_t _component_id = 2; // seems like it can be any # except the number of what Pixhawk sys_id is uint8_t _target_system = 1; // Id # of Pixhawk (should be 1) uint8_t _base_mode = 1; uint32_t _custom_mode = 10; //10 = auto mode /* Flight / Driving Modes (change custom mode above) 0 - Manual 1 - Acro 3 - Steering 4 - Hold 5 - Loiter 6 - Follow 7 - Simple 10 Auto 11 RTL 12 SmartRTL 15 Guided */ // Initialize the required buffers mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Pack the message mavlink_msg_set_mode_pack(_system_id, _component_id, &msg, _target_system, _base_mode, _custom_mode); uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // Send the message (.write sends as bytes) Serial.print("\nsending set mode command..."); Serial1.write(buf, len); //Write data to serial port } |
Hello,
Thanks for the code. In the current setup for the mission planner, to change it to auto mode the number is 3. Rest all is perfect. You are a savior man. Have a great life wherever you are.