commander: make sure all code paths return an (n)ack to MAV_CMD_PREFLIGHT_CALIBRATION

This commit is contained in:
Beat Küng
2017-01-11 14:39:21 +01:00
parent 8cd913c148
commit 48c5ec54bb
+3 -2
View File
@@ -4024,14 +4024,15 @@ void *commander_low_prio_loop(void *arg)
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status_flags.rc_input_blocked) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
/* enable RC control input */
status_flags.rc_input_blocked = false;
mavlink_log_info(&mavlink_log_pub, "CAL: Re-enabling RC IN");
calib_ret = OK;
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
/* this always succeeds */
calib_ret = OK;
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub, command_ack);
}
status_flags.condition_calibration_enabled = false;