mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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 <hamishwillee@gmail.com> Signed-off-by: Julian Oes <julian@oes.ch> Assisted-by: Claude:claude-opus-5-5 * docs(navigator): list all supported mission commands Co-authored-by: Hamish Willee <hamishwillee@gmail.com> Assisted-by: Claude:claude-opus-5-5 Signed-off-by: Julian Oes <julian@oes.ch> * 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 <julian@oes.ch> 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 <julian@oes.ch> Co-authored-by: Julian Oes <julian@oes.ch>
This commit is contained in:
1 parent
722ebc9635
commit
4d755a614f
7 files changed
+234
-10
No files matched your search
@@ -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).
|
||||
|
||||
@@ -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. <!-- at LEAST until PX4 v1.13: https://github.com/PX4/PX4-Autopilot/issues/12660 -->
|
||||
- [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) .
|
||||
|
||||
@@ -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<int32_t>(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<double>(mission_item->params[4]));
|
||||
mavlink_mission_item->y = actuator_set_param_to_int(static_cast<double>(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;
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
Reference in new issue
Block a user