APM: ensure DO_SET_SERVO channels are enabled

This commit is contained in:
Andrew Tridgell 2012-09-09 09:27:29 +10:00
parent 1e51988ecc
commit cf9dd9281c
2 changed files with 2 additions and 0 deletions

View File

@ -1106,6 +1106,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
APM_RC.enable_out(packet.param1 - 1);
APM_RC.OutputCh(packet.param1 - 1, packet.param2);
result = MAV_RESULT_ACCEPTED;
break;

View File

@ -576,6 +576,7 @@ static void do_set_home()
static void do_set_servo()
{
APM_RC.enable_out(next_nonnav_command.p1 - 1);
APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
}