diff --git a/src/modules/mavlink/mavlink_command_params.hpp b/src/modules/mavlink/mavlink_command_params.hpp new file mode 100644 index 000000000000..3e4b0e1effb2 --- /dev/null +++ b/src/modules/mavlink/mavlink_command_params.hpp @@ -0,0 +1,306 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_command_params.hpp + * + * Per-MAV_CMD bitmask table declaring which params (1–7) PX4 supports. + * Params not in the mask must be NaN (or 0.0, the conventional GCS default) + * on receipt; any other value is rejected at the MAVLink boundary. + * + * Two masks per entry — one for mission items, one for commands: + * bits 0–3: params 1–4 (bit 0 = param1, …, bit 3 = param4) + * bits 4–6: params 5–7 (bit 4 = param5, bit 5 = param6, bit 6 = param7) + * + * For global-frame MISSION_ITEM_INT, check_params_int_for_vehicle() is used so + * that p5/p6 are validated as int32 (INT32_MAX = unset) and p7 as float. + * + * A secondary VehicleParamOverrides table holds per-airframe additions (e.g. + * NAV_TAKEOFF pitch angle on FW); use check_params_for_vehicle() for callers + * that know the vehicle type. + * + * The table must remain sorted ascending by cmd for binary search. + * The static_assert below enforces this at compile time. + * When adding a new MAV_CMD to PX4's mission or command handler, add an entry. + */ + +#pragma once + +#include +#include +#include +#include + +namespace mavlink_cmd_params +{ + +struct Entry { + uint16_t cmd; + uint8_t mission; // bits 0-3: params 1-4; bits 4-6: params 5-7 (mission items) + uint8_t command; // bits 0-3: params 1-4; bits 4-6: params 5-7 (COMMAND_LONG/INT) +}; + +// Keep sorted by cmd value. Update when adding new supported commands or params. +// Symbolic names are listed in comments; raw integers are used so this header +// remains self-contained (aside from vehicle_status.h for VEHICLE_TYPE_* constants). +// +// Encoding: mission/command byte = (params1_4_mask) | (params5_7_mask << 4) +// where params1_4_mask has bit N = param N+1 for N in 0..3, +// and params5_7_mask has bit N = param N+5 for N in 0..2. +static constexpr Entry SupportedCommandParams[] = { + // cmd mission command + { 16, 0x7B, 0x7B }, // NAV_WAYPOINT: p1:hold,p2:accept_r,p4:yaw; p5-7:lat/lon/alt + { 17, 0x7C, 0x7C }, // NAV_LOITER_UNLIM: p3:radius,p4:yaw; p5-7:lat/lon/alt + { 19, 0x7F, 0x7F }, // NAV_LOITER_TIME: p1-p4 all used; p5-7:lat/lon/alt + { 20, 0x00, 0x00 }, // NAV_RETURN_TO_LAUNCH: no params + { 21, 0x7B, 0x7B }, // NAV_LAND: p1:abort_alt,p2:precision,p4:yaw; p5-7:lat/lon/alt + { 22, 0x78, 0x78 }, // NAV_TAKEOFF: p4:yaw; p5-7:lat/lon/alt (FW/VTOL also get p1 via override) + { 31, 0x7B, 0x7B }, // NAV_LOITER_TO_ALT: p1:hdg,p2:radius,p4:xtrack; p5-7:lat/lon/alt + { 80, 0x77, 0x77 }, // NAV_ROI: p1:mode,p2:wp_idx,p3:roi_idx; p5-7:lat/lon/alt + { 84, 0x78, 0x7C }, // NAV_VTOL_TAKEOFF: mission:p4:yaw only (p3 unused by mission_block); cmd:p3:approach_hdg,p4:yaw; p5-7:lat/lon/alt + { 85, 0x78, 0x7F }, // NAV_VTOL_LAND: mission:p4:yaw only (p1-3 unused by mission_block); cmd:p1:options,p2:approach_hdg,p3:loiter_r,p4:yaw; p5-7:lat/lon/alt + { 93, 0x0F, 0x0F }, // NAV_DELAY: p1:delay,p2:hour,p3:min,p4:sec + { 112, 0x01, 0x01 }, // CONDITION_DELAY: p1:seconds + { 114, 0x01, 0x01 }, // CONDITION_DISTANCE: p1:distance + { 176, 0x07, 0x07 }, // DO_SET_MODE: p1:mode,p2:custom,p3:submode + { 177, 0x03, 0x03 }, // DO_JUMP: p1:index,p2:count + { 178, 0x07, 0x07 }, // DO_CHANGE_SPEED: p1:type,p2:speed,p3:throttle + { 179, 0x7F, 0x7F }, // DO_SET_HOME: p1:use_current,p2:roll,p3:pitch,p4:yaw; p5-7:lat/lon/alt + { 189, 0x00, 0x00 }, // DO_LAND_START: no params + { 195, 0x70, 0x71 }, // DO_SET_ROI_LOCATION: mission:p5-7:lat/lon/alt; cmd:p1:gimbal,p5-7:lat/lon/alt + { 196, 0x01, 0x01 }, // DO_SET_ROI_WPNEXT_OFFSET: p1:gimbal_id + { 197, 0x01, 0x01 }, // DO_SET_ROI_NONE: p1:gimbal_id + { 201, 0x07, 0x07 }, // DO_SET_ROI: p1:mode,p2:wp_idx,p3:roi_idx + { 206, 0x0F, 0x0F }, // DO_SET_CAM_TRIGG_DIST: p1:dist,p2:shutter,p3:trigger,p4:camera_id + { 211, 0x03, 0x03 }, // DO_GRIPPER: p1:id,p2:action + { 212, 0x03, 0x03 }, // DO_AUTOTUNE_ENABLE: p1:enable,p2:axis + { 214, 0x07, 0x07 }, // DO_SET_CAM_TRIGG_INTERVAL: p1:cycle,p2:shutter,p3:camera_id + { 400, 0x03, 0x03 }, // COMPONENT_ARM_DISARM: p1:arm,p2:force + { 420, 0x07, 0x07 }, // INJECT_FAILURE: p1:unit,p2:type,p3:instance + { 530, 0x03, 0x03 }, // SET_CAMERA_MODE: p1:camera_id,p2:mode + { 532, 0x07, 0x07 }, // SET_CAMERA_FOCUS: p1:focus_type,p2:value,p3:camera_id + { 534, 0x07, 0x07 }, // SET_CAMERA_SOURCE: p1:camera_id,p2:primary,p3:secondary + { 2000, 0x0F, 0x0F }, // IMAGE_START_CAPTURE: p1:camera_id,p2:interval,p3:total,p4:seq + { 2001, 0x01, 0x01 }, // IMAGE_STOP_CAPTURE: p1:camera_id + { 2003, 0x0F, 0x0F }, // DO_TRIGGER_CONTROL: p1:enable,p2:reset,p3:pause,p4:camera_id + { 2500, 0x07, 0x07 }, // VIDEO_START_CAPTURE: p1:stream_id,p2:status_freq,p3:camera_id + { 2501, 0x03, 0x03 }, // VIDEO_STOP_CAPTURE: p1:stream_id,p2:camera_id + { 3000, 0x03, 0x03 }, // DO_VTOL_TRANSITION: p1:state,p2:force_immediate + { 4501, 0x00, 0x00 }, // CONDITION_GATE: no params used by PX4 + { 5000, 0x70, 0x00 }, // NAV_FENCE_RETURN_POINT: mission:p5-7:lat/lon/alt; cmd:none + { 5001, 0x31, 0x01 }, // NAV_FENCE_POLYGON_VERTEX_INCLUSION: p1:vertex_count; mission:p5-6:lat/lon + { 5002, 0x31, 0x01 }, // NAV_FENCE_POLYGON_VERTEX_EXCLUSION: p1:vertex_count; mission:p5-6:lat/lon + { 5003, 0x31, 0x01 }, // NAV_FENCE_CIRCLE_INCLUSION: p1:radius; mission:p5-6:lat/lon + { 5004, 0x31, 0x01 }, // NAV_FENCE_CIRCLE_EXCLUSION: p1:radius; mission:p5-6:lat/lon + { 5100, 0x70, 0x00 }, // NAV_RALLY_POINT: mission:p5-7:lat/lon/alt; cmd:none + {42600, 0x0F, 0x0F }, // DO_WINCH: p1-p4 all used +}; + +static constexpr size_t SupportedCommandParamsCount = sizeof(SupportedCommandParams) / sizeof(SupportedCommandParams[0]); + +// Verify the table is sorted at compile time. +static constexpr bool _is_sorted() +{ + for (size_t i = 1; i < SupportedCommandParamsCount; ++i) { + if (SupportedCommandParams[i].cmd <= SupportedCommandParams[i - 1].cmd) { return false; } + } + + return true; +} + +static_assert(_is_sorted(), "mavlink_command_params::SupportedCommandParams must be sorted ascending by cmd"); + +// Bit-mask helpers avoid float comparison (-Wfloat-equal) and fpclassify (absent in NuttX). +static inline bool param_is_unset(float v) +{ + uint32_t bits; + __builtin_memcpy(&bits, &v, sizeof(bits)); + return std::isnan(v) || (bits & 0x7FFFFFFFu) == 0u; +} + +// True when v is exactly ±0.0 (not NaN). Used to detect GCS sending 0 instead of NaN. +static inline bool param_is_zero(float v) +{ + uint32_t bits; + __builtin_memcpy(&bits, &v, sizeof(bits)); + return !std::isnan(v) && (bits & 0x7FFFFFFFu) == 0u; +} + +// INT32_MAX is the MAVLink sentinel for "int32 param not provided". +static inline bool int_param_is_unset(int32_t v) +{ + return v == INT32_MAX; +} + +// Vehicle type bitmask for per-vehicle parameter support. +// bit 0 = fixed-wing (FW), bit 1 = multicopter (MC), bit 2 = VTOL, bit 3 = rover. +enum VehicleType : uint8_t { + VEHICLE_FW = (1u << 0), + VEHICLE_MC = (1u << 1), + VEHICLE_VTOL = (1u << 2), + VEHICLE_ROVER = (1u << 3), +}; + +// Maps vehicle_status_s fields to a VehicleType bitmask. +static inline uint8_t vehicle_type_bitmask(bool is_vtol, uint8_t vehicle_type) +{ + if (is_vtol) { return VEHICLE_VTOL; } + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { return VEHICLE_FW; } + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { return VEHICLE_MC; } + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { return VEHICLE_ROVER; } + + return 0; +} + +// Extends the base table for vehicle-specific params that differ across airframe types. +// Each entry's masks are OR'd with the base Entry masks when the vehicle_mask matches. +struct VehicleOverride { + uint16_t cmd; + uint8_t vehicle_mask; // bitmask of VehicleType values this entry applies to + uint8_t mission; // additional allowed param bits (bits 0-3 = p1-p4, bits 4-6 = p5-p7) + uint8_t command; // additional allowed param bits (bits 0-3 = p1-p4, bits 4-6 = p5-p7) +}; + +// When adding vehicle-specific param differences, add entries here. +static constexpr VehicleOverride VehicleParamOverrides[] = { + // NAV_TAKEOFF: p1 (minimum pitch angle) is used by FW and VTOL-FW; not applicable to MC. + { 22, VEHICLE_FW | VEHICLE_VTOL, 0x01, 0x01 }, +}; + +// Binary search + vehicle override lookup. Returns the adjusted mask, or -1 if cmd not in table. +static int _find_mask(uint16_t cmd, bool for_mission, uint8_t vehicle_type) +{ + size_t lo = 0; + size_t hi = SupportedCommandParamsCount - 1; + + while (lo <= hi) { + const size_t mid = lo + (hi - lo) / 2; + + if (SupportedCommandParams[mid].cmd == cmd) { + uint8_t mask = for_mission + ? SupportedCommandParams[mid].mission + : SupportedCommandParams[mid].command; + + for (const auto &ov : VehicleParamOverrides) { + if (ov.cmd == cmd && (ov.vehicle_mask & vehicle_type)) { + mask |= for_mission ? ov.mission : ov.command; + } + } + + return mask; + + } else if (SupportedCommandParams[mid].cmd < cmd) { + lo = mid + 1; + + } else { + if (mid == 0) { break; } + + hi = mid - 1; + } + } + + return -1; +} + +// Shared by check_params_for_vehicle and check_params_int_for_vehicle so the scan +// logic exists once instead of being duplicated per param-count variant. +static int _scan_params(uint8_t mask, const float *ps, int count, uint8_t *zero_sentinel_mask) +{ + for (int i = 0; i < count; ++i) { + if (!((mask >> i) & 1u)) { + if (!param_is_unset(ps[i])) { return i + 1; } + + if (zero_sentinel_mask && param_is_zero(ps[i])) { + *zero_sentinel_mask |= (uint8_t)(1u << i); + } + } + } + + return 0; +} + +/** + * Check params 1–7 of an incoming mission item or command against the base table, + * then OR-extend the allowed mask with any matching VehicleParamOverrides entry. + * + * @param vehicle_type bitmask from vehicle_type_bitmask() — VEHICLE_FW / VEHICLE_MC / VEHICLE_VTOL + * @return 0 all unsupported params are unset + * 1–7 1-based index of the first offending param + * -1 command not in table (no validation applied) + */ +[[maybe_unused]] static int check_params_for_vehicle(uint16_t cmd, bool for_mission, uint8_t vehicle_type, + float p1, float p2, float p3, float p4, + float p5 = 0.0f, float p6 = 0.0f, float p7 = 0.0f, + uint8_t *zero_sentinel_mask = nullptr) +{ + const int mask_result = _find_mask(cmd, for_mission, vehicle_type); + + if (mask_result < 0) { return -1; } + + const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; + return _scan_params((uint8_t)mask_result, ps, 7, zero_sentinel_mask); +} + +/** + * Variant of check_params_for_vehicle for global-frame MISSION_ITEM_INT where p5/p6 + * are raw int32 lat/lon (INT32_MAX = unset) and p7 (altitude) remains a float. + */ +[[maybe_unused]] static int check_params_int_for_vehicle(uint16_t cmd, bool for_mission, uint8_t vehicle_type, + float p1, float p2, float p3, float p4, + int32_t p5_int, int32_t p6_int, float p7 = 0.0f, + uint8_t *zero_sentinel_mask = nullptr) +{ + const int mask_result = _find_mask(cmd, for_mission, vehicle_type); + + if (mask_result < 0) { return -1; } + + const uint8_t mask = (uint8_t)mask_result; + const float ps14[4] = {p1, p2, p3, p4}; + + const int bad = _scan_params(mask, ps14, 4, zero_sentinel_mask); + + if (bad != 0) { return bad; } + + if (!((mask >> 4) & 1u) && !int_param_is_unset(p5_int)) { return 5; } + + if (!((mask >> 5) & 1u) && !int_param_is_unset(p6_int)) { return 6; } + + if (!((mask >> 6) & 1u) && !param_is_unset(p7)) { return 7; } + + return 0; +} + +} // namespace mavlink_cmd_params diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 441407952dc1..6a1c59a8fe96 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -42,6 +42,7 @@ #include "mavlink_mission.h" #include "mavlink_main.h" +#include "mavlink_command_params.hpp" #include #include @@ -1442,6 +1443,34 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->altitude_is_relative = true; } + { + uint8_t zero_mask = 0; + int bad = -1; + + if (_int_mode) { + const mavlink_mission_item_int_t *item_int = + reinterpret_cast(mavlink_mission_item); + bad = mavlink_cmd_params::check_params_int_for_vehicle(mavlink_mission_item->command, true, _vehicle_type_bitmask, + mavlink_mission_item->param1, mavlink_mission_item->param2, + mavlink_mission_item->param3, mavlink_mission_item->param4, + item_int->x, item_int->y, + mavlink_mission_item->z, &zero_mask); + + } else { + bad = mavlink_cmd_params::check_params_for_vehicle(mavlink_mission_item->command, true, _vehicle_type_bitmask, + mavlink_mission_item->param1, mavlink_mission_item->param2, + mavlink_mission_item->param3, mavlink_mission_item->param4, + mavlink_mission_item->x, mavlink_mission_item->y, + mavlink_mission_item->z, &zero_mask); + } + + if (bad > 0) { return MAV_MISSION_INVALID_PARAM1 + (bad - 1); } + + if (bad < 0) { PX4_DEBUG("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.hpp", (unsigned)mavlink_mission_item->command); } + + if (zero_mask) { PX4_DEBUG("MAV_CMD %u: unsupported params with 0.0 sentinel (use NaN) mask=0x%02x", (unsigned)mavlink_mission_item->command, zero_mask); } + } + // Depending on the received MAV_CMD_* (MAVLink Commands), assign the corresponding // NAV_CMD value to the mission item's nav_cmd. switch (mavlink_mission_item->command) { @@ -1571,6 +1600,38 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * // This is a mission item with no coordinates + { + uint8_t zero_mask = 0; + int bad = -1; + + if (_int_mode) { + const mavlink_mission_item_int_t *item_int = + reinterpret_cast(mavlink_mission_item); + // x/y are p5/p6 generic params (not lat/lon) for non-position frames. + // Normalize INT32_MAX (MISSION_ITEM_INT "unused" sentinel) to NaN so + // both the int sentinel and the NaN are treated as unset. + const float p5 = mavlink_cmd_params::int_param_is_unset(item_int->x) ? NAN : (float)item_int->x; + const float p6 = mavlink_cmd_params::int_param_is_unset(item_int->y) ? NAN : (float)item_int->y; + bad = mavlink_cmd_params::check_params_for_vehicle(mavlink_mission_item->command, true, _vehicle_type_bitmask, + mavlink_mission_item->param1, mavlink_mission_item->param2, + mavlink_mission_item->param3, mavlink_mission_item->param4, + p5, p6, mavlink_mission_item->z, &zero_mask); + + } else { + bad = mavlink_cmd_params::check_params_for_vehicle(mavlink_mission_item->command, true, _vehicle_type_bitmask, + mavlink_mission_item->param1, mavlink_mission_item->param2, + mavlink_mission_item->param3, mavlink_mission_item->param4, + (float)mavlink_mission_item->x, (float)mavlink_mission_item->y, + mavlink_mission_item->z, &zero_mask); + } + + if (bad > 0) { return MAV_MISSION_INVALID_PARAM1 + (bad - 1); } + + if (bad < 0) { PX4_DEBUG("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.hpp", (unsigned)mavlink_mission_item->command); } + + if (zero_mask) { PX4_DEBUG("MAV_CMD %u: unsupported params with 0.0 sentinel (use NaN) mask=0x%02x", (unsigned)mavlink_mission_item->command, zero_mask); } + } + mission_item->params[0] = mavlink_mission_item->param1; mission_item->params[1] = mavlink_mission_item->param2; mission_item->params[2] = mavlink_mission_item->param3; @@ -1950,6 +2011,8 @@ MavlinkMissionManager::update_mission_state() return; } + _vehicle_type_bitmask = mavlink_cmd_params::vehicle_type_bitmask(vehicle_status.is_vtol, vehicle_status.vehicle_type); + // Get mission result const mission_result_s &mission_result = _mission_result_sub.get(); diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index e35c668c918d..aef8c356ddd4 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -151,6 +151,7 @@ class MavlinkMissionManager uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; uORB::SubscriptionData _mission_sub{ORB_ID(mission)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; ///< vehicle status subscription + uint8_t _vehicle_type_bitmask{0}; ///< cached from vehicle_status; vehicle type requires reboot to change uORB::Publication _offboard_mission_pub{ORB_ID(mission)}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9d271f25145d..0c5a65c81a7b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -58,6 +58,7 @@ #endif #include "mavlink_command_sender.h" +#include "mavlink_command_params.hpp" #include "mavlink_main.h" #include "mavlink_receiver.h" @@ -671,6 +672,23 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const return; } + uint8_t zero_mask = 0; + const int command_invalid = mavlink_cmd_params::check_params_for_vehicle(cmd_mavlink.command, false, _vehicle_type_bitmask, + vehicle_command.param1, vehicle_command.param2, + vehicle_command.param3, vehicle_command.param4, + vehicle_command.param5, vehicle_command.param6, vehicle_command.param7, + &zero_mask); + + if (command_invalid > 0) { + acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, + vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED); + return; + } + + if (command_invalid < 0) { PX4_DEBUG("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.hpp", (unsigned)cmd_mavlink.command); } + + if (zero_mask) { PX4_DEBUG("MAV_CMD %u: unsupported params with 0.0 sentinel (use NaN) mask=0x%02x", (unsigned)cmd_mavlink.command, zero_mask); } + if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { if (set_message_interval( (int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3, cmd_mavlink.param4, vehicle_command.param7)) { @@ -3677,6 +3695,12 @@ MavlinkReceiver::run() updateParams(); } + if (_vehicle_status_sub.updated()) { + vehicle_status_s vs{}; + _vehicle_status_sub.copy(&vs); + _vehicle_type_bitmask = mavlink_cmd_params::vehicle_type_bitmask(vs.is_vtol, vs.vehicle_type); + } + // Reload signing key if another instance updated it _mavlink.check_signing_key_dirty(); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 644f1afe8711..d42d7cdca51a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -418,6 +418,7 @@ class MavlinkReceiver : public ModuleParams uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uint8_t _vehicle_type_bitmask{0}; ///< cached from vehicle_status; vehicle type requires reboot to change uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/test/test_mavlink_param_validation.py b/test/test_mavlink_param_validation.py new file mode 100644 index 000000000000..eba4aa38d86f --- /dev/null +++ b/test/test_mavlink_param_validation.py @@ -0,0 +1,426 @@ +#!/usr/bin/env python3 +""" +MAVLink parameter-validation regression test. + +Verifies that PX4 rejects mission items and commands carrying non-zero +values in parameter slots that the MAVLink spec marks as unsupported for +that MAV_CMD, and accepts identical items/commands with those slots +zeroed or NaN. + +Usage: + python3 test_mavlink_param_validation.py [--url udp://:14540] [--timeout 5] + +Requirements: + pip install pymavlink +""" + +import argparse +import sys +import time +from typing import Any, Optional + +try: + from pymavlink import mavutil # type: ignore[import-not-found] +except ImportError: + print("ERROR: pymavlink not installed. Run: pip install pymavlink") + sys.exit(1) + +# MAVLink enum constants + +MAV_MISSION_ACCEPTED = 0 +MAV_MISSION_INVALID_PARAM1 = 6 +MAV_MISSION_INVALID_PARAM2 = 7 +MAV_MISSION_INVALID_PARAM3 = 8 +MAV_MISSION_INVALID_PARAM4 = 9 +MAV_MISSION_INVALID_PARAM5 = 10 +MAV_MISSION_INVALID_PARAM6 = 11 +MAV_MISSION_INVALID_PARAM7 = 12 + +MAV_RESULT_ACCEPTED = 0 +MAV_RESULT_DENIED = 2 + +MAV_FRAME_GLOBAL_INT = 5 +MAV_FRAME_GLOBAL_RELATIVE_ALT = 3 +MAV_FRAME_MISSION = 2 + +# MAV_CMD values used in tests +CMD_NAV_WAYPOINT = 16 +CMD_NAV_RTL = 20 +CMD_NAV_TAKEOFF = 22 +CMD_NAV_VTOL_TAKEOFF = 84 +CMD_NAV_VTOL_LAND = 85 +CMD_NAV_DELAY = 93 +CMD_COMPONENT_ARM_DISARM = 400 + +NAN = float("nan") +INT32_MAX = 2_147_483_647 + +# Helpers + +PASS = "\033[32mPASS\033[0m" +FAIL = "\033[31mFAIL\033[0m" +_results: list[bool] = [] + + +def _check(label: str, got: Any, expected: Any) -> bool: + ok = bool(got == expected) + _results.append(ok) + status = PASS if ok else FAIL + exp_name = ( + _mission_result_name(expected) + if isinstance(expected, int) else expected + ) + got_name = ( + _mission_result_name(got) + if isinstance(got, int) else got + ) + print(f" [{status}] {label}") + if not ok: + print( + f" got={got_name} ({got})," + f" expected={exp_name} ({expected})" + ) + return ok + + +def _mission_result_name(v: int) -> str: + names = { + 0: "ACCEPTED", 6: "INVALID_PARAM1", 7: "INVALID_PARAM2", + 8: "INVALID_PARAM3", 9: "INVALID_PARAM4", 10: "INVALID_PARAM5", + 11: "INVALID_PARAM6", 12: "INVALID_PARAM7", + } + return names.get(v, str(v)) + + +def connect(url: str) -> Any: + mav = mavutil.mavlink_connection(url) + print(f"Waiting for heartbeat on {url} ...") + mav.wait_heartbeat(timeout=15) + print( + f"Connected: sysid={mav.target_system}" + f" compid={mav.target_component}\n" + ) + return mav + + +def _upload_mission( + mav: Any, items: list[dict[str, Any]], timeout: float +) -> Optional[int]: + """ + Run the MISSION_ITEM_INT upload protocol and return the + MAV_MISSION_RESULT from the final MISSION_ACK, or None on timeout. + """ + mav.mav.mission_count_send( + mav.target_system, mav.target_component, + len(items), 0, # mission_type=0 (main) + ) + + deadline = time.monotonic() + timeout * (len(items) + 2) + + while time.monotonic() < deadline: + msg = mav.recv_match( + type=["MISSION_REQUEST_INT", "MISSION_REQUEST", "MISSION_ACK"], + blocking=True, + timeout=timeout, + ) + if msg is None: + return None + + t = msg.get_type() + + if t in ("MISSION_REQUEST_INT", "MISSION_REQUEST"): + item = items[msg.seq] + mav.mav.mission_item_int_send( + item["sys"], item["comp"], + item["seq"], item["frame"], item["cmd"], + item["current"], item["autocontinue"], + item["p1"], item["p2"], item["p3"], item["p4"], + item["x"], item["y"], item["z"], + 0, # mission_type + ) + + elif t == "MISSION_ACK": + return int(msg.type) + + return None + + +def _item( + seq: int, cmd: int, frame: int, + p1: float = NAN, p2: float = NAN, + p3: float = NAN, p4: float = NAN, + x: int = INT32_MAX, y: int = INT32_MAX, z: float = 0.0, + current: int = 0, +) -> dict[str, Any]: + return dict( + sys=1, comp=1, seq=seq, cmd=cmd, frame=frame, + current=current, autocontinue=1, + p1=p1, p2=p2, p3=p3, p4=p4, + x=x, y=y, z=z, + ) + + +def _send_command( + mav: Any, cmd: int, timeout: float, + p1: float = 0.0, p2: float = 0.0, + p3: float = 0.0, p4: float = 0.0, + p5: float = 0.0, p6: float = 0.0, p7: float = 0.0, +) -> Optional[int]: + """Send COMMAND_LONG and return the MAV_RESULT from the ACK, or None.""" + mav.mav.command_long_send( + mav.target_system, mav.target_component, + cmd, 0, + p1, p2, p3, p4, p5, p6, p7, + ) + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + msg = mav.recv_match( + type="COMMAND_ACK", blocking=True, timeout=timeout, + ) + if msg and msg.command == cmd: + return int(msg.result) + return None + + +# Test cases + + +def run_mission_tests(mav: Any, timeout: float) -> None: + print("=== Mission upload tests ===") + + # Coordinates for a valid waypoint (47.397 N 8.545 E 50 m) + LAT = 473_977_420 # 1e-7 deg + LON = 85_456_060 # 1e-7 deg + ALT = 50.0 + + # 1. Valid NAV_WAYPOINT + # mask 0x0B: p1 (hold), p2 (accept_radius), p4 (yaw); p3 unsupported. + result = _upload_mission(mav, [ + _item(0, CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_INT, + p1=0.0, p2=2.0, p3=NAN, p4=NAN, + x=LAT, y=LON, z=ALT, current=1), + ], timeout) + _check("Valid NAV_WAYPOINT -> ACCEPTED", result, MAV_MISSION_ACCEPTED) + + # 2. NAV_WAYPOINT with unsupported param3 set + result = _upload_mission(mav, [ + _item(0, CMD_NAV_WAYPOINT, MAV_FRAME_GLOBAL_INT, + p1=0.0, p2=2.0, p3=1.0, p4=NAN, + x=LAT, y=LON, z=ALT, current=1), + ], timeout) + _check( + "NAV_WAYPOINT unsupported param3 -> INVALID_PARAM3", + result, MAV_MISSION_INVALID_PARAM3, + ) + + # 3. NAV_RTL with param1 set (mask 0x00, no params). RTL is only a valid + # mission item under MAV_FRAME_MISSION (no coordinates) in PX4; sending + # it as MAV_FRAME_GLOBAL_INT hits the frame switch's default case + # (MAV_MISSION_UNSUPPORTED) once params pass, regardless of param values. + result = _upload_mission(mav, [ + _item(0, CMD_NAV_RTL, MAV_FRAME_MISSION, + p1=1.0, p2=NAN, p3=NAN, p4=NAN, current=1), + ], timeout) + _check( + "NAV_RTL unsupported param1 -> INVALID_PARAM1", + result, MAV_MISSION_INVALID_PARAM1, + ) + + # 4. NAV_RTL with all params NaN (should pass). mask 0x00 means p5-7 + # (x/y/z) are unsupported too, so they must stay at the unset sentinel. + result = _upload_mission(mav, [ + _item(0, CMD_NAV_RTL, MAV_FRAME_MISSION, + p1=NAN, p2=NAN, p3=NAN, p4=NAN, current=1), + ], timeout) + _check( + "NAV_RTL all params NaN -> ACCEPTED", + result, MAV_MISSION_ACCEPTED, + ) + + # 5. NAV_DELAY with unsupported param5 (MAV_FRAME_MISSION, x=param5) + # mask 0x0F; mission567=0x00 -> p5/p6/p7 not supported + result = _upload_mission(mav, [ + _item(0, CMD_NAV_DELAY, MAV_FRAME_MISSION, + p1=10.0, p2=NAN, p3=NAN, p4=NAN, + x=42, y=0, z=0.0), + ], timeout) + _check( + "NAV_DELAY unsupported param5 (x=42) -> INVALID_PARAM5", + result, MAV_MISSION_INVALID_PARAM5, + ) + + # 6. NAV_DELAY valid (p1 set, p5/p6/p7 zero) + result = _upload_mission(mav, [ + _item(0, CMD_NAV_DELAY, MAV_FRAME_MISSION, + p1=10.0, p2=NAN, p3=NAN, p4=NAN, + x=0, y=0, z=0.0), + ], timeout) + _check( + "NAV_DELAY valid (p1=10, p5/p6/p7=0) -> ACCEPTED", + result, MAV_MISSION_ACCEPTED, + ) + + # 7. NAV_TAKEOFF with unsupported param2 set (mask 0x08: only p4) + result = _upload_mission(mav, [ + _item(0, CMD_NAV_TAKEOFF, MAV_FRAME_GLOBAL_INT, + p1=NAN, p2=5.0, p3=NAN, p4=NAN, + x=LAT, y=LON, z=ALT, current=1), + ], timeout) + _check( + "NAV_TAKEOFF unsupported param2 -> INVALID_PARAM2", + result, MAV_MISSION_INVALID_PARAM2, + ) + + # 13. NAV_VTOL_LAND mission with unsupported param1 set (mission mask + # 0x78: only p4/yaw; mission_block.cpp never reads p1-p3 for VTOL_LAND). + result = _upload_mission(mav, [ + _item(0, CMD_NAV_VTOL_LAND, MAV_FRAME_GLOBAL_INT, + p1=1.0, p2=NAN, p3=NAN, p4=NAN, + x=LAT, y=LON, z=ALT, current=1), + ], timeout) + _check( + "NAV_VTOL_LAND mission unsupported param1 -> INVALID_PARAM1", + result, MAV_MISSION_INVALID_PARAM1, + ) + + # 14. NAV_VTOL_LAND mission valid (only p4/yaw set) + result = _upload_mission(mav, [ + _item(0, CMD_NAV_VTOL_LAND, MAV_FRAME_GLOBAL_INT, + p1=NAN, p2=NAN, p3=NAN, p4=90.0, + x=LAT, y=LON, z=ALT, current=1), + ], timeout) + _check( + "NAV_VTOL_LAND mission valid (p4=yaw only) -> ACCEPTED", + result, MAV_MISSION_ACCEPTED, + ) + + # 15. NAV_VTOL_TAKEOFF mission with unsupported param3 set (mission mask + # 0x78: only p4/yaw; mission_block.cpp never reads p3/approach_hdg). + result = _upload_mission(mav, [ + _item(0, CMD_NAV_VTOL_TAKEOFF, MAV_FRAME_GLOBAL_INT, + p1=NAN, p2=NAN, p3=45.0, p4=NAN, + x=LAT, y=LON, z=ALT, current=1), + ], timeout) + _check( + "NAV_VTOL_TAKEOFF mission unsupported param3 -> INVALID_PARAM3", + result, MAV_MISSION_INVALID_PARAM3, + ) + + # 16. NAV_VTOL_TAKEOFF mission valid (only p4/yaw set) + result = _upload_mission(mav, [ + _item(0, CMD_NAV_VTOL_TAKEOFF, MAV_FRAME_GLOBAL_INT, + p1=NAN, p2=NAN, p3=NAN, p4=90.0, + x=LAT, y=LON, z=ALT, current=1), + ], timeout) + _check( + "NAV_VTOL_TAKEOFF mission valid (p4=yaw only) -> ACCEPTED", + result, MAV_MISSION_ACCEPTED, + ) + + +def run_command_tests(mav: Any, timeout: float) -> None: + print("\n=== Command (COMMAND_LONG) tests ===") + + # 8. Valid COMPONENT_ARM_DISARM (p1=0 disarm, p2=0 no-force) + result = _send_command( + mav, CMD_COMPONENT_ARM_DISARM, timeout, p1=0.0, p2=0.0, + ) + _check( + "COMPONENT_ARM_DISARM valid params -> not DENIED", + result != MAV_RESULT_DENIED, True, + ) + + # 9. COMPONENT_ARM_DISARM with unsupported param3 set (mask 0x03) + result = _send_command( + mav, CMD_COMPONENT_ARM_DISARM, timeout, p1=0.0, p2=0.0, p3=1.0, + ) + _check( + "COMPONENT_ARM_DISARM unsupported param3 -> DENIED", + result, MAV_RESULT_DENIED, + ) + + # 10. NAV_RTL command with param1 set (mask 0x00) + result = _send_command(mav, CMD_NAV_RTL, timeout, p1=1.0) + _check( + "NAV_RTL command unsupported param1 -> DENIED", + result, MAV_RESULT_DENIED, + ) + + # 11. NAV_RTL command all params zero -> not DENIED + result = _send_command(mav, CMD_NAV_RTL, timeout) + _check( + "NAV_RTL command all params zero -> not DENIED", + result != MAV_RESULT_DENIED, True, + ) + + # 12. NAV_WAYPOINT command unsupported param3 (mask 0x0B) + result = _send_command( + mav, CMD_NAV_WAYPOINT, timeout, p1=0.0, p2=2.0, p3=1.0, p4=0.0, + ) + _check( + "NAV_WAYPOINT command unsupported param3 -> DENIED", + result, MAV_RESULT_DENIED, + ) + + # 17. NAV_VTOL_LAND command with p1 (land options) set -> supported + # on the command path (mask 0x7F), unlike the mission path. + result = _send_command( + mav, CMD_NAV_VTOL_LAND, timeout, p1=1.0, + ) + _check( + "NAV_VTOL_LAND command p1 (options) -> not DENIED", + result != MAV_RESULT_DENIED, True, + ) + + # 18. NAV_VTOL_TAKEOFF command with p1 set -> unsupported even on the + # command path (mask 0x7C: only p3/p4 + p5-7). + result = _send_command( + mav, CMD_NAV_VTOL_TAKEOFF, timeout, p1=1.0, + ) + _check( + "NAV_VTOL_TAKEOFF command unsupported param1 -> DENIED", + result, MAV_RESULT_DENIED, + ) + + # 19. NAV_VTOL_TAKEOFF command with p3 (approach heading) set -> + # supported on the command path (mask 0x7C). + result = _send_command( + mav, CMD_NAV_VTOL_TAKEOFF, timeout, p3=45.0, + ) + _check( + "NAV_VTOL_TAKEOFF command p3 (approach_hdg) -> not DENIED", + result != MAV_RESULT_DENIED, True, + ) + + +# Entry point + + +def main() -> int: + parser = argparse.ArgumentParser( + description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + parser.add_argument( + "--url", default="udp://:14540", + help="MAVLink connection URL (default: udp://:14540)", + ) + parser.add_argument( + "--timeout", type=float, default=5.0, + help="Per-message receive timeout in seconds (default: 5)", + ) + args = parser.parse_args() + + mav = connect(args.url) + + run_mission_tests(mav, args.timeout) + run_command_tests(mav, args.timeout) + + passed = sum(_results) + total = len(_results) + print(f"\nResult: {passed}/{total} passed") + return 0 if passed == total else 1 + + +if __name__ == "__main__": + sys.exit(main())