From 4d755a614f682509ccd4bb243260996c3433b655 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Tue, 17 Nov 2026 08:45:39 +1100 Subject: [PATCH] fix(mavlink): Make MAV_CMD_SET_ACTUATOR scale properly in missions (#27336) * fix(mavlink): scale DO_SET_ACTUATOR params 5/6 in mission items MISSION_ITEM_INT carries params 5/6 in the int32 x/y fields, so the actuator 5 and 6 values (-1..1) only arrived as -1, 0 or 1, and an unset (INT32_MAX) value drove the actuator to saturation instead of leaving it unchanged. Scale them by 1e7 like COMMAND_INT does, map unset to NaN, and encode them the same way on download. Co-authored-by: Hamish Willee Signed-off-by: Julian Oes Assisted-by: Claude:claude-opus-5-5 * docs(navigator): list all supported mission commands Co-authored-by: Hamish Willee Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes * fix(mavlink): keep ARM_DISARM and AUTOTUNE params on global-frame download A global-frame mission item zeroes param1-4 before filling in the command-specific ones, and COMPONENT_ARM_DISARM and DO_AUTOTUNE_ENABLE had no case there. An uploaded "arm" (param1 = 1) therefore downloaded as "disarm" (param1 = 0). Signed-off-by: Julian Oes Assisted-by: Claude:claude-opus-5-5 * fix(mavlink): ignore unset COMMAND_INT x and y individually COMMAND_INT x/y were only treated as unset when both were INT32_MAX. A DO_SET_ACTUATOR setting only actuator 5 (y = INT32_MAX) therefore set actuator 6 to 214.7, saturating it, instead of leaving it unchanged. Map each of x and y to NaN on its own when unset, which also covers INT32_MIN from a NaN converted to int. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes Co-authored-by: Julian Oes --- docs/en/flight_modes_fw/mission.md | 8 ++ docs/en/flight_modes_mc/mission.md | 8 ++ src/modules/mavlink/mavlink_mission.cpp | 47 +++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 18 +++-- test/mavsdk_tests/autopilot_tester.py | 45 +++++++++++ test/mavsdk_tests/test_command_int.py | 35 +++++++++ test/mavsdk_tests/test_mission_raw_items.py | 83 +++++++++++++++++++++ 7 files changed, 234 insertions(+), 10 deletions(-) create mode 100644 test/mavsdk_tests/test_command_int.py create mode 100644 test/mavsdk_tests/test_mission_raw_items.py diff --git a/docs/en/flight_modes_fw/mission.md b/docs/en/flight_modes_fw/mission.md index 64f47b73d29..84c714f797e 100644 --- a/docs/en/flight_modes_fw/mission.md +++ b/docs/en/flight_modes_fw/mission.md @@ -161,6 +161,8 @@ Mission Items: - [MAV_CMD_DO_SET_ROI_NONE](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_NONE) - [MAV_CMD_DO_CHANGE_SPEED](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_CHANGE_SPEED) - [MAV_CMD_DO_SET_HOME](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_HOME) +- [MAV_CMD_DO_WINCH](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_WINCH) +- [MAV_CMD_DO_GRIPPER](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GRIPPER) - [MAV_CMD_DO_SET_SERVO](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_SERVO) - [MAV_CMD_DO_LAND_START](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_LAND_START) - [MAV_CMD_DO_TRIGGER_CONTROL](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_TRIGGER_CONTROL) @@ -174,7 +176,11 @@ Mission Items: - [MAV_CMD_DO_SET_CAM_TRIGG_DIST](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_CAM_TRIGG_DIST) - [MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL) - [MAV_CMD_SET_CAMERA_MODE](https://mavlink.io/en/messages/common.html#MAV_CMD_SET_CAMERA_MODE) +- [MAV_CMD_SET_CAMERA_SOURCE](https://mavlink.io/en/messages/common.html#MAV_CMD_SET_CAMERA_SOURCE) - [MAV_CMD_NAV_DELAY](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_DELAY) +- [MAV_CMD_CONDITION_DELAY](https://mavlink.io/en/messages/common.html#MAV_CMD_CONDITION_DELAY) + - PX4 converts it to a `MAV_CMD_NAV_DELAY`: the vehicle stops and waits for _param1_ seconds. + It is also downloaded as `MAV_CMD_NAV_DELAY`. - [MAV_CMD_NAV_RETURN_TO_LAUNCH](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_RETURN_TO_LAUNCH) - [MAV_CMD_DO_CONTROL_VIDEO](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_CONTROL_VIDEO) - [MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) @@ -182,6 +188,8 @@ 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_SET_ACTUATOR](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ACTUATOR) +- [MAV_CMD_COMPONENT_ARM_DISARM](https://mavlink.io/en/messages/common.html#MAV_CMD_COMPONENT_ARM_DISARM) - [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). diff --git a/docs/en/flight_modes_mc/mission.md b/docs/en/flight_modes_mc/mission.md index bd3d9443b26..0ee0aa72269 100644 --- a/docs/en/flight_modes_mc/mission.md +++ b/docs/en/flight_modes_mc/mission.md @@ -163,6 +163,8 @@ Mission Items: - [MAV_CMD_DO_SET_ROI_NONE](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_NONE) - [MAV_CMD_DO_CHANGE_SPEED](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_CHANGE_SPEED) - [MAV_CMD_DO_SET_HOME](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_HOME) +- [MAV_CMD_DO_WINCH](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_WINCH) +- [MAV_CMD_DO_GRIPPER](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GRIPPER) - [MAV_CMD_DO_SET_SERVO](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_SERVO) - [MAV_CMD_DO_LAND_START](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_LAND_START) - [MAV_CMD_DO_TRIGGER_CONTROL](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_TRIGGER_CONTROL) @@ -176,7 +178,11 @@ Mission Items: - [MAV_CMD_DO_SET_CAM_TRIGG_DIST](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_CAM_TRIGG_DIST) - [MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL) - [MAV_CMD_SET_CAMERA_MODE](https://mavlink.io/en/messages/common.html#MAV_CMD_SET_CAMERA_MODE) +- [MAV_CMD_SET_CAMERA_SOURCE](https://mavlink.io/en/messages/common.html#MAV_CMD_SET_CAMERA_SOURCE) - [MAV_CMD_NAV_DELAY](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_DELAY) +- [MAV_CMD_CONDITION_DELAY](https://mavlink.io/en/messages/common.html#MAV_CMD_CONDITION_DELAY) + - PX4 converts it to a `MAV_CMD_NAV_DELAY`: the vehicle stops and waits for _param1_ seconds. + It is also downloaded as `MAV_CMD_NAV_DELAY`. - [MAV_CMD_NAV_RETURN_TO_LAUNCH](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_RETURN_TO_LAUNCH) - [MAV_CMD_DO_CONTROL_VIDEO](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_CONTROL_VIDEO) - [MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) @@ -187,6 +193,8 @@ 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_SET_ACTUATOR](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ACTUATOR) +- [MAV_CMD_COMPONENT_ARM_DISARM](https://mavlink.io/en/messages/common.html#MAV_CMD_COMPONENT_ARM_DISARM) - [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) . diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index edecf65fd32..9dab758a5c6 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -63,6 +63,23 @@ using matrix::wrap_2pi; +namespace +{ + +// DO_SET_ACTUATOR param5/param6 (actuator 5 and 6, -1..1) are scaled by 1e7 in the int32 x/y fields. +constexpr double ACTUATOR_SET_INT_SCALE = 1e7; + +int32_t actuator_set_param_to_int(double value) +{ + if (!PX4_ISFINITE(value)) { + return INT32_MAX; // "param not used" + } + + return static_cast(round(value * ACTUATOR_SET_INT_SCALE)); +} + +} // namespace + dm_item_t MavlinkMissionManager::_mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; dm_item_t MavlinkMissionManager::_safepoint_dataman_id = DM_KEY_SAFE_POINTS_0; dm_item_t MavlinkMissionManager::_fence_dataman_id = DM_KEY_FENCE_POINTS_0; @@ -1689,12 +1706,21 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_int case MAV_CMD_NAV_RETURN_TO_LAUNCH: case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET: case MAV_CMD_DO_SET_ROI_NONE: - 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; + case MAV_CMD_DO_SET_ACTUATOR: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + // Actuator 5 and 6 are floats (-1..1) carried in the int32 x/y fields, scaled by 1e7 + // (same as COMMAND_INT). An unset value becomes NaN, which leaves the actuator unchanged. + mission_item->params[4] = mavlink_cmd_params::int_param_is_unset(mavlink_mission_item->x) ? (double)NAN : + mavlink_mission_item->x / ACTUATOR_SET_INT_SCALE; + mission_item->params[5] = mavlink_cmd_params::int_param_is_unset(mavlink_mission_item->y) ? (double)NAN : + mavlink_mission_item->y / ACTUATOR_SET_INT_SCALE; + break; + case MAV_CMD_CONDITION_DELAY: // PX4 has no dedicated condition-delay execution; treat it as a NAV_DELAY, // which holds for param1 (stored in params[0], aliasing time_inside) seconds. @@ -1741,8 +1767,15 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->param3 = mission_item->params[2]; mavlink_mission_item->param4 = mission_item->params[3]; - mavlink_mission_item->x = round(mission_item->params[4]); - mavlink_mission_item->y = round(mission_item->params[5]); + if (mission_item->nav_cmd == NAV_CMD_DO_SET_ACTUATOR) { + mavlink_mission_item->x = actuator_set_param_to_int(static_cast(mission_item->params[4])); + mavlink_mission_item->y = actuator_set_param_to_int(static_cast(mission_item->params[5])); + + } else { + mavlink_mission_item->x = round(mission_item->params[4]); + mavlink_mission_item->y = round(mission_item->params[5]); + } + mavlink_mission_item->z = mission_item->params[6]; switch (mavlink_mission_item->command) { @@ -1853,6 +1886,14 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case MAV_CMD_NAV_RALLY_POINT: break; + case NAV_CMD_COMPONENT_ARM_DISARM: + mavlink_mission_item->param1 = mission_item->params[0]; + break; + + case NAV_CMD_DO_AUTOTUNE_ENABLE: + mavlink_mission_item->param1 = mission_item->params[0]; + mavlink_mission_item->param2 = mission_item->params[1]; + break; default: return PX4_ERROR; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d88a8b6dc8b..31421ce0d90 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -675,13 +675,8 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) vcmd.param3 = cmd_mavlink.param3; vcmd.param4 = cmd_mavlink.param4; - if (cmd_mavlink.x == INT32_MAX && cmd_mavlink.y == INT32_MAX) { - // INT32_MAX for x and y means to ignore it. - vcmd.param5 = (double)NAN; - vcmd.param6 = (double)NAN; - - } else if (command_has_location(cmd_mavlink.command) - || cmd_mavlink.command == MAV_CMD_DO_SET_ACTUATOR) { // actuator values use 1e7 scaling + if (command_has_location(cmd_mavlink.command) + || cmd_mavlink.command == MAV_CMD_DO_SET_ACTUATOR) { // actuator values use 1e7 scaling if (cmd_mavlink.frame == MAV_FRAME_LOCAL_NED || cmd_mavlink.frame == MAV_FRAME_LOCAL_ENU || cmd_mavlink.frame == MAV_FRAME_LOCAL_OFFSET_NED @@ -704,6 +699,15 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) vcmd.param6 = (double)cmd_mavlink.y; } + // Each of x and y is ignored on its own when unset, e.g. DO_SET_ACTUATOR setting only actuator 5. + if (mavlink_cmd_params::int_param_is_unset(cmd_mavlink.x)) { + vcmd.param5 = (double)NAN; + } + + if (mavlink_cmd_params::int_param_is_unset(cmd_mavlink.y)) { + vcmd.param6 = (double)NAN; + } + vcmd.param7 = cmd_mavlink.z; vcmd.command = cmd_mavlink.command; vcmd.target_system = mavlink_target_system(msg, cmd_mavlink.target_system); diff --git a/test/mavsdk_tests/autopilot_tester.py b/test/mavsdk_tests/autopilot_tester.py index 1a99cec278a..82a7ad893b3 100644 --- a/test/mavsdk_tests/autopilot_tester.py +++ b/test/mavsdk_tests/autopilot_tester.py @@ -45,6 +45,8 @@ MAV_CMD_NAV_LAND = 21 MAV_CMD_NAV_TAKEOFF = 22 MAV_CMD_NAV_VTOL_LAND = 85 MAV_CMD_DO_JUMP = 177 +MAV_CMD_DO_SET_ACTUATOR = 187 +MAV_CMD_COMPONENT_ARM_DISARM = 400 MAV_CMD_REQUEST_MESSAGE = 512 MAV_FRAME_GLOBAL_INT = 5 MAV_FRAME_GLOBAL_RELATIVE_ALT = 3 @@ -55,6 +57,16 @@ MAVLINK_MSG_ID_HOME_POSITION = 242 MAV_RESULT_ACCEPTED = 0 MAV_RESULT_IN_PROGRESS = 5 +# MAVLink "int32 param not used" value for the x/y fields. +INT32_MAX = 2**31 - 1 + +# DO_SET_ACTUATOR param5/param6 (actuator 5 and 6, -1..1) are scaled by 1e7 in the int32 x/y fields. +ACTUATOR_SET_INT_SCALE = 1e7 +# Output function of "Peripheral via Actuator Set 1"; set N is this + N - 1. +ACTUATOR_SET1_FUNCTION = 301 +PWM_MIN_US = 1000.0 +PWM_MAX_US = 2000.0 + # Like MAVSDK's command sender, but in host time with a longer timeout for sanitizer builds. COMMAND_ACK_TIMEOUT_S = 1.0 COMMAND_ATTEMPTS = 3 @@ -401,6 +413,36 @@ class AutopilotTester: def enable_actuator_output_status(self) -> None: self._telemetry.set_rate_actuator_output_status(20.0) + def route_actuator_set_to_output(self, actuator: int, output: int) -> None: + """Drive PWM output (1-based) with DO_SET_ACTUATOR actuator (1-based) of set index 0.""" + self.set_param_int("PWM_MAIN_FUNC{}".format(output), ACTUATOR_SET1_FUNCTION + actuator - 1) + + def wait_for_actuator_set_output(self, output: int, value: float, timeout_s: float) -> None: + """Wait until PWM output (1-based) shows actuator set value (-1..1).""" + pwm_us = PWM_MIN_US + (value + 1.0) / 2.0 * (PWM_MAX_US - PWM_MIN_US) + self.wait_for_actuator_output(output - 1, pwm_us, timeout_s) + + def wait_for_actuator_output_value(self, index: int, timeout_s: float) -> float: + """Return output index (0-based) of ACTUATOR_OUTPUT_STATUS once one has arrived.""" + def received() -> bool: + return index < len(self._telemetry.actuator_output_status().actuator) + + assert self.poll_condition_with_timeout(received, timeout_s), \ + "No ACTUATOR_OUTPUT_STATUS within {} s".format(timeout_s) + return float(self._telemetry.actuator_output_status().actuator[index]) + + def wait_for_actuator_output(self, index: int, value: float, timeout_s: float, + tolerance: float = 1.0) -> None: + """Wait until output index (0-based) of ACTUATOR_OUTPUT_STATUS is within tolerance.""" + def reached() -> bool: + outputs = self._telemetry.actuator_output_status().actuator + # Empty until the first ACTUATOR_OUTPUT_STATUS arrives. + return index < len(outputs) and abs(float(outputs[index]) - value) <= tolerance + + assert self.poll_condition_with_timeout(reached, timeout_s), \ + "Actuator output {} not {} within {} s, last outputs {}".format( + index, value, timeout_s, self._telemetry.actuator_output_status().actuator) + def ensure_motors_stopped(self, stopped_mask: int, num_motors: int) -> None: """Check that exactly the motors in stopped_mask (0-based bits) are stopped.""" status = self._telemetry.actuator_output_status() @@ -702,6 +744,9 @@ class AutopilotTester: # PX4 needs time to realize that it now has a mission available. self.sleep_for(1) + def download_mission_raw(self) -> List[Any]: + return list(self._mission_raw.download_mission()) + def load_qgc_mission_raw_and_move_here(self, plan_file: str) -> None: """Upload a QGC .plan mission, shifted so that it starts at the current position.""" items = self._mission_raw.import_qgroundcontrol_mission(plan_file).mission_items diff --git a/test/mavsdk_tests/test_command_int.py b/test/mavsdk_tests/test_command_int.py new file mode 100644 index 00000000000..004ac8d776c --- /dev/null +++ b/test/mavsdk_tests/test_command_int.py @@ -0,0 +1,35 @@ +import math + +import pytest + +from autopilot_tester import ( + ACTUATOR_SET_INT_SCALE, INT32_MAX, MAV_CMD_DO_SET_ACTUATOR, MAV_FRAME_MISSION, AutopilotTester) + +# PWM outputs unused by the SIH quad. +ACTUATOR5_OUTPUT = 5 +ACTUATOR6_OUTPUT = 6 + + +@pytest.mark.multicopter +def test_command_int_set_actuator_single_unset_param(tester: AutopilotTester) -> None: + """A COMMAND_INT DO_SET_ACTUATOR leaves an actuator unchanged when only its own x/y is unset. + + Only x and y both unset used to be ignored, so setting just actuator 5 (y = INT32_MAX) + drove actuator 6 to saturation. + """ + tester.wait_until_ready() + tester.route_actuator_set_to_output(5, ACTUATOR5_OUTPUT) + tester.route_actuator_set_to_output(6, ACTUATOR6_OUTPUT) + tester.enable_actuator_output_status() + tester.arm() + # Never set, so it outputs its disarmed value. + actuator6_us = tester.wait_for_actuator_output_value(ACTUATOR6_OUTPUT - 1, 5) + + actuator5 = 0.5 + tester.send_command_int(MAV_CMD_DO_SET_ACTUATOR, param1=math.nan, param2=math.nan, + param3=math.nan, param4=math.nan, + x=round(actuator5 * ACTUATOR_SET_INT_SCALE), y=INT32_MAX, z=0.0, + frame=MAV_FRAME_MISSION) + + tester.wait_for_actuator_set_output(ACTUATOR5_OUTPUT, actuator5, 5) + tester.wait_for_actuator_output(ACTUATOR6_OUTPUT - 1, actuator6_us, 1) diff --git a/test/mavsdk_tests/test_mission_raw_items.py b/test/mavsdk_tests/test_mission_raw_items.py new file mode 100644 index 00000000000..cb502b24aaf --- /dev/null +++ b/test/mavsdk_tests/test_mission_raw_items.py @@ -0,0 +1,83 @@ +import math +from typing import Any + +import pytest + +from autopilot_tester import ( + ACTUATOR_SET_INT_SCALE, INT32_MAX, MAV_CMD_COMPONENT_ARM_DISARM, MAV_CMD_DO_SET_ACTUATOR, + MAV_CMD_NAV_LAND, MAV_CMD_NAV_TAKEOFF, MAV_CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_RELATIVE_ALT, + MAV_FRAME_MISSION, AutopilotTester, raw_mission_item) + +SPARE_OUTPUT = 5 # PWM output unused by the SIH quad + + +def _global_item(tester: AutopilotTester, seq: int, command: int, north_m: float, east_m: float, + altitude_m: float, current: int = 0) -> Any: + latitude_deg, longitude_deg = tester.get_coordinate_transformation().global_from_local( + north_m, east_m) + return raw_mission_item(seq, command, MAV_FRAME_GLOBAL_RELATIVE_ALT, + x=round(latitude_deg * 1e7), y=round(longitude_deg * 1e7), + z=altitude_m, current=current) + + +@pytest.mark.multicopter +def test_mission_set_actuator_scaling(tester: AutopilotTester) -> None: + """DO_SET_ACTUATOR param5/param6 in a mission item are scaled by 1e7, like COMMAND_INT. + + Unscaled, a fractional actuator 5 value arrived as an integer (-1, 0 or 1) and an unset + (INT32_MAX) one drove the actuator to saturation instead of leaving it unchanged. + """ + tester.wait_until_ready() + tester.route_actuator_set_to_output(5, SPARE_OUTPUT) + tester.enable_actuator_output_status() + + actuator5 = 0.5 + set_actuator = raw_mission_item(2, MAV_CMD_DO_SET_ACTUATOR, MAV_FRAME_MISSION, + x=round(actuator5 * ACTUATOR_SET_INT_SCALE), y=INT32_MAX) + # Actuators 1 to 4 left unchanged, actuator set index 0. + set_actuator.param1 = set_actuator.param2 = set_actuator.param3 = set_actuator.param4 = math.nan + + altitude_m = 5.0 + tester.upload_mission_raw([ + _global_item(tester, 0, MAV_CMD_NAV_TAKEOFF, 0.0, 0.0, altitude_m, current=1), + _global_item(tester, 1, MAV_CMD_NAV_WAYPOINT, 5.0, 0.0, altitude_m), + set_actuator, + _global_item(tester, 3, MAV_CMD_NAV_WAYPOINT, 0.0, 0.0, altitude_m), + _global_item(tester, 4, MAV_CMD_NAV_LAND, 0.0, 0.0, 0.0), + ]) + + downloaded = tester.download_mission_raw()[2] + assert downloaded.command == MAV_CMD_DO_SET_ACTUATOR + assert downloaded.x == round(actuator5 * ACTUATOR_SET_INT_SCALE), \ + "actuator 5 downloaded as {}".format(downloaded.x) + assert downloaded.y == INT32_MAX, "unset actuator 6 downloaded as {}".format(downloaded.y) + + tester.arm() + tester.start_mission_raw_and_wait_for_sequence(3) + tester.wait_for_actuator_set_output(SPARE_OUTPUT, actuator5, 5) + tester.wait_until_disarmed(120) + + +@pytest.mark.multicopter +def test_mission_global_frame_arm_disarm(tester: AutopilotTester) -> None: + """COMPONENT_ARM_DISARM is accepted in a global frame too, not only in MAV_FRAME_MISSION, + and downloads with its params (a dropped param1 would turn "arm" into "disarm"). + + upload_mission_raw() raises if the vehicle rejects the mission. + """ + tester.wait_until_ready() + + # x/y (latitude/longitude) must be unset: COMPONENT_ARM_DISARM only supports param1/param2. + arm = raw_mission_item(1, MAV_CMD_COMPONENT_ARM_DISARM, MAV_FRAME_GLOBAL_RELATIVE_ALT, + x=INT32_MAX, y=INT32_MAX, z=math.nan, param1=1.0) + tester.upload_mission_raw([ + _global_item(tester, 0, MAV_CMD_NAV_TAKEOFF, 0.0, 0.0, 5.0, current=1), + arm, + _global_item(tester, 2, MAV_CMD_NAV_LAND, 0.0, 0.0, 0.0), + ]) + + downloaded = tester.download_mission_raw()[1] + assert downloaded.command == MAV_CMD_COMPONENT_ARM_DISARM + assert downloaded.param1 == 1.0, "param1 downloaded as {}".format(downloaded.param1) + assert (downloaded.x, downloaded.y) == (INT32_MAX, INT32_MAX), \ + "x/y downloaded as {}/{}".format(downloaded.x, downloaded.y)