Copter: Impliment mavlink landing gear control

Uses MAV_CMD_AIRFRAME_CONFIGURATION to retract and deploy landing gear
This commit is contained in:
Matt 2018-08-31 09:48:00 -04:00 committed by Randy Mackay
parent 837903a2bf
commit 3c76ec492b

View File

@ -1080,6 +1080,23 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
break;
#endif
case MAV_CMD_AIRFRAME_CONFIGURATION: {
// Param 1: Select which gear, not used in ArduPilot
// Param 2: 0 = Deploy, 1 = Retract
// For safety, anything other than 1 will deploy
switch ((uint8_t)packet.param2) {
case 1:
copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract);
result = MAV_RESULT_ACCEPTED;
break;
default:
copter.landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
result = MAV_RESULT_ACCEPTED;
break;
}
break;
}
/* Solo user presses Fly button */
case MAV_CMD_SOLO_BTN_FLY_CLICK: {
result = MAV_RESULT_ACCEPTED;