diff --git a/docs/en/flight_modes_fw/mission.md b/docs/en/flight_modes_fw/mission.md index 10f147d326..91f48a8300 100644 --- a/docs/en/flight_modes_fw/mission.md +++ b/docs/en/flight_modes_fw/mission.md @@ -162,6 +162,10 @@ Mission Items: - [MAV_CMD_OBLIQUE_SURVEY](https://mavlink.io/en/messages/common.html#MAV_CMD_OBLIQUE_SURVEY) - [MAV_CMD_DO_SET_CAMERA_ZOOM](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_CAMERA_ZOOM) - [MAV_CMD_DO_SET_CAMERA_FOCUS](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_CAMERA_FOCUS) +- [MAV_CMD_DO_AUTOTUNE_ENABLE](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_AUTOTUNE_ENABLE) + - Disabling autotune by setting `param1` to zero is currently not supported. To abort autotune during a mission, switch to another flight mode. + - Axis selection specified in the MAVLink message is ignored (`param2` must be set to 0). + Instead, the axis bitmask defined by [`FW_AT_AXES`](../advanced_config/parameter_reference.md#FW_AT_AXES) is used. GeoFence Definitions diff --git a/docs/en/flight_modes_mc/mission.md b/docs/en/flight_modes_mc/mission.md index 0b5b9bced4..61e2358929 100644 --- a/docs/en/flight_modes_mc/mission.md +++ b/docs/en/flight_modes_mc/mission.md @@ -166,6 +166,9 @@ Mission Items: - [MAV_CMD_NAV_VTOL_TAKEOFF](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_VTOL_TAKEOFF) - `MAV_CMD_NAV_VTOL_TAKEOFF.param2` (transition heading) is ignored. Instead the heading to the next waypoint is used for the transition heading. +- [MAV_CMD_DO_AUTOTUNE_ENABLE](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_AUTOTUNE_ENABLE) + - Disabling autotune by setting `param1` to zero is currently not supported. To abort autotune during a mission, switch to another flight mode. + - Axis selection specified in the MAVLink message is ignored (`param2` must be set to 0) . GeoFence Definitions diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index b5d642b140..b5aca3b13c 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -61,6 +61,7 @@ uint16 VEHICLE_CMD_DO_PARACHUTE=208 # Mission command to trigger a parachute. |a uint16 VEHICLE_CMD_DO_MOTOR_TEST=209 # Motor test command. |Instance (@range 1, )|throttle type|throttle|timeout [s]|Motor count|Test order|Unused| uint16 VEHICLE_CMD_DO_INVERTED_FLIGHT=210 # Change to/from inverted flight. |inverted (0=normal, 1=inverted)|Unused|Unused|Unused|Unused|Unused|Unused| uint16 VEHICLE_CMD_DO_GRIPPER = 211 # Command to operate a gripper. +uint16 VEHICLE_CMD_DO_AUTOTUNE_ENABLE = 212 # Enable autotune module. |1 to enable|Unused|Unused|Unused|Unused|Unused|Unused| uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214 # Mission command to set TRIG_INTERVAL for this flight. |[m] Camera trigger distance|Shutter integration time (ms)|Unused|Unused|Unused|Unused|Unused| uint16 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220 # Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)|q2 - quaternion param #2, x (0 in null-rotation)|q3 - quaternion param #3, y (0 in null-rotation)|q4 - quaternion param #4, z (0 in null-rotation)|Unused|Unused|Unused| uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221 # Set id of master controller. |System ID|Component ID|Unused|Unused|Unused|Unused|Unused| diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 3923eb379f..ebbbfa98e5 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1550,6 +1550,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE: case vehicle_command_s::VEHICLE_CMD_REQUEST_CAMERA_INFORMATION: case vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE: + case vehicle_command_s::VEHICLE_CMD_DO_AUTOTUNE_ENABLE: /* ignore commands that are handled by other parts of the system */ break; diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index cc34f92bf5..e45741d2cf 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -48,7 +48,6 @@ FwAutotuneAttitudeControl::FwAutotuneAttitudeControl(bool is_vtol) : _actuator_controls_status_sub(is_vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)) { _autotune_attitude_control_status_pub.advertise(); - reset(); } FwAutotuneAttitudeControl::~FwAutotuneAttitudeControl() @@ -73,11 +72,6 @@ bool FwAutotuneAttitudeControl::init() return true; } -void FwAutotuneAttitudeControl::reset() -{ - _param_fw_at_start.reset(); -} - void FwAutotuneAttitudeControl::Run() { if (should_exit()) { @@ -107,10 +101,23 @@ void FwAutotuneAttitudeControl::Run() } } + if (_vehicle_command_sub.updated()) { + vehicle_command_s vehicle_command; + + if (_vehicle_command_sub.copy(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_AUTOTUNE_ENABLE) { + if (fabsf(vehicle_command.param1 - 1.0f) < FLT_EPSILON && fabsf(vehicle_command.param2) < FLT_EPSILON) { + _vehicle_cmd_start_autotune = true; + } + } + } + } + _aux_switch_en = isAuxEnableSwitchEnabled(); + _want_start_autotune = _vehicle_cmd_start_autotune || _aux_switch_en; // new control data needed every iteration - if ((_state == state::idle && !_aux_switch_en) + if ((_state == state::idle && !_want_start_autotune) || !_vehicle_torque_setpoint_sub.updated()) { return; @@ -310,7 +317,8 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) switch (_state) { case state::idle: - if (_param_fw_at_start.get() || _aux_switch_en) { + + if (_want_start_autotune) { mavlink_log_info(&_mavlink_log_pub, "Autotune started"); _state = state::init; @@ -540,8 +548,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) orb_advert_t mavlink_log_pub = nullptr; mavlink_log_info(&mavlink_log_pub, "Autotune returned to idle"); _state = state::idle; - _param_fw_at_start.set(false); - _param_fw_at_start.commit(); + _vehicle_cmd_start_autotune = false; } break; @@ -561,7 +568,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) // Abort mavlink_log_critical(&mavlink_log_pub, "Autotune aborted before finishing"); _state = state::fail; - + _start_flight_mode = _nav_state; _state_start_time = now; } else if (timeout) { @@ -589,10 +596,9 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) break; } + _start_flight_mode = _nav_state; _state_start_time = now; } - - } } diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp index 9b63f5ba2b..5a8a780c90 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -101,8 +102,6 @@ private: void Run() override; - void reset(); - void checkFilters(); void updateStateMachine(hrt_abstime now); @@ -126,6 +125,7 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::PublicationData _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)}; @@ -178,6 +178,8 @@ private: uint8_t _nav_state{0}; uint8_t _start_flight_mode{0}; bool _aux_switch_en{false}; + bool _vehicle_cmd_start_autotune{false}; + bool _want_start_autotune{false}; orb_advert_t _mavlink_log_pub{nullptr}; @@ -215,7 +217,6 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_fw_at_axes, - (ParamBool) _param_fw_at_start, (ParamInt) _param_fw_at_man_aux, (ParamInt) _param_fw_at_apply, diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c index 339d003818..4a60977f01 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c @@ -39,22 +39,7 @@ * @author Mathieu Bresciani */ -/** - * Start the autotuning sequence - * - * WARNING: this will inject steps to the rate controller - * and can be dangerous. Only activate if you know what you - * are doing, and in a safe environment. - * - * Any motion of the remote stick will abort the signal - * injection and reset this parameter - * Best is to perform the identification in position or - * hold mode. - * - * @boolean - * @group Autotune - */ -PARAM_DEFINE_INT32(FW_AT_START, 0); + /** * Controls when to apply the new gains diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 822b0144e7..4453b7ab00 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1538,6 +1538,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->params[0] = (uint16_t)mavlink_mission_item->param1; break; + case MAV_CMD_DO_AUTOTUNE_ENABLE: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->params[0] = (uint16_t)mavlink_mission_item->param1; + mission_item->params[1] = (uint16_t)mavlink_mission_item->param2; + break; + default: mission_item->nav_cmd = NAV_CMD_INVALID; PX4_DEBUG("Unsupported command %d", mavlink_mission_item->command); @@ -1627,6 +1633,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_CONDITION_DELAY: case MAV_CMD_CONDITION_DISTANCE: case MAV_CMD_DO_SET_ACTUATOR: + case MAV_CMD_DO_AUTOTUNE_ENABLE: case MAV_CMD_COMPONENT_ARM_DISARM: mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; break; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a54e425d9d..c913909657 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -622,46 +622,37 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const autotune_attitude_control_status_s status{}; _autotune_attitude_control_status_sub.copy(&status); - // 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 + // publish vehicle command once if: + // - autotune is not already running + // - we are not in transition + // - autotune module is enabled if (status.state == autotune_attitude_control_status_s::STATE_IDLE) { vehicle_status_s vehicle_status{}; _vehicle_status_sub.copy(&vehicle_status); if (!vehicle_status.in_transition_mode) { - param_t atune_start; switch (vehicle_status.vehicle_type) { case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: - atune_start = param_find("FW_AT_START"); - + has_module = param_find("FW_AT_APPLY"); break; case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING: - atune_start = param_find("MC_AT_START"); - + has_module = param_find("MC_AT_APPLY"); break; default: - atune_start = PARAM_INVALID; + has_module = false; break; } - if (atune_start == PARAM_INVALID) { - has_module = false; - - } else { - int32_t start = 1; - param_set(atune_start, &start); + if (has_module) { + _cmd_pub.publish(vehicle_command); } - - } else { - has_module = false; } } - if (has_module) { + if (has_module && fabsf(vehicle_command.param1 - 1.f) <= FLT_EPSILON && fabsf(vehicle_command.param2) < FLT_EPSILON) { // most are in progress result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS; @@ -715,6 +706,10 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const break; } + } else if (has_module && (fabsf(vehicle_command.param1 - 1.f) > FLT_EPSILON || fabsf(vehicle_command.param2) > FLT_EPSILON)) { + + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; + } else { result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; } diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp index 3d2c0dab7f..c638492067 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp @@ -46,7 +46,6 @@ McAutotuneAttitudeControl::McAutotuneAttitudeControl() : WorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { _autotune_attitude_control_status_pub.advertise(); - reset(); } McAutotuneAttitudeControl::~McAutotuneAttitudeControl() @@ -56,7 +55,8 @@ McAutotuneAttitudeControl::~McAutotuneAttitudeControl() bool McAutotuneAttitudeControl::init() { - if (!_parameter_update_sub.registerCallback()) { + + if (!_vehicle_torque_setpoint_sub.registerCallback()) { PX4_ERR("callback registration failed"); return false; } @@ -66,11 +66,6 @@ bool McAutotuneAttitudeControl::init() return true; } -void McAutotuneAttitudeControl::reset() -{ - _param_mc_at_start.reset(); -} - void McAutotuneAttitudeControl::Run() { if (should_exit()) { @@ -91,17 +86,12 @@ void McAutotuneAttitudeControl::Run() updateStateMachine(hrt_absolute_time()); } - // new control data needed every iteration - if (_state == state::idle - || !_vehicle_torque_setpoint_sub.updated()) { - return; - } - if (_vehicle_status_sub.updated()) { vehicle_status_s vehicle_status; if (_vehicle_status_sub.copy(&vehicle_status)) { _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + _nav_state = vehicle_status.nav_state; } } @@ -113,6 +103,24 @@ void McAutotuneAttitudeControl::Run() } } + if (_vehicle_command_sub.updated()) { + vehicle_command_s vehicle_command; + + if (_vehicle_command_sub.copy(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_AUTOTUNE_ENABLE) { + if (fabsf(vehicle_command.param1 - 1.0f) < FLT_EPSILON && fabsf(vehicle_command.param2) < FLT_EPSILON) { + _vehicle_cmd_start_autotune = true; + } + } + } + } + + // new control data needed every iteration + if ((_state == state::idle && !_vehicle_cmd_start_autotune) + || !_vehicle_torque_setpoint_sub.updated()) { + return; + } + vehicle_torque_setpoint_s vehicle_torque_setpoint; vehicle_angular_velocity_s angular_velocity; @@ -271,7 +279,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) switch (_state) { case state::idle: - if (_param_mc_at_start.get()) { + if (_vehicle_cmd_start_autotune) { if (registerActuatorControlsCallback()) { _state = state::init; @@ -280,6 +288,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) } _state_start_time = now; + _start_flight_mode = _nav_state; } break; @@ -438,17 +447,23 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) break; } - // In case of convergence timeout or pilot intervention, + // In case of convergence timeout, pilot intervention or mode change, // the identification sequence is aborted immediately manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); + const bool timeout = (now - _state_start_time) > 20_s; + const bool mode_changed = (_start_flight_mode != _nav_state); + const bool pilot_intervention = ((fabsf(manual_control_setpoint.roll) > 0.05f) + || (fabsf(manual_control_setpoint.pitch) > 0.05f)); + + const bool should_abort = timeout || mode_changed || pilot_intervention; + if (_state != state::wait_for_disarm - && _state != state::idle - && (((now - _state_start_time) > 20_s) - || (fabsf(manual_control_setpoint.roll) > 0.05f) - || (fabsf(manual_control_setpoint.pitch) > 0.05f))) { + && _state != state::idle && should_abort) { + _state = state::fail; + _start_flight_mode = _nav_state; _state_start_time = now; } } @@ -580,8 +595,7 @@ void McAutotuneAttitudeControl::saveGainsToParams() void McAutotuneAttitudeControl::stopAutotune() { - _param_mc_at_start.set(false); - _param_mc_at_start.commit(); + _vehicle_cmd_start_autotune = false; _vehicle_torque_setpoint_sub.unregisterCallback(); } diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp index 4307edfcd1..cc6ce692e5 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -86,8 +87,6 @@ public: private: void Run() override; - void reset(); - void checkFilters(); void updateStateMachine(hrt_abstime now); @@ -109,6 +108,7 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::PublicationData _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)}; @@ -137,6 +137,9 @@ private: int8_t _signal_sign{0}; bool _armed{false}; + uint8_t _nav_state{0}; + uint8_t _start_flight_mode{0}; + bool _vehicle_cmd_start_autotune{false}; matrix::Vector3f _kid{}; matrix::Vector3f _rate_k{}; @@ -177,7 +180,6 @@ private: perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; DEFINE_PARAMETERS( - (ParamBool) _param_mc_at_start, (ParamFloat) _param_mc_at_sysid_amp, (ParamInt) _param_mc_at_apply, (ParamFloat) _param_mc_at_rise_time, diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c index 1dc1e43c22..c2df3fd8f6 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c @@ -47,24 +47,6 @@ */ PARAM_DEFINE_INT32(MC_AT_EN, 0); -/** - * Start the autotuning sequence - * - * WARNING: this will inject steps to the rate controller - * and can be dangerous. Only activate if you know what you - * are doing, and in a safe environment. - * - * Any motion of the remote stick will abort the signal - * injection and reset this parameter - * Best is to perform the identification in position or - * hold mode. - * Increase the amplitude of the injected signal using - * MC_AT_SYSID_AMP for more signal/noise ratio - * - * @boolean - * @group Autotune - */ -PARAM_DEFINE_INT32(MC_AT_START, 0); /** * Amplitude of the injected signal diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index 713eb151b7..448fc57038 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -257,6 +257,7 @@ bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item, mission_item.nav_cmd != NAV_CMD_CONDITION_GATE && mission_item.nav_cmd != NAV_CMD_DO_WINCH && mission_item.nav_cmd != NAV_CMD_DO_GRIPPER && + mission_item.nav_cmd != NAV_CMD_DO_AUTOTUNE_ENABLE && mission_item.nav_cmd != NAV_CMD_DO_JUMP && mission_item.nav_cmd != NAV_CMD_DO_CHANGE_SPEED && mission_item.nav_cmd != NAV_CMD_DO_SET_HOME && diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 63a51042b1..d28abffc03 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -83,6 +83,7 @@ MissionBlock::is_mission_item_reached_or_completed() case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case NAV_CMD_COMPONENT_ARM_DISARM: + case NAV_CMD_DO_AUTOTUNE_ENABLE: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_ROI_LOCATION: case NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET: diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 1c32f3042e..a6b9977dfc 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -73,6 +73,7 @@ enum NAV_CMD { NAV_CMD_DO_MOUNT_CONFIGURE = 204, NAV_CMD_DO_MOUNT_CONTROL = 205, NAV_CMD_DO_GRIPPER = 211, + NAV_CMD_DO_AUTOTUNE_ENABLE = 212, NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214, NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206, NAV_CMD_OBLIQUE_SURVEY = 260, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c7bdd337b5..b07d92c176 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -759,6 +759,21 @@ void Navigator::run() reset_cruising_speed(); set_cruising_throttle(); } + + else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_AUTOTUNE_ENABLE) { + + + if (fabsf(cmd.param1 - 1.f) > FLT_EPSILON) { + // only support enabling autotune (consistent with autotune module) + events::send(events::ID("navigator_autotune_unsupported_input"), {events::Log::Warning, events::LogInternal::Warning}, + "Provided autotune command is not supported. To enable autotune in mission, set param1 to 1"); + + } else if (fabsf(cmd.param2) > FLT_EPSILON) { + // warn user about axis selection + events::send(events::ID("navigator_autotune_unsupported_ax"), {events::Log::Warning, events::LogInternal::Info}, + "Autotune axis selection not supported through Mission. Use FW_AT_AXES to set axes for fixed-wing vehicles"); + } + } } #if CONFIG_NAVIGATOR_ADSB