Copter: add support for DO_PARACHUTE from GCS

Mission command support was already included but this adds support for
DO_PARACHUTE received as a COMMAND_LONG message which are sent from the
GCS when the command should be executed immediately
This commit is contained in:
Randy Mackay 2014-09-17 21:00:27 +09:00
parent adf00a207b
commit 9f3803052f

View File

@ -1218,6 +1218,29 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#endif
break;
#if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE:
// configure or release parachute
result = MAV_RESULT_ACCEPTED;
switch ((uint16_t)packet.param1) {
case PARACHUTE_DISABLE:
parachute.enabled(false);
Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case PARACHUTE_ENABLE:
parachute.enabled(true);
Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case PARACHUTE_RELEASE:
// treat as a manual release which performs some additional check of altitude
parachute_manual_release();
break;
default:
result = MAV_RESULT_FAILED;
break;
}
#endif
case MAV_CMD_DO_MOTOR_TEST:
// param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
// param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)