diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a54e425d9d..f93bdc3817 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -618,108 +618,118 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const } else if (cmd_mavlink.command == MAV_CMD_DO_AUTOTUNE_ENABLE) { - bool has_module = true; - autotune_attitude_control_status_s status{}; - _autotune_attitude_control_status_sub.copy(&status); + // Check command validity + if ((int)roundf(vehicle_command.param1) != 1 || (int)roundf(vehicle_command.param2) != 0) { + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + send_ack = true; - // if not busy enable via the parameter - // do not check the return value of the uORB copy above because the module - // starts publishing only when MC_AT_START is set - if (status.state == autotune_attitude_control_status_s::STATE_IDLE) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); + } else { - if (!vehicle_status.in_transition_mode) { - param_t atune_start; + bool has_module = true; + autotune_attitude_control_status_s status{}; + _autotune_attitude_control_status_sub.copy(&status); - switch (vehicle_status.vehicle_type) { - case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: - atune_start = param_find("FW_AT_START"); + // if not busy enable via the parameter + // do not check the return value of the uORB copy above because the module + // starts publishing only when MC_AT_START is set + if (status.state == autotune_attitude_control_status_s::STATE_IDLE) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); - break; + if (!vehicle_status.in_transition_mode) { + param_t atune_start; - case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: - atune_start = param_find("MC_AT_START"); + switch (vehicle_status.vehicle_type) { + case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: + atune_start = param_find("FW_AT_START"); - break; + break; - default: - atune_start = PARAM_INVALID; - break; - } + case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: + atune_start = param_find("MC_AT_START"); - if (atune_start == PARAM_INVALID) { - has_module = false; + break; + + default: + atune_start = PARAM_INVALID; + break; + } + + if (atune_start == PARAM_INVALID) { + has_module = false; + + } else { + int32_t start = 1; + param_set(atune_start, &start); + } } else { - int32_t start = 1; - param_set(atune_start, &start); + has_module = false; + } + } + + if (has_module) { + + // most are in progress + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS; + + switch (status.state) { + case autotune_attitude_control_status_s::STATE_IDLE: + case autotune_attitude_control_status_s::STATE_INIT: + progress = 0; + break; + + case autotune_attitude_control_status_s::STATE_ROLL: + case autotune_attitude_control_status_s::STATE_ROLL_PAUSE: + progress = 20; + break; + + case autotune_attitude_control_status_s::STATE_PITCH: + case autotune_attitude_control_status_s::STATE_PITCH_PAUSE: + progress = 40; + break; + + case autotune_attitude_control_status_s::STATE_YAW: + case autotune_attitude_control_status_s::STATE_YAW_PAUSE: + progress = 60; + break; + + case autotune_attitude_control_status_s::STATE_VERIFICATION: + progress = 80; + break; + + case autotune_attitude_control_status_s::STATE_APPLY: + progress = 85; + break; + + case autotune_attitude_control_status_s::STATE_TEST: + progress = 90; + break; + + case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM: + progress = 95; + break; + + case autotune_attitude_control_status_s::STATE_COMPLETE: + progress = 100; + // ack it properly with an ACCEPTED once we're done + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + break; + + case autotune_attitude_control_status_s::STATE_FAIL: + progress = 0; + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; + break; } } else { - has_module = false; - } - } - - if (has_module) { - - // most are in progress - result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS; - - switch (status.state) { - case autotune_attitude_control_status_s::STATE_IDLE: - case autotune_attitude_control_status_s::STATE_INIT: - progress = 0; - break; - - case autotune_attitude_control_status_s::STATE_ROLL: - case autotune_attitude_control_status_s::STATE_ROLL_PAUSE: - progress = 20; - break; - - case autotune_attitude_control_status_s::STATE_PITCH: - case autotune_attitude_control_status_s::STATE_PITCH_PAUSE: - progress = 40; - break; - - case autotune_attitude_control_status_s::STATE_YAW: - case autotune_attitude_control_status_s::STATE_YAW_PAUSE: - progress = 60; - break; - - case autotune_attitude_control_status_s::STATE_VERIFICATION: - progress = 80; - break; - - case autotune_attitude_control_status_s::STATE_APPLY: - progress = 85; - break; - - case autotune_attitude_control_status_s::STATE_TEST: - progress = 90; - break; - - case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM: - progress = 95; - break; - - case autotune_attitude_control_status_s::STATE_COMPLETE: - progress = 100; - // ack it properly with an ACCEPTED once we're done - result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - break; - - case autotune_attitude_control_status_s::STATE_FAIL: - progress = 0; - result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; - break; + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; } - } else { - result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + send_ack = true; + } - send_ack = true; } else { send_ack = false;