From 07bc1d198a4b60b9e6e85e0f7065fe112a13cea7 Mon Sep 17 00:00:00 2001 From: Himaghna Date: Mon, 1 Jun 2026 19:34:45 -0400 Subject: [PATCH 01/14] fix(mavlink): reject unsupported params in commands and missions PX4 accepted any non-NaN value in params it does not use for a given MAV_CMD, silently storing or ignoring them. This made it impossible for a GCS to detect that a mission item or command was malformed, and risked unexpected behaviour if the param meaning is assigned in a future update. Add mavlink_command_params.h: a header-only sorted lookup table mapping each supported MAV_CMD to uint8 bitmasks (one for mission items, one for commands) indicating which of params 1-4 are valid. check_params() does a binary search and returns the 1-based index of the first offending param, or 0 if all unsupported params are unset. A param is considered unset when it is NaN (the MAVLink standard) or 0.0 (the conventional GCS default for unused float fields). Any other value in an unsupported slot is rejected: - Mission uploads (MISSION_ITEM / MISSION_ITEM_INT): NACK with MAV_MISSION_INVALID_PARAMn for the offending param index. - Commands (COMMAND_LONG / COMMAND_INT): ACK with MAV_RESULT_DENIED. Validation is placed at the MAVLink ingress boundary (parse_mavlink_mission_item and handle_message_command_both) before any downstream processing, keeping Commander and Navigator unmodified. The table covers all 45 MAV_CMDs handled by PX4's mission and command parsers. The static_assert enforces sort order at compile time. Fixes #27483 Signed-off-by: Himaghna --- src/modules/mavlink/mavlink_command_params.h | 205 +++++++++++++++++++ src/modules/mavlink/mavlink_mission.cpp | 35 ++++ src/modules/mavlink/mavlink_receiver.cpp | 16 ++ 3 files changed, 256 insertions(+) create mode 100644 src/modules/mavlink/mavlink_command_params.h diff --git a/src/modules/mavlink/mavlink_command_params.h b/src/modules/mavlink/mavlink_command_params.h new file mode 100644 index 000000000000..1073e88cb892 --- /dev/null +++ b/src/modules/mavlink/mavlink_command_params.h @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * 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.h + * + * 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. + * + * Three masks per entry: + * mission – params 1-4 for mission items (MISSION_ITEM / MISSION_ITEM_INT) + * command – params 1-4 for commands (COMMAND_LONG / COMMAND_INT) + * params567 – params 5-7 for both; callers pass 0.0 to skip checking + * + * mission/command: bit 0 = param1, bit 1 = param2, bit 2 = param3, bit 3 = param4. + * params567: bit 0 = param5, bit 1 = param6, bit 2 = param7. + * + * For global-frame mission items the caller passes 0.0 for p5–p7 so that + * lat/lon/alt are never checked through this table. + * + * 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 + +namespace mavlink_cmd_params +{ + +struct Entry { + uint16_t cmd; + uint8_t mission; // supported param bits (1-4) for mission items + uint8_t command; // supported param bits (1-4) for COMMAND_LONG/INT + uint8_t params567; // supported param bits for params 5-7 (same for mission and command) +}; + +// 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 and requires no MAVLink includes. +static constexpr Entry SupportedCommandParams[] = { + { 16, 0x0B, 0x0B, 0x07 }, // NAV_WAYPOINT: p1:hold,p2:accept_r,p4:yaw; p5-7:lat/lon/alt + { 17, 0x0C, 0x0C, 0x07 }, // NAV_LOITER_UNLIM: p3:radius,p4:yaw; p5-7:lat/lon/alt + { 19, 0x0F, 0x0F, 0x07 }, // NAV_LOITER_TIME: p1-p4 all used; p5-7:lat/lon/alt + { 20, 0x00, 0x00, 0x00 }, // NAV_RETURN_TO_LAUNCH: no params + { 21, 0x0A, 0x0A, 0x07 }, // NAV_LAND: p2:precision,p4:yaw; p5-7:lat/lon/alt + { 22, 0x08, 0x08, 0x07 }, // NAV_TAKEOFF: p4:yaw; p5-7:lat/lon/alt + { 31, 0x0B, 0x0B, 0x07 }, // NAV_LOITER_TO_ALT: p1:hdg,p2:radius,p4:xtrack; p5-7:lat/lon/alt + { 80, 0x07, 0x07, 0x07 }, // NAV_ROI: p1:mode,p2:wp_idx,p3:roi_idx; p5-7:lat/lon/alt + { 84, 0x08, 0x08, 0x07 }, // NAV_VTOL_TAKEOFF: p4:yaw; p5-7:lat/lon/alt + { 85, 0x08, 0x08, 0x07 }, // NAV_VTOL_LAND: p4:yaw; p5-7:lat/lon/alt + { 93, 0x0F, 0x0F, 0x00 }, // NAV_DELAY: p1:delay,p2:hour,p3:min,p4:sec + { 112, 0x01, 0x01, 0x00 }, // CONDITION_DELAY: p1:seconds + { 114, 0x01, 0x01, 0x00 }, // CONDITION_DISTANCE: p1:distance + { 176, 0x07, 0x07, 0x00 }, // DO_SET_MODE: p1:mode,p2:custom,p3:submode + { 177, 0x03, 0x03, 0x00 }, // DO_JUMP: p1:index,p2:count + { 178, 0x07, 0x07, 0x00 }, // DO_CHANGE_SPEED: p1:type,p2:speed,p3:throttle + { 179, 0x0F, 0x0F, 0x07 }, // DO_SET_HOME: p1:use_current,p2:roll,p3:pitch,p4:yaw; p5-7:lat/lon/alt + { 189, 0x00, 0x00, 0x00 }, // DO_LAND_START: no params + { 195, 0x00, 0x01, 0x07 }, // DO_SET_ROI_LOCATION: cmd:p1:gimbal; p5-7:lat/lon/alt + { 196, 0x01, 0x01, 0x00 }, // DO_SET_ROI_WPNEXT_OFFSET: p1:gimbal_id + { 197, 0x01, 0x01, 0x00 }, // DO_SET_ROI_NONE: p1:gimbal_id + { 201, 0x07, 0x07, 0x00 }, // DO_SET_ROI: p1:mode,p2:wp_idx,p3:roi_idx + { 206, 0x0F, 0x0F, 0x00 }, // DO_SET_CAM_TRIGG_DIST: p1:dist,p2:shutter,p3:trigger,p4:camera_id + { 211, 0x03, 0x03, 0x00 }, // DO_GRIPPER: p1:id,p2:action + { 212, 0x03, 0x03, 0x00 }, // DO_AUTOTUNE_ENABLE: p1:enable,p2:axis + { 214, 0x07, 0x07, 0x00 }, // DO_SET_CAM_TRIGG_INTERVAL: p1:cycle,p2:shutter,p3:camera_id + { 400, 0x03, 0x03, 0x00 }, // COMPONENT_ARM_DISARM: p1:arm,p2:force + { 420, 0x07, 0x07, 0x00 }, // INJECT_FAILURE: p1:unit,p2:type,p3:instance + { 530, 0x03, 0x03, 0x00 }, // SET_CAMERA_MODE: p1:camera_id,p2:mode + { 532, 0x07, 0x07, 0x00 }, // SET_CAMERA_FOCUS: p1:focus_type,p2:value,p3:camera_id + { 534, 0x07, 0x07, 0x00 }, // SET_CAMERA_SOURCE: p1:camera_id,p2:primary,p3:secondary + { 2000, 0x0F, 0x0F, 0x00 }, // IMAGE_START_CAPTURE: p1:camera_id,p2:interval,p3:total,p4:seq + { 2001, 0x01, 0x01, 0x00 }, // IMAGE_STOP_CAPTURE: p1:camera_id + { 2003, 0x0F, 0x0F, 0x00 }, // DO_TRIGGER_CONTROL: p1:enable,p2:reset,p3:pause,p4:camera_id + { 2500, 0x07, 0x07, 0x00 }, // VIDEO_START_CAPTURE: p1:stream_id,p2:status_freq,p3:camera_id + { 2501, 0x03, 0x03, 0x00 }, // VIDEO_STOP_CAPTURE: p1:stream_id,p2:camera_id + { 3000, 0x03, 0x03, 0x00 }, // DO_VTOL_TRANSITION: p1:state,p2:force_immediate + { 4501, 0x00, 0x00, 0x00 }, // CONDITION_GATE: no params used by PX4 + { 5000, 0x00, 0x00, 0x07 }, // NAV_FENCE_RETURN_POINT: p5-7:lat/lon/alt + { 5001, 0x01, 0x01, 0x03 }, // NAV_FENCE_POLYGON_VERTEX_INCLUSION: p1:vertex_count; p5-6:lat/lon + { 5002, 0x01, 0x01, 0x03 }, // NAV_FENCE_POLYGON_VERTEX_EXCLUSION: p1:vertex_count; p5-6:lat/lon + { 5003, 0x01, 0x01, 0x03 }, // NAV_FENCE_CIRCLE_INCLUSION: p1:radius; p5-6:lat/lon + { 5004, 0x01, 0x01, 0x03 }, // NAV_FENCE_CIRCLE_EXCLUSION: p1:radius; p5-6:lat/lon + { 5100, 0x00, 0x00, 0x07 }, // NAV_RALLY_POINT: p5-7:lat/lon/alt + {42600, 0x0F, 0x0F, 0x00 }, // 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"); + +// Returns true when the float value means "param not provided" per MAVLink convention. +// Accepts NaN (the standard) and ±0.0 (the common GCS default for unused fields). +// Bit-mask avoids 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; +} + +/** + * Check params 1–7 of an incoming mission item or command against the table. + * + * Pass 0.0f for p5–p7 to skip checking those params (e.g. for global-frame + * mission items where p5–p7 carry lat/lon/alt). + * + * @param cmd MAV_CMD value + * @param for_mission true for mission items, false for COMMAND_LONG/INT + * @param p1–p7 raw float param values from the MAVLink message + * @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(uint16_t cmd, bool for_mission, + float p1, float p2, float p3, float p4, + float p5 = 0.0f, float p6 = 0.0f, float p7 = 0.0f) +{ + // Binary search + 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) { + const uint8_t mask = for_mission ? SupportedCommandParams[mid].mission : SupportedCommandParams[mid].command; + const uint8_t mask567 = SupportedCommandParams[mid].params567; + + if (!(mask & (1u << 0)) && !param_is_unset(p1)) { return 1; } + + if (!(mask & (1u << 1)) && !param_is_unset(p2)) { return 2; } + + if (!(mask & (1u << 2)) && !param_is_unset(p3)) { return 3; } + + if (!(mask & (1u << 3)) && !param_is_unset(p4)) { return 4; } + + if (!(mask567 & (1u << 0)) && !param_is_unset(p5)) { return 5; } + + if (!(mask567 & (1u << 1)) && !param_is_unset(p6)) { return 6; } + + if (!(mask567 & (1u << 2)) && !param_is_unset(p7)) { return 7; } + + return 0; + + } else if (SupportedCommandParams[mid].cmd < cmd) { + lo = mid + 1; + + } else { + if (mid == 0) { break; } + + hi = mid - 1; + } + } + + return -1; // command not in table — no validation applied +} + +} // namespace mavlink_cmd_params diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 441407952dc1..b0a60d053b70 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.h" #include #include @@ -1442,6 +1443,22 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->altitude_is_relative = true; } + // Reject populated params that this command does not support. + // p5-7 are lat/lon/alt for global-frame items; pass 0.0 to skip checking them. + { + const int bad = mavlink_cmd_params::check_params(mavlink_mission_item->command, true, + mavlink_mission_item->param1, mavlink_mission_item->param2, + mavlink_mission_item->param3, mavlink_mission_item->param4); + + if (bad > 0) { + return MAV_MISSION_INVALID_PARAM1 + (bad - 1); + } + + if (bad < 0) { + PX4_DEBUG("MAV_CMD %u not in param validation table", (unsigned)mavlink_mission_item->command); + } + } + // 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 +1588,24 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * // This is a mission item with no coordinates + // Reject populated params that this command does not support. + // For MAV_FRAME_MISSION items x/y/z are command data, not coordinates. + { + const int bad = mavlink_cmd_params::check_params(mavlink_mission_item->command, true, + 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); + + if (bad > 0) { + return MAV_MISSION_INVALID_PARAM1 + (bad - 1); + } + + if (bad < 0) { + PX4_DEBUG("MAV_CMD %u not in param validation table", (unsigned)mavlink_mission_item->command); + } + } + mission_item->params[0] = mavlink_mission_item->param1; mission_item->params[1] = mavlink_mission_item->param2; mission_item->params[2] = mavlink_mission_item->param3; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9d271f25145d..a61531f64a2d 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.h" #include "mavlink_main.h" #include "mavlink_receiver.h" @@ -671,6 +672,21 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const return; } + const int command_invalid = mavlink_cmd_params::check_params(cmd_mavlink.command, false, + vehicle_command.param1, vehicle_command.param2, + vehicle_command.param3, vehicle_command.param4, + vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); + + 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", (unsigned)cmd_mavlink.command); + } + 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)) { From bfd93c3e9efa9a2eeb38150b22bd0a9861c931f6 Mon Sep 17 00:00:00 2001 From: Himaghna Date: Tue, 9 Jun 2026 19:24:49 -0400 Subject: [PATCH 02/14] fix(mavlink): split mission/command p5-7 masks, fix int_mode branch, add vehicle overrides Signed-off-by: Himaghna --- src/modules/mavlink/mavlink_command_params.h | 336 ++++++++++++++---- src/modules/mavlink/mavlink_mission.cpp | 47 ++- src/modules/mavlink/mavlink_receiver.cpp | 10 +- .../test_mavlink_param_validation.py | 288 +++++++++++++++ 4 files changed, 580 insertions(+), 101 deletions(-) create mode 100644 test/mavsdk_tests/test_mavlink_param_validation.py diff --git a/src/modules/mavlink/mavlink_command_params.h b/src/modules/mavlink/mavlink_command_params.h index 1073e88cb892..e2a2db1c23b5 100644 --- a/src/modules/mavlink/mavlink_command_params.h +++ b/src/modules/mavlink/mavlink_command_params.h @@ -38,16 +38,21 @@ * 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. * - * Three masks per entry: - * mission – params 1-4 for mission items (MISSION_ITEM / MISSION_ITEM_INT) - * command – params 1-4 for commands (COMMAND_LONG / COMMAND_INT) - * params567 – params 5-7 for both; callers pass 0.0 to skip checking + * Four masks per entry: + * mission – params 1-4 for mission items (MISSION_ITEM / MISSION_ITEM_INT) + * command – params 1-4 for commands (COMMAND_LONG / COMMAND_INT) + * mission567 – params 5-7 for mission items + * command567 – params 5-7 for commands * - * mission/command: bit 0 = param1, bit 1 = param2, bit 2 = param3, bit 3 = param4. - * params567: bit 0 = param5, bit 1 = param6, bit 2 = param7. + * mission/command: bit 0 = param1, bit 1 = param2, bit 2 = param3, bit 3 = param4. + * mission567/command567: bit 0 = param5, bit 1 = param6, bit 2 = param7. * - * For global-frame mission items the caller passes 0.0 for p5–p7 so that - * lat/lon/alt are never checked through this table. + * For global-frame MISSION_ITEM_INT, check_params_int() is used so that p5/p6 + * are validated as int32 (INT32_MAX = unset) and p7 as float (altitude). + * + * 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. @@ -65,60 +70,62 @@ namespace mavlink_cmd_params struct Entry { uint16_t cmd; - uint8_t mission; // supported param bits (1-4) for mission items - uint8_t command; // supported param bits (1-4) for COMMAND_LONG/INT - uint8_t params567; // supported param bits for params 5-7 (same for mission and command) + uint8_t mission; // supported param bits (1-4) for mission items + uint8_t command; // supported param bits (1-4) for COMMAND_LONG/INT + uint8_t mission567; // supported param bits for params 5-7 in mission items + uint8_t command567; // supported param bits for params 5-7 in commands }; // 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 and requires no MAVLink includes. static constexpr Entry SupportedCommandParams[] = { - { 16, 0x0B, 0x0B, 0x07 }, // NAV_WAYPOINT: p1:hold,p2:accept_r,p4:yaw; p5-7:lat/lon/alt - { 17, 0x0C, 0x0C, 0x07 }, // NAV_LOITER_UNLIM: p3:radius,p4:yaw; p5-7:lat/lon/alt - { 19, 0x0F, 0x0F, 0x07 }, // NAV_LOITER_TIME: p1-p4 all used; p5-7:lat/lon/alt - { 20, 0x00, 0x00, 0x00 }, // NAV_RETURN_TO_LAUNCH: no params - { 21, 0x0A, 0x0A, 0x07 }, // NAV_LAND: p2:precision,p4:yaw; p5-7:lat/lon/alt - { 22, 0x08, 0x08, 0x07 }, // NAV_TAKEOFF: p4:yaw; p5-7:lat/lon/alt - { 31, 0x0B, 0x0B, 0x07 }, // NAV_LOITER_TO_ALT: p1:hdg,p2:radius,p4:xtrack; p5-7:lat/lon/alt - { 80, 0x07, 0x07, 0x07 }, // NAV_ROI: p1:mode,p2:wp_idx,p3:roi_idx; p5-7:lat/lon/alt - { 84, 0x08, 0x08, 0x07 }, // NAV_VTOL_TAKEOFF: p4:yaw; p5-7:lat/lon/alt - { 85, 0x08, 0x08, 0x07 }, // NAV_VTOL_LAND: p4:yaw; p5-7:lat/lon/alt - { 93, 0x0F, 0x0F, 0x00 }, // NAV_DELAY: p1:delay,p2:hour,p3:min,p4:sec - { 112, 0x01, 0x01, 0x00 }, // CONDITION_DELAY: p1:seconds - { 114, 0x01, 0x01, 0x00 }, // CONDITION_DISTANCE: p1:distance - { 176, 0x07, 0x07, 0x00 }, // DO_SET_MODE: p1:mode,p2:custom,p3:submode - { 177, 0x03, 0x03, 0x00 }, // DO_JUMP: p1:index,p2:count - { 178, 0x07, 0x07, 0x00 }, // DO_CHANGE_SPEED: p1:type,p2:speed,p3:throttle - { 179, 0x0F, 0x0F, 0x07 }, // DO_SET_HOME: p1:use_current,p2:roll,p3:pitch,p4:yaw; p5-7:lat/lon/alt - { 189, 0x00, 0x00, 0x00 }, // DO_LAND_START: no params - { 195, 0x00, 0x01, 0x07 }, // DO_SET_ROI_LOCATION: cmd:p1:gimbal; p5-7:lat/lon/alt - { 196, 0x01, 0x01, 0x00 }, // DO_SET_ROI_WPNEXT_OFFSET: p1:gimbal_id - { 197, 0x01, 0x01, 0x00 }, // DO_SET_ROI_NONE: p1:gimbal_id - { 201, 0x07, 0x07, 0x00 }, // DO_SET_ROI: p1:mode,p2:wp_idx,p3:roi_idx - { 206, 0x0F, 0x0F, 0x00 }, // DO_SET_CAM_TRIGG_DIST: p1:dist,p2:shutter,p3:trigger,p4:camera_id - { 211, 0x03, 0x03, 0x00 }, // DO_GRIPPER: p1:id,p2:action - { 212, 0x03, 0x03, 0x00 }, // DO_AUTOTUNE_ENABLE: p1:enable,p2:axis - { 214, 0x07, 0x07, 0x00 }, // DO_SET_CAM_TRIGG_INTERVAL: p1:cycle,p2:shutter,p3:camera_id - { 400, 0x03, 0x03, 0x00 }, // COMPONENT_ARM_DISARM: p1:arm,p2:force - { 420, 0x07, 0x07, 0x00 }, // INJECT_FAILURE: p1:unit,p2:type,p3:instance - { 530, 0x03, 0x03, 0x00 }, // SET_CAMERA_MODE: p1:camera_id,p2:mode - { 532, 0x07, 0x07, 0x00 }, // SET_CAMERA_FOCUS: p1:focus_type,p2:value,p3:camera_id - { 534, 0x07, 0x07, 0x00 }, // SET_CAMERA_SOURCE: p1:camera_id,p2:primary,p3:secondary - { 2000, 0x0F, 0x0F, 0x00 }, // IMAGE_START_CAPTURE: p1:camera_id,p2:interval,p3:total,p4:seq - { 2001, 0x01, 0x01, 0x00 }, // IMAGE_STOP_CAPTURE: p1:camera_id - { 2003, 0x0F, 0x0F, 0x00 }, // DO_TRIGGER_CONTROL: p1:enable,p2:reset,p3:pause,p4:camera_id - { 2500, 0x07, 0x07, 0x00 }, // VIDEO_START_CAPTURE: p1:stream_id,p2:status_freq,p3:camera_id - { 2501, 0x03, 0x03, 0x00 }, // VIDEO_STOP_CAPTURE: p1:stream_id,p2:camera_id - { 3000, 0x03, 0x03, 0x00 }, // DO_VTOL_TRANSITION: p1:state,p2:force_immediate - { 4501, 0x00, 0x00, 0x00 }, // CONDITION_GATE: no params used by PX4 - { 5000, 0x00, 0x00, 0x07 }, // NAV_FENCE_RETURN_POINT: p5-7:lat/lon/alt - { 5001, 0x01, 0x01, 0x03 }, // NAV_FENCE_POLYGON_VERTEX_INCLUSION: p1:vertex_count; p5-6:lat/lon - { 5002, 0x01, 0x01, 0x03 }, // NAV_FENCE_POLYGON_VERTEX_EXCLUSION: p1:vertex_count; p5-6:lat/lon - { 5003, 0x01, 0x01, 0x03 }, // NAV_FENCE_CIRCLE_INCLUSION: p1:radius; p5-6:lat/lon - { 5004, 0x01, 0x01, 0x03 }, // NAV_FENCE_CIRCLE_EXCLUSION: p1:radius; p5-6:lat/lon - { 5100, 0x00, 0x00, 0x07 }, // NAV_RALLY_POINT: p5-7:lat/lon/alt - {42600, 0x0F, 0x0F, 0x00 }, // DO_WINCH: p1-p4 all used + // cmd miss cmd m567 c567 + { 16, 0x0B, 0x0B, 0x07, 0x07 }, // NAV_WAYPOINT: p1:hold,p2:accept_r,p4:yaw; p5-7:lat/lon/alt + { 17, 0x0C, 0x0C, 0x07, 0x07 }, // NAV_LOITER_UNLIM: p3:radius,p4:yaw; p5-7:lat/lon/alt + { 19, 0x0F, 0x0F, 0x07, 0x07 }, // NAV_LOITER_TIME: p1-p4 all used; p5-7:lat/lon/alt + { 20, 0x00, 0x00, 0x00, 0x00 }, // NAV_RETURN_TO_LAUNCH: no params + { 21, 0x0A, 0x0A, 0x07, 0x07 }, // NAV_LAND: p2:precision,p4:yaw; p5-7:lat/lon/alt + { 22, 0x08, 0x08, 0x07, 0x07 }, // NAV_TAKEOFF: p4:yaw; p5-7:lat/lon/alt + { 31, 0x0B, 0x0B, 0x07, 0x07 }, // NAV_LOITER_TO_ALT: p1:hdg,p2:radius,p4:xtrack; p5-7:lat/lon/alt + { 80, 0x07, 0x07, 0x07, 0x07 }, // NAV_ROI: p1:mode,p2:wp_idx,p3:roi_idx; p5-7:lat/lon/alt + { 84, 0x08, 0x08, 0x07, 0x07 }, // NAV_VTOL_TAKEOFF: p4:yaw; p5-7:lat/lon/alt + { 85, 0x08, 0x08, 0x07, 0x07 }, // NAV_VTOL_LAND: p4:yaw; p5-7:lat/lon/alt + { 93, 0x0F, 0x0F, 0x00, 0x00 }, // NAV_DELAY: p1:delay,p2:hour,p3:min,p4:sec + { 112, 0x01, 0x01, 0x00, 0x00 }, // CONDITION_DELAY: p1:seconds + { 114, 0x01, 0x01, 0x00, 0x00 }, // CONDITION_DISTANCE: p1:distance + { 176, 0x07, 0x07, 0x00, 0x00 }, // DO_SET_MODE: p1:mode,p2:custom,p3:submode + { 177, 0x03, 0x03, 0x00, 0x00 }, // DO_JUMP: p1:index,p2:count + { 178, 0x07, 0x07, 0x00, 0x00 }, // DO_CHANGE_SPEED: p1:type,p2:speed,p3:throttle + { 179, 0x0F, 0x0F, 0x07, 0x07 }, // DO_SET_HOME: p1:use_current,p2:roll,p3:pitch,p4:yaw; p5-7:lat/lon/alt + { 189, 0x00, 0x00, 0x00, 0x00 }, // DO_LAND_START: no params + { 195, 0x00, 0x01, 0x07, 0x07 }, // DO_SET_ROI_LOCATION: cmd:p1:gimbal; p5-7:lat/lon/alt + { 196, 0x01, 0x01, 0x00, 0x00 }, // DO_SET_ROI_WPNEXT_OFFSET: p1:gimbal_id + { 197, 0x01, 0x01, 0x00, 0x00 }, // DO_SET_ROI_NONE: p1:gimbal_id + { 201, 0x07, 0x07, 0x00, 0x00 }, // DO_SET_ROI: p1:mode,p2:wp_idx,p3:roi_idx + { 206, 0x0F, 0x0F, 0x00, 0x00 }, // DO_SET_CAM_TRIGG_DIST: p1:dist,p2:shutter,p3:trigger,p4:camera_id + { 211, 0x03, 0x03, 0x00, 0x00 }, // DO_GRIPPER: p1:id,p2:action + { 212, 0x03, 0x03, 0x00, 0x00 }, // DO_AUTOTUNE_ENABLE: p1:enable,p2:axis + { 214, 0x07, 0x07, 0x00, 0x00 }, // DO_SET_CAM_TRIGG_INTERVAL: p1:cycle,p2:shutter,p3:camera_id + { 400, 0x03, 0x03, 0x00, 0x00 }, // COMPONENT_ARM_DISARM: p1:arm,p2:force + { 420, 0x07, 0x07, 0x00, 0x00 }, // INJECT_FAILURE: p1:unit,p2:type,p3:instance + { 530, 0x03, 0x03, 0x00, 0x00 }, // SET_CAMERA_MODE: p1:camera_id,p2:mode + { 532, 0x07, 0x07, 0x00, 0x00 }, // SET_CAMERA_FOCUS: p1:focus_type,p2:value,p3:camera_id + { 534, 0x07, 0x07, 0x00, 0x00 }, // SET_CAMERA_SOURCE: p1:camera_id,p2:primary,p3:secondary + { 2000, 0x0F, 0x0F, 0x00, 0x00 }, // IMAGE_START_CAPTURE: p1:camera_id,p2:interval,p3:total,p4:seq + { 2001, 0x01, 0x01, 0x00, 0x00 }, // IMAGE_STOP_CAPTURE: p1:camera_id + { 2003, 0x0F, 0x0F, 0x00, 0x00 }, // DO_TRIGGER_CONTROL: p1:enable,p2:reset,p3:pause,p4:camera_id + { 2500, 0x07, 0x07, 0x00, 0x00 }, // VIDEO_START_CAPTURE: p1:stream_id,p2:status_freq,p3:camera_id + { 2501, 0x03, 0x03, 0x00, 0x00 }, // VIDEO_STOP_CAPTURE: p1:stream_id,p2:camera_id + { 3000, 0x03, 0x03, 0x00, 0x00 }, // DO_VTOL_TRANSITION: p1:state,p2:force_immediate + { 4501, 0x00, 0x00, 0x00, 0x00 }, // CONDITION_GATE: no params used by PX4 + { 5000, 0x00, 0x00, 0x07, 0x00 }, // NAV_FENCE_RETURN_POINT: mission:p5-7:lat/lon/alt; cmd:none + { 5001, 0x01, 0x01, 0x03, 0x00 }, // NAV_FENCE_POLYGON_VERTEX_INCLUSION: p1:vertex_count; mission:p5-6:lat/lon + { 5002, 0x01, 0x01, 0x03, 0x00 }, // NAV_FENCE_POLYGON_VERTEX_EXCLUSION: p1:vertex_count; mission:p5-6:lat/lon + { 5003, 0x01, 0x01, 0x03, 0x00 }, // NAV_FENCE_CIRCLE_INCLUSION: p1:radius; mission:p5-6:lat/lon + { 5004, 0x01, 0x01, 0x03, 0x00 }, // NAV_FENCE_CIRCLE_EXCLUSION: p1:radius; mission:p5-6:lat/lon + { 5100, 0x00, 0x00, 0x07, 0x00 }, // NAV_RALLY_POINT: mission:p5-7:lat/lon/alt; cmd:none + {42600, 0x0F, 0x0F, 0x00, 0x00 }, // DO_WINCH: p1-p4 all used }; static constexpr size_t SupportedCommandParamsCount = sizeof(SupportedCommandParams) / sizeof(SupportedCommandParams[0]); @@ -135,9 +142,7 @@ static constexpr bool _is_sorted() static_assert(_is_sorted(), "mavlink_command_params::SupportedCommandParams must be sorted ascending by cmd"); -// Returns true when the float value means "param not provided" per MAVLink convention. -// Accepts NaN (the standard) and ±0.0 (the common GCS default for unused fields). -// Bit-mask avoids float comparison (-Wfloat-equal) and fpclassify (absent in NuttX ). +// Bit-mask helpers avoid float comparison (-Wfloat-equal) and fpclassify (absent in NuttX). static inline bool param_is_unset(float v) { uint32_t bits; @@ -145,22 +150,34 @@ static inline bool param_is_unset(float v) 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; +} + /** * Check params 1–7 of an incoming mission item or command against the table. * - * Pass 0.0f for p5–p7 to skip checking those params (e.g. for global-frame - * mission items where p5–p7 carry lat/lon/alt). + * Pass 0.0f for any of p5–p7 to skip checking that param (treated as unset). + * For global-frame MISSION_ITEM_INT prefer check_params_int() which validates + * p5/p6 correctly as int32 against INT32_MAX. * - * @param cmd MAV_CMD value - * @param for_mission true for mission items, false for COMMAND_LONG/INT - * @param p1–p7 raw float param values from the MAVLink message - * @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) + * @param cmd MAV_CMD value + * @param for_mission true for mission items, false for COMMAND_LONG/INT + * @param p1–p7 raw float param values from the MAVLink message + * @param zero_sentinel_mask optional out: bitmask of unsupported params where + * the GCS sent 0.0 instead of NaN (bit 0=p1 … bit 6=p7) + * @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(uint16_t cmd, bool for_mission, float p1, float p2, float p3, float p4, - float p5 = 0.0f, float p6 = 0.0f, float p7 = 0.0f) + float p5 = 0.0f, float p6 = 0.0f, float p7 = 0.0f, + uint8_t *zero_sentinel_mask = nullptr) { // Binary search size_t lo = 0; @@ -170,22 +187,89 @@ static inline bool param_is_unset(float v) const size_t mid = lo + (hi - lo) / 2; if (SupportedCommandParams[mid].cmd == cmd) { - const uint8_t mask = for_mission ? SupportedCommandParams[mid].mission : SupportedCommandParams[mid].command; - const uint8_t mask567 = SupportedCommandParams[mid].params567; + const uint8_t mask = for_mission ? SupportedCommandParams[mid].mission : SupportedCommandParams[mid].command; + const uint8_t mask567 = for_mission ? SupportedCommandParams[mid].mission567 : SupportedCommandParams[mid].command567; + + const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; + const uint8_t masks[7] = { + (uint8_t)((mask >> 0) & 1u), (uint8_t)((mask >> 1) & 1u), + (uint8_t)((mask >> 2) & 1u), (uint8_t)((mask >> 3) & 1u), + (uint8_t)((mask567 >> 0) & 1u), (uint8_t)((mask567 >> 1) & 1u), + (uint8_t)((mask567 >> 2) & 1u), + }; + + for (int i = 0; i < 7; ++i) { + if (!masks[i]) { + 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; + + } else if (SupportedCommandParams[mid].cmd < cmd) { + lo = mid + 1; + + } else { + if (mid == 0) { break; } + + hi = mid - 1; + } + } + + return -1; // command not in table — no validation applied +} + +// 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; +} + +/** + * Variant of check_params for global-frame MISSION_ITEM_INT where p5 and p6 + * are raw int32 lat/lon fields. p7 (z/altitude) remains a float. + * + * @param p5_int raw int32 x (latitude in 1e-7 deg, or INT32_MAX if unset) + * @param p6_int raw int32 y (longitude in 1e-7 deg, or INT32_MAX if unset) + * @param p7 float z (altitude); 0.0f to skip + * @return same as check_params + */ +[[maybe_unused]] static int check_params_int(uint16_t cmd, bool for_mission, + 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) +{ + size_t lo = 0; + size_t hi = SupportedCommandParamsCount - 1; - if (!(mask & (1u << 0)) && !param_is_unset(p1)) { return 1; } + while (lo <= hi) { + const size_t mid = lo + (hi - lo) / 2; - if (!(mask & (1u << 1)) && !param_is_unset(p2)) { return 2; } + if (SupportedCommandParams[mid].cmd == cmd) { + const uint8_t mask = for_mission ? SupportedCommandParams[mid].mission : SupportedCommandParams[mid].command; + const uint8_t mask567 = for_mission ? SupportedCommandParams[mid].mission567 : SupportedCommandParams[mid].command567; - if (!(mask & (1u << 2)) && !param_is_unset(p3)) { return 3; } + const float ps14[4] = {p1, p2, p3, p4}; - if (!(mask & (1u << 3)) && !param_is_unset(p4)) { return 4; } + for (int i = 0; i < 4; ++i) { + if (!((mask >> i) & 1u)) { + if (!param_is_unset(ps14[i])) { return i + 1; } - if (!(mask567 & (1u << 0)) && !param_is_unset(p5)) { return 5; } + if (zero_sentinel_mask && param_is_zero(ps14[i])) { + *zero_sentinel_mask |= (uint8_t)(1u << i); + } + } + } - if (!(mask567 & (1u << 1)) && !param_is_unset(p6)) { return 6; } + if (!((mask567 >> 0) & 1u) && !int_param_is_unset(p5_int)) { return 5; } - if (!(mask567 & (1u << 2)) && !param_is_unset(p7)) { return 7; } + if (!((mask567 >> 1) & 1u) && !int_param_is_unset(p6_int)) { return 6; } + + if (!((mask567 >> 2) & 1u) && !param_is_unset(p7)) { return 7; } return 0; @@ -199,7 +283,103 @@ static inline bool param_is_unset(float v) } } - return -1; // command not in table — no validation applied + return -1; +} + +// Vehicle type bitmask for per-vehicle parameter support. +// bit 0 = fixed-wing (FW), bit 1 = multicopter (MC), bit 2 = VTOL. +// 0xFF matches any vehicle type. +enum VehicleType : uint8_t { + VEHICLE_FW = (1u << 0), + VEHICLE_MC = (1u << 1), + VEHICLE_VTOL = (1u << 2), + VEHICLE_ANY = 0xFF, +}; + +// 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. +// Example (not yet active): +// { 22, VEHICLE_FW, 0x04, 0x04, 0x00, 0x00 } // NAV_TAKEOFF: allow p3 (pitch) on FW only +struct VehicleOverride { + uint16_t cmd; + uint8_t vehicle_mask; // bitmask of VehicleType values this entry applies to + uint8_t mission; // additional allowed param bits (1-4) for mission items + uint8_t command; // additional allowed param bits (1-4) for commands + uint8_t mission567; // additional allowed param bits (5-7) for mission items + uint8_t command567; // additional allowed param bits (5-7) for commands +}; + +static constexpr VehicleOverride VehicleParamOverrides[] = { + // Populate as per-vehicle param differences are identified. +}; + +static constexpr size_t VehicleParamOverridesCount = sizeof(VehicleParamOverrides) / sizeof(VehicleParamOverrides[0]); + +/** + * Variant of check_params that applies vehicle-type-specific parameter overrides. + * + * Performs the same binary-search validation as check_params, then OR-extends the + * allowed-param masks with any matching VehicleParamOverrides entry for vehicle_type. + * Use this when the vehicle type is known at the call site; existing callers that do + * not have type information can continue to use check_params unchanged. + * + * @param vehicle_type bitmask of VehicleType (VEHICLE_FW / VEHICLE_MC / VEHICLE_VTOL) + * @return same as check_params + */ +[[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) +{ + 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; + uint8_t mask567 = for_mission ? SupportedCommandParams[mid].mission567 : SupportedCommandParams[mid].command567; + + for (size_t i = 0; i < VehicleParamOverridesCount; ++i) { + if (VehicleParamOverrides[i].cmd == cmd && + (VehicleParamOverrides[i].vehicle_mask & vehicle_type)) { + mask |= for_mission ? VehicleParamOverrides[i].mission : VehicleParamOverrides[i].command; + mask567 |= for_mission ? VehicleParamOverrides[i].mission567 : VehicleParamOverrides[i].command567; + } + } + + const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; + const uint8_t masks[7] = { + (uint8_t)((mask >> 0) & 1u), (uint8_t)((mask >> 1) & 1u), + (uint8_t)((mask >> 2) & 1u), (uint8_t)((mask >> 3) & 1u), + (uint8_t)((mask567 >> 0) & 1u), (uint8_t)((mask567 >> 1) & 1u), + (uint8_t)((mask567 >> 2) & 1u), + }; + + for (int i = 0; i < 7; ++i) { + if (!masks[i]) { + 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; + + } else if (SupportedCommandParams[mid].cmd < cmd) { + lo = mid + 1; + + } else { + if (mid == 0) { break; } + + hi = mid - 1; + } + } + + return -1; } } // namespace mavlink_cmd_params diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index b0a60d053b70..0bf0f982d362 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1443,20 +1443,32 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->altitude_is_relative = true; } - // Reject populated params that this command does not support. - // p5-7 are lat/lon/alt for global-frame items; pass 0.0 to skip checking them. { - const int bad = mavlink_cmd_params::check_params(mavlink_mission_item->command, true, - mavlink_mission_item->param1, mavlink_mission_item->param2, - mavlink_mission_item->param3, mavlink_mission_item->param4); + uint8_t zero_mask = 0; + int bad = -1; - if (bad > 0) { - return MAV_MISSION_INVALID_PARAM1 + (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(mavlink_mission_item->command, true, + 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); - if (bad < 0) { - PX4_DEBUG("MAV_CMD %u not in param validation table", (unsigned)mavlink_mission_item->command); + } else { + bad = mavlink_cmd_params::check_params(mavlink_mission_item->command, true, + 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_WARN("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.h", (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 @@ -1588,22 +1600,19 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * // This is a mission item with no coordinates - // Reject populated params that this command does not support. - // For MAV_FRAME_MISSION items x/y/z are command data, not coordinates. { + uint8_t zero_mask = 0; const int bad = mavlink_cmd_params::check_params(mavlink_mission_item->command, true, 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); + mavlink_mission_item->z, &zero_mask); - if (bad > 0) { - return MAV_MISSION_INVALID_PARAM1 + (bad - 1); - } + if (bad > 0) { return MAV_MISSION_INVALID_PARAM1 + (bad - 1); } - if (bad < 0) { - PX4_DEBUG("MAV_CMD %u not in param validation table", (unsigned)mavlink_mission_item->command); - } + if (bad < 0) { PX4_WARN("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.h", (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; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a61531f64a2d..1f31176247ee 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -672,10 +672,12 @@ 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(cmd_mavlink.command, false, vehicle_command.param1, vehicle_command.param2, vehicle_command.param3, vehicle_command.param4, - vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); + vehicle_command.param5, vehicle_command.param6, vehicle_command.param7, + &zero_mask); if (command_invalid > 0) { acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, @@ -683,9 +685,9 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const return; } - if (command_invalid < 0) { - PX4_DEBUG("MAV_CMD %u not in param validation table", (unsigned)cmd_mavlink.command); - } + if (command_invalid < 0) { PX4_WARN("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.h", (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( diff --git a/test/mavsdk_tests/test_mavlink_param_validation.py b/test/mavsdk_tests/test_mavlink_param_validation.py new file mode 100644 index 000000000000..f972959b67d0 --- /dev/null +++ b/test/mavsdk_tests/test_mavlink_param_validation.py @@ -0,0 +1,288 @@ +#!/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 math +import sys +import time + +try: + from pymavlink import mavutil +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_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, expected) -> bool: + ok = got == expected + _results.append(ok) + status = PASS if ok else FAIL + expected_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}), expected={expected_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) -> "mavutil.mavfile": + mav = mavutil.mavlink_connection(url) + print(f"Waiting for heartbeat on {url} …") + mav.wait_heartbeat(timeout=15) + print(f"Connected: sysid={mav.target_system} compid={mav.target_component}\n") + return mav + + +def _upload_mission(mav, items: list[dict], timeout: float) -> int | None: + """ + 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 msg.type + + return None + + +def _item(seq, cmd, frame, + p1=NAN, p2=NAN, p3=NAN, p4=NAN, + x=INT32_MAX, y=INT32_MAX, z=0.0, + current=0) -> dict: + 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, cmd: int, timeout: float, + p1=0.0, p2=0.0, p3=0.0, p4=0.0, + p5=0.0, p6=0.0, p7=0.0) -> int | None: + """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, + ) + # Drain until we get an ACK for this specific command + 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 msg.result + return None + + +# ── Test cases ──────────────────────────────────────────────────────────────── + +def run_mission_tests(mav, 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) supported; p3 NOT supported. + 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, # p3 not in mask 0x0B + x=LAT, y=LON, z=ALT, current=1), + ], timeout) + _check("NAV_WAYPOINT unsupported param3 set → INVALID_PARAM3", + result, MAV_MISSION_INVALID_PARAM3) + + # --- 3. NAV_RTL with param1 set (mask 0x00 — no params) --- + result = _upload_mission(mav, [ + _item(0, CMD_NAV_RTL, MAV_FRAME_GLOBAL_INT, + p1=1.0, p2=NAN, p3=NAN, p4=NAN, + x=LAT, y=LON, z=ALT, current=1), + ], timeout) + _check("NAV_RTL unsupported param1 set → INVALID_PARAM1", + result, MAV_MISSION_INVALID_PARAM1) + + # --- 4. NAV_RTL with all params NaN (mask 0x00 — should pass) --- + result = _upload_mission(mav, [ + _item(0, CMD_NAV_RTL, MAV_FRAME_GLOBAL_INT, + p1=NAN, p2=NAN, p3=NAN, p4=NAN, + x=LAT, y=LON, z=ALT, 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), # x != 0 → invalid param5 + ], 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 supported) --- + result = _upload_mission(mav, [ + _item(0, CMD_NAV_TAKEOFF, MAV_FRAME_GLOBAL_INT, + p1=NAN, p2=5.0, p3=NAN, p4=NAN, # p2 not in mask 0x08 + x=LAT, y=LON, z=ALT, current=1), + ], timeout) + _check("NAV_TAKEOFF unsupported param2 set → INVALID_PARAM2", + result, MAV_MISSION_INVALID_PARAM2) + + +def run_command_tests(mav, 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 set → 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 set → 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 set → DENIED", + result, MAV_RESULT_DENIED) + + +# ── 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()) From 501524f4d5b5222537179cfb10db87244c4ddb8f Mon Sep 17 00:00:00 2001 From: Himaghna Date: Tue, 9 Jun 2026 19:48:00 -0400 Subject: [PATCH 03/14] fix(mavlink): fix 142-char lines and Python CI type/style errors Signed-off-by: Himaghna --- src/modules/mavlink/mavlink_command_params.h | 10 +- .../test_mavlink_param_validation.py | 250 +++++++++++------- 2 files changed, 160 insertions(+), 100 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_params.h b/src/modules/mavlink/mavlink_command_params.h index e2a2db1c23b5..77fde982c3c8 100644 --- a/src/modules/mavlink/mavlink_command_params.h +++ b/src/modules/mavlink/mavlink_command_params.h @@ -187,8 +187,9 @@ static inline bool param_is_zero(float v) const size_t mid = lo + (hi - lo) / 2; if (SupportedCommandParams[mid].cmd == cmd) { - const uint8_t mask = for_mission ? SupportedCommandParams[mid].mission : SupportedCommandParams[mid].command; - const uint8_t mask567 = for_mission ? SupportedCommandParams[mid].mission567 : SupportedCommandParams[mid].command567; + const Entry &e = SupportedCommandParams[mid]; + const uint8_t mask = for_mission ? e.mission : e.command; + const uint8_t mask567 = for_mission ? e.mission567 : e.command567; const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; const uint8_t masks[7] = { @@ -250,8 +251,9 @@ static inline bool int_param_is_unset(int32_t v) const size_t mid = lo + (hi - lo) / 2; if (SupportedCommandParams[mid].cmd == cmd) { - const uint8_t mask = for_mission ? SupportedCommandParams[mid].mission : SupportedCommandParams[mid].command; - const uint8_t mask567 = for_mission ? SupportedCommandParams[mid].mission567 : SupportedCommandParams[mid].command567; + const Entry &e = SupportedCommandParams[mid]; + const uint8_t mask = for_mission ? e.mission : e.command; + const uint8_t mask567 = for_mission ? e.mission567 : e.command567; const float ps14[4] = {p1, p2, p3, p4}; diff --git a/test/mavsdk_tests/test_mavlink_param_validation.py b/test/mavsdk_tests/test_mavlink_param_validation.py index f972959b67d0..54b3e10d85af 100644 --- a/test/mavsdk_tests/test_mavlink_param_validation.py +++ b/test/mavsdk_tests/test_mavlink_param_validation.py @@ -2,9 +2,10 @@ """ 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. +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] @@ -14,19 +15,19 @@ """ import argparse -import math import sys import time +from typing import Any, Optional try: - from pymavlink import mavutil + from pymavlink import mavutil # type: ignore[import-untyped] except ImportError: print("ERROR: pymavlink not installed. Run: pip install pymavlink") sys.exit(1) -# ── MAVLink enum constants ──────────────────────────────────────────────────── +# MAVLink enum constants -MAV_MISSION_ACCEPTED = 0 +MAV_MISSION_ACCEPTED = 0 MAV_MISSION_INVALID_PARAM1 = 6 MAV_MISSION_INVALID_PARAM2 = 7 MAV_MISSION_INVALID_PARAM3 = 8 @@ -36,38 +37,47 @@ MAV_MISSION_INVALID_PARAM7 = 12 MAV_RESULT_ACCEPTED = 0 -MAV_RESULT_DENIED = 2 +MAV_RESULT_DENIED = 2 -MAV_FRAME_GLOBAL_INT = 5 +MAV_FRAME_GLOBAL_INT = 5 MAV_FRAME_GLOBAL_RELATIVE_ALT = 3 -MAV_FRAME_MISSION = 2 +MAV_FRAME_MISSION = 2 # MAV_CMD values used in tests -CMD_NAV_WAYPOINT = 16 -CMD_NAV_RTL = 20 -CMD_NAV_TAKEOFF = 22 -CMD_NAV_DELAY = 93 +CMD_NAV_WAYPOINT = 16 +CMD_NAV_RTL = 20 +CMD_NAV_TAKEOFF = 22 +CMD_NAV_DELAY = 93 CMD_COMPONENT_ARM_DISARM = 400 NAN = float("nan") INT32_MAX = 2_147_483_647 -# ── Helpers ─────────────────────────────────────────────────────────────────── +# Helpers PASS = "\033[32mPASS\033[0m" FAIL = "\033[31mFAIL\033[0m" _results: list[bool] = [] -def _check(label: str, got, expected) -> bool: - ok = got == expected +def _check(label: str, got: Any, expected: Any) -> bool: + ok = bool(got == expected) _results.append(ok) status = PASS if ok else FAIL - expected_name = _mission_result_name(expected) if isinstance(expected, int) else expected - got_name = _mission_result_name(got) if isinstance(got, int) else got + 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}), expected={expected_name} ({expected})") + print( + f" got={got_name} ({got})," + f" expected={exp_name} ({expected})" + ) return ok @@ -80,21 +90,27 @@ def _mission_result_name(v: int) -> str: return names.get(v, str(v)) -def connect(url: str) -> "mavutil.mavfile": +def connect(url: str) -> Any: mav = mavutil.mavlink_connection(url) - print(f"Waiting for heartbeat on {url} …") + print(f"Waiting for heartbeat on {url} ...") mav.wait_heartbeat(timeout=15) - print(f"Connected: sysid={mav.target_system} compid={mav.target_component}\n") + print( + f"Connected: sysid={mav.target_system}" + f" compid={mav.target_component}\n" + ) return mav -def _upload_mission(mav, items: list[dict], timeout: float) -> int | None: +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. + 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) + mav.target_system, mav.target_component, + len(items), 0, # mission_type=0 (main) ) deadline = time.monotonic() + timeout * (len(items) + 2) @@ -122,15 +138,18 @@ def _upload_mission(mav, items: list[dict], timeout: float) -> int | None: ) elif t == "MISSION_ACK": - return msg.type + return int(msg.type) return None -def _item(seq, cmd, frame, - p1=NAN, p2=NAN, p3=NAN, p4=NAN, - x=INT32_MAX, y=INT32_MAX, z=0.0, - current=0) -> dict: +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, @@ -139,138 +158,177 @@ def _item(seq, cmd, frame, ) -def _send_command(mav, cmd: int, timeout: float, - p1=0.0, p2=0.0, p3=0.0, p4=0.0, - p5=0.0, p6=0.0, p7=0.0) -> int | None: +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, ) - # Drain until we get an ACK for this specific command deadline = time.monotonic() + timeout while time.monotonic() < deadline: - msg = mav.recv_match(type="COMMAND_ACK", blocking=True, timeout=timeout) + msg = mav.recv_match( + type="COMMAND_ACK", blocking=True, timeout=timeout, + ) if msg and msg.command == cmd: - return msg.result + return int(msg.result) return None -# ── Test cases ──────────────────────────────────────────────────────────────── +# Test cases -def run_mission_tests(mav, timeout: float) -> None: + +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 + # 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) supported; p3 NOT supported. + # 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) + _check("Valid NAV_WAYPOINT -> ACCEPTED", result, MAV_MISSION_ACCEPTED) - # --- 2. NAV_WAYPOINT with unsupported param3 set --- + # 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, # p3 not in mask 0x0B + 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 set → INVALID_PARAM3", - result, MAV_MISSION_INVALID_PARAM3) + _check( + "NAV_WAYPOINT unsupported param3 -> INVALID_PARAM3", + result, MAV_MISSION_INVALID_PARAM3, + ) - # --- 3. NAV_RTL with param1 set (mask 0x00 — no params) --- + # 3. NAV_RTL with param1 set (mask 0x00, no params) result = _upload_mission(mav, [ _item(0, CMD_NAV_RTL, MAV_FRAME_GLOBAL_INT, p1=1.0, p2=NAN, p3=NAN, p4=NAN, x=LAT, y=LON, z=ALT, current=1), ], timeout) - _check("NAV_RTL unsupported param1 set → INVALID_PARAM1", - result, MAV_MISSION_INVALID_PARAM1) + _check( + "NAV_RTL unsupported param1 -> INVALID_PARAM1", + result, MAV_MISSION_INVALID_PARAM1, + ) - # --- 4. NAV_RTL with all params NaN (mask 0x00 — should pass) --- + # 4. NAV_RTL with all params NaN (should pass) result = _upload_mission(mav, [ _item(0, CMD_NAV_RTL, MAV_FRAME_GLOBAL_INT, p1=NAN, p2=NAN, p3=NAN, p4=NAN, x=LAT, y=LON, z=ALT, current=1), ], timeout) - _check("NAV_RTL all params NaN → ACCEPTED", result, MAV_MISSION_ACCEPTED) + _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 + # 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), # x != 0 → invalid param5 + x=42, y=0, z=0.0), ], timeout) - _check("NAV_DELAY unsupported param5 (x=42) → INVALID_PARAM5", - result, MAV_MISSION_INVALID_PARAM5) + _check( + "NAV_DELAY unsupported param5 (x=42) -> INVALID_PARAM5", + result, MAV_MISSION_INVALID_PARAM5, + ) - # --- 6. NAV_DELAY valid (p1 set, p5/p6/p7 zero) --- + # 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) + _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 supported) --- + # 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, # p2 not in mask 0x08 + p1=NAN, p2=5.0, p3=NAN, p4=NAN, x=LAT, y=LON, z=ALT, current=1), ], timeout) - _check("NAV_TAKEOFF unsupported param2 set → INVALID_PARAM2", - result, MAV_MISSION_INVALID_PARAM2) + _check( + "NAV_TAKEOFF unsupported param2 -> INVALID_PARAM2", + result, MAV_MISSION_INVALID_PARAM2, + ) -def run_command_tests(mav, timeout: float) -> None: +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) + # 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 set → DENIED", - result, MAV_RESULT_DENIED) + # 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) --- + # 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 set → DENIED", - result, MAV_RESULT_DENIED) + _check( + "NAV_RTL command unsupported param1 -> DENIED", + result, MAV_RESULT_DENIED, + ) - # --- 11. NAV_RTL command all params zero → not 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) + _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, + ) - # --- 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 set → DENIED", - result, MAV_RESULT_DENIED) +# Entry point -# ── 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)") + 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) @@ -279,7 +337,7 @@ def main() -> int: run_command_tests(mav, args.timeout) passed = sum(_results) - total = len(_results) + total = len(_results) print(f"\nResult: {passed}/{total} passed") return 0 if passed == total else 1 From 03a3a1fa931e34303ab6fab62cb9d8eead9377a7 Mon Sep 17 00:00:00 2001 From: Himaghna Date: Tue, 9 Jun 2026 20:28:23 -0400 Subject: [PATCH 04/14] fix(mavlink): pack p5-7 bits into mission/command byte, fix astyle/type-limits/mypy --- src/modules/mavlink/mavlink_command_params.h | 178 ++++++++---------- src/modules/mavlink/mavlink_mission.cpp | 8 +- .../test_mavlink_param_validation.py | 2 +- 3 files changed, 87 insertions(+), 101 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_params.h b/src/modules/mavlink/mavlink_command_params.h index 77fde982c3c8..1d1c60ee61b3 100644 --- a/src/modules/mavlink/mavlink_command_params.h +++ b/src/modules/mavlink/mavlink_command_params.h @@ -38,14 +38,9 @@ * 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. * - * Four masks per entry: - * mission – params 1-4 for mission items (MISSION_ITEM / MISSION_ITEM_INT) - * command – params 1-4 for commands (COMMAND_LONG / COMMAND_INT) - * mission567 – params 5-7 for mission items - * command567 – params 5-7 for commands - * - * mission/command: bit 0 = param1, bit 1 = param2, bit 2 = param3, bit 3 = param4. - * mission567/command567: bit 0 = param5, bit 1 = param6, bit 2 = param7. + * 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() is used so that p5/p6 * are validated as int32 (INT32_MAX = unset) and p7 as float (altitude). @@ -70,62 +65,64 @@ namespace mavlink_cmd_params struct Entry { uint16_t cmd; - uint8_t mission; // supported param bits (1-4) for mission items - uint8_t command; // supported param bits (1-4) for COMMAND_LONG/INT - uint8_t mission567; // supported param bits for params 5-7 in mission items - uint8_t command567; // supported param bits for params 5-7 in commands + 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 and requires no MAVLink includes. +// +// 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 miss cmd m567 c567 - { 16, 0x0B, 0x0B, 0x07, 0x07 }, // NAV_WAYPOINT: p1:hold,p2:accept_r,p4:yaw; p5-7:lat/lon/alt - { 17, 0x0C, 0x0C, 0x07, 0x07 }, // NAV_LOITER_UNLIM: p3:radius,p4:yaw; p5-7:lat/lon/alt - { 19, 0x0F, 0x0F, 0x07, 0x07 }, // NAV_LOITER_TIME: p1-p4 all used; p5-7:lat/lon/alt - { 20, 0x00, 0x00, 0x00, 0x00 }, // NAV_RETURN_TO_LAUNCH: no params - { 21, 0x0A, 0x0A, 0x07, 0x07 }, // NAV_LAND: p2:precision,p4:yaw; p5-7:lat/lon/alt - { 22, 0x08, 0x08, 0x07, 0x07 }, // NAV_TAKEOFF: p4:yaw; p5-7:lat/lon/alt - { 31, 0x0B, 0x0B, 0x07, 0x07 }, // NAV_LOITER_TO_ALT: p1:hdg,p2:radius,p4:xtrack; p5-7:lat/lon/alt - { 80, 0x07, 0x07, 0x07, 0x07 }, // NAV_ROI: p1:mode,p2:wp_idx,p3:roi_idx; p5-7:lat/lon/alt - { 84, 0x08, 0x08, 0x07, 0x07 }, // NAV_VTOL_TAKEOFF: p4:yaw; p5-7:lat/lon/alt - { 85, 0x08, 0x08, 0x07, 0x07 }, // NAV_VTOL_LAND: p4:yaw; p5-7:lat/lon/alt - { 93, 0x0F, 0x0F, 0x00, 0x00 }, // NAV_DELAY: p1:delay,p2:hour,p3:min,p4:sec - { 112, 0x01, 0x01, 0x00, 0x00 }, // CONDITION_DELAY: p1:seconds - { 114, 0x01, 0x01, 0x00, 0x00 }, // CONDITION_DISTANCE: p1:distance - { 176, 0x07, 0x07, 0x00, 0x00 }, // DO_SET_MODE: p1:mode,p2:custom,p3:submode - { 177, 0x03, 0x03, 0x00, 0x00 }, // DO_JUMP: p1:index,p2:count - { 178, 0x07, 0x07, 0x00, 0x00 }, // DO_CHANGE_SPEED: p1:type,p2:speed,p3:throttle - { 179, 0x0F, 0x0F, 0x07, 0x07 }, // DO_SET_HOME: p1:use_current,p2:roll,p3:pitch,p4:yaw; p5-7:lat/lon/alt - { 189, 0x00, 0x00, 0x00, 0x00 }, // DO_LAND_START: no params - { 195, 0x00, 0x01, 0x07, 0x07 }, // DO_SET_ROI_LOCATION: cmd:p1:gimbal; p5-7:lat/lon/alt - { 196, 0x01, 0x01, 0x00, 0x00 }, // DO_SET_ROI_WPNEXT_OFFSET: p1:gimbal_id - { 197, 0x01, 0x01, 0x00, 0x00 }, // DO_SET_ROI_NONE: p1:gimbal_id - { 201, 0x07, 0x07, 0x00, 0x00 }, // DO_SET_ROI: p1:mode,p2:wp_idx,p3:roi_idx - { 206, 0x0F, 0x0F, 0x00, 0x00 }, // DO_SET_CAM_TRIGG_DIST: p1:dist,p2:shutter,p3:trigger,p4:camera_id - { 211, 0x03, 0x03, 0x00, 0x00 }, // DO_GRIPPER: p1:id,p2:action - { 212, 0x03, 0x03, 0x00, 0x00 }, // DO_AUTOTUNE_ENABLE: p1:enable,p2:axis - { 214, 0x07, 0x07, 0x00, 0x00 }, // DO_SET_CAM_TRIGG_INTERVAL: p1:cycle,p2:shutter,p3:camera_id - { 400, 0x03, 0x03, 0x00, 0x00 }, // COMPONENT_ARM_DISARM: p1:arm,p2:force - { 420, 0x07, 0x07, 0x00, 0x00 }, // INJECT_FAILURE: p1:unit,p2:type,p3:instance - { 530, 0x03, 0x03, 0x00, 0x00 }, // SET_CAMERA_MODE: p1:camera_id,p2:mode - { 532, 0x07, 0x07, 0x00, 0x00 }, // SET_CAMERA_FOCUS: p1:focus_type,p2:value,p3:camera_id - { 534, 0x07, 0x07, 0x00, 0x00 }, // SET_CAMERA_SOURCE: p1:camera_id,p2:primary,p3:secondary - { 2000, 0x0F, 0x0F, 0x00, 0x00 }, // IMAGE_START_CAPTURE: p1:camera_id,p2:interval,p3:total,p4:seq - { 2001, 0x01, 0x01, 0x00, 0x00 }, // IMAGE_STOP_CAPTURE: p1:camera_id - { 2003, 0x0F, 0x0F, 0x00, 0x00 }, // DO_TRIGGER_CONTROL: p1:enable,p2:reset,p3:pause,p4:camera_id - { 2500, 0x07, 0x07, 0x00, 0x00 }, // VIDEO_START_CAPTURE: p1:stream_id,p2:status_freq,p3:camera_id - { 2501, 0x03, 0x03, 0x00, 0x00 }, // VIDEO_STOP_CAPTURE: p1:stream_id,p2:camera_id - { 3000, 0x03, 0x03, 0x00, 0x00 }, // DO_VTOL_TRANSITION: p1:state,p2:force_immediate - { 4501, 0x00, 0x00, 0x00, 0x00 }, // CONDITION_GATE: no params used by PX4 - { 5000, 0x00, 0x00, 0x07, 0x00 }, // NAV_FENCE_RETURN_POINT: mission:p5-7:lat/lon/alt; cmd:none - { 5001, 0x01, 0x01, 0x03, 0x00 }, // NAV_FENCE_POLYGON_VERTEX_INCLUSION: p1:vertex_count; mission:p5-6:lat/lon - { 5002, 0x01, 0x01, 0x03, 0x00 }, // NAV_FENCE_POLYGON_VERTEX_EXCLUSION: p1:vertex_count; mission:p5-6:lat/lon - { 5003, 0x01, 0x01, 0x03, 0x00 }, // NAV_FENCE_CIRCLE_INCLUSION: p1:radius; mission:p5-6:lat/lon - { 5004, 0x01, 0x01, 0x03, 0x00 }, // NAV_FENCE_CIRCLE_EXCLUSION: p1:radius; mission:p5-6:lat/lon - { 5100, 0x00, 0x00, 0x07, 0x00 }, // NAV_RALLY_POINT: mission:p5-7:lat/lon/alt; cmd:none - {42600, 0x0F, 0x0F, 0x00, 0x00 }, // DO_WINCH: p1-p4 all used + // 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, 0x7A, 0x7A }, // NAV_LAND: p2:precision,p4:yaw; p5-7:lat/lon/alt + { 22, 0x78, 0x78 }, // NAV_TAKEOFF: p4:yaw; p5-7:lat/lon/alt + { 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, 0x78 }, // NAV_VTOL_TAKEOFF: p4:yaw; p5-7:lat/lon/alt + { 85, 0x78, 0x78 }, // NAV_VTOL_LAND: 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]); @@ -179,7 +176,6 @@ static inline bool param_is_zero(float v) float p5 = 0.0f, float p6 = 0.0f, float p7 = 0.0f, uint8_t *zero_sentinel_mask = nullptr) { - // Binary search size_t lo = 0; size_t hi = SupportedCommandParamsCount - 1; @@ -187,20 +183,14 @@ static inline bool param_is_zero(float v) const size_t mid = lo + (hi - lo) / 2; if (SupportedCommandParams[mid].cmd == cmd) { - const Entry &e = SupportedCommandParams[mid]; - const uint8_t mask = for_mission ? e.mission : e.command; - const uint8_t mask567 = for_mission ? e.mission567 : e.command567; + const uint8_t mask = for_mission + ? SupportedCommandParams[mid].mission + : SupportedCommandParams[mid].command; const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; - const uint8_t masks[7] = { - (uint8_t)((mask >> 0) & 1u), (uint8_t)((mask >> 1) & 1u), - (uint8_t)((mask >> 2) & 1u), (uint8_t)((mask >> 3) & 1u), - (uint8_t)((mask567 >> 0) & 1u), (uint8_t)((mask567 >> 1) & 1u), - (uint8_t)((mask567 >> 2) & 1u), - }; for (int i = 0; i < 7; ++i) { - if (!masks[i]) { + if (!((mask >> i) & 1u)) { if (!param_is_unset(ps[i])) { return i + 1; } if (zero_sentinel_mask && param_is_zero(ps[i])) { @@ -251,9 +241,9 @@ static inline bool int_param_is_unset(int32_t v) const size_t mid = lo + (hi - lo) / 2; if (SupportedCommandParams[mid].cmd == cmd) { - const Entry &e = SupportedCommandParams[mid]; - const uint8_t mask = for_mission ? e.mission : e.command; - const uint8_t mask567 = for_mission ? e.mission567 : e.command567; + const uint8_t mask = for_mission + ? SupportedCommandParams[mid].mission + : SupportedCommandParams[mid].command; const float ps14[4] = {p1, p2, p3, p4}; @@ -267,11 +257,11 @@ static inline bool int_param_is_unset(int32_t v) } } - if (!((mask567 >> 0) & 1u) && !int_param_is_unset(p5_int)) { return 5; } + if (!((mask >> 4) & 1u) && !int_param_is_unset(p5_int)) { return 5; } - if (!((mask567 >> 1) & 1u) && !int_param_is_unset(p6_int)) { return 6; } + if (!((mask >> 5) & 1u) && !int_param_is_unset(p6_int)) { return 6; } - if (!((mask567 >> 2) & 1u) && !param_is_unset(p7)) { return 7; } + if (!((mask >> 6) & 1u) && !param_is_unset(p7)) { return 7; } return 0; @@ -301,14 +291,12 @@ enum VehicleType : uint8_t { // 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. // Example (not yet active): -// { 22, VEHICLE_FW, 0x04, 0x04, 0x00, 0x00 } // NAV_TAKEOFF: allow p3 (pitch) on FW only +// { 22, VEHICLE_FW, 0x04, 0x04 } // NAV_TAKEOFF: allow p3 (pitch) on FW only struct VehicleOverride { uint16_t cmd; uint8_t vehicle_mask; // bitmask of VehicleType values this entry applies to - uint8_t mission; // additional allowed param bits (1-4) for mission items - uint8_t command; // additional allowed param bits (1-4) for commands - uint8_t mission567; // additional allowed param bits (5-7) for mission items - uint8_t command567; // additional allowed param bits (5-7) for commands + uint8_t mission; // additional allowed param bits (bits 0-6 = p1-p7) + uint8_t command; // additional allowed param bits (bits 0-6 = p1-p7) }; static constexpr VehicleOverride VehicleParamOverrides[] = { @@ -321,7 +309,7 @@ static constexpr size_t VehicleParamOverridesCount = sizeof(VehicleParamOverride * Variant of check_params that applies vehicle-type-specific parameter overrides. * * Performs the same binary-search validation as check_params, then OR-extends the - * allowed-param masks with any matching VehicleParamOverrides entry for vehicle_type. + * allowed-param mask with any matching VehicleParamOverrides entry for vehicle_type. * Use this when the vehicle type is known at the call site; existing callers that do * not have type information can continue to use check_params unchanged. * @@ -340,27 +328,25 @@ static constexpr size_t VehicleParamOverridesCount = sizeof(VehicleParamOverride const size_t mid = lo + (hi - lo) / 2; if (SupportedCommandParams[mid].cmd == cmd) { - uint8_t mask = for_mission ? SupportedCommandParams[mid].mission : SupportedCommandParams[mid].command; - uint8_t mask567 = for_mission ? SupportedCommandParams[mid].mission567 : SupportedCommandParams[mid].command567; - - for (size_t i = 0; i < VehicleParamOverridesCount; ++i) { - if (VehicleParamOverrides[i].cmd == cmd && - (VehicleParamOverrides[i].vehicle_mask & vehicle_type)) { - mask |= for_mission ? VehicleParamOverrides[i].mission : VehicleParamOverrides[i].command; - mask567 |= for_mission ? VehicleParamOverrides[i].mission567 : VehicleParamOverrides[i].command567; + uint8_t mask = for_mission + ? SupportedCommandParams[mid].mission + : SupportedCommandParams[mid].command; + + if constexpr(VehicleParamOverridesCount > 0) { + for (size_t i = 0; i < VehicleParamOverridesCount; ++i) { + if (VehicleParamOverrides[i].cmd == cmd && + (VehicleParamOverrides[i].vehicle_mask & vehicle_type)) { + mask |= for_mission + ? VehicleParamOverrides[i].mission + : VehicleParamOverrides[i].command; + } } } const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; - const uint8_t masks[7] = { - (uint8_t)((mask >> 0) & 1u), (uint8_t)((mask >> 1) & 1u), - (uint8_t)((mask >> 2) & 1u), (uint8_t)((mask >> 3) & 1u), - (uint8_t)((mask567 >> 0) & 1u), (uint8_t)((mask567 >> 1) & 1u), - (uint8_t)((mask567 >> 2) & 1u), - }; for (int i = 0; i < 7; ++i) { - if (!masks[i]) { + if (!((mask >> i) & 1u)) { if (!param_is_unset(ps[i])) { return i + 1; } if (zero_sentinel_mask && param_is_zero(ps[i])) { diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 0bf0f982d362..2560665e7a48 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1458,10 +1458,10 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * } else { bad = mavlink_cmd_params::check_params(mavlink_mission_item->command, true, - 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); + 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); } diff --git a/test/mavsdk_tests/test_mavlink_param_validation.py b/test/mavsdk_tests/test_mavlink_param_validation.py index 54b3e10d85af..f9437d0e62ec 100644 --- a/test/mavsdk_tests/test_mavlink_param_validation.py +++ b/test/mavsdk_tests/test_mavlink_param_validation.py @@ -20,7 +20,7 @@ from typing import Any, Optional try: - from pymavlink import mavutil # type: ignore[import-untyped] + from pymavlink import mavutil # type: ignore[import-not-found, import-untyped] except ImportError: print("ERROR: pymavlink not installed. Run: pip install pymavlink") sys.exit(1) From 7b731c5a2a98a4fe3ffb66ad6f228d0fe53fba09 Mon Sep 17 00:00:00 2001 From: Himaghna Date: Tue, 9 Jun 2026 21:06:11 -0400 Subject: [PATCH 05/14] fix(mavlink): range-for over VehicleParamOverrides, fix mypy ignore code --- src/modules/mavlink/mavlink_command_params.h | 13 +++---------- test/mavsdk_tests/test_mavlink_param_validation.py | 2 +- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_params.h b/src/modules/mavlink/mavlink_command_params.h index 1d1c60ee61b3..4a78aedadc69 100644 --- a/src/modules/mavlink/mavlink_command_params.h +++ b/src/modules/mavlink/mavlink_command_params.h @@ -303,8 +303,6 @@ static constexpr VehicleOverride VehicleParamOverrides[] = { // Populate as per-vehicle param differences are identified. }; -static constexpr size_t VehicleParamOverridesCount = sizeof(VehicleParamOverrides) / sizeof(VehicleParamOverrides[0]); - /** * Variant of check_params that applies vehicle-type-specific parameter overrides. * @@ -332,14 +330,9 @@ static constexpr size_t VehicleParamOverridesCount = sizeof(VehicleParamOverride ? SupportedCommandParams[mid].mission : SupportedCommandParams[mid].command; - if constexpr(VehicleParamOverridesCount > 0) { - for (size_t i = 0; i < VehicleParamOverridesCount; ++i) { - if (VehicleParamOverrides[i].cmd == cmd && - (VehicleParamOverrides[i].vehicle_mask & vehicle_type)) { - mask |= for_mission - ? VehicleParamOverrides[i].mission - : VehicleParamOverrides[i].command; - } + for (const auto &ov : VehicleParamOverrides) { + if (ov.cmd == cmd && (ov.vehicle_mask & vehicle_type)) { + mask |= for_mission ? ov.mission : ov.command; } } diff --git a/test/mavsdk_tests/test_mavlink_param_validation.py b/test/mavsdk_tests/test_mavlink_param_validation.py index f9437d0e62ec..19de113f1504 100644 --- a/test/mavsdk_tests/test_mavlink_param_validation.py +++ b/test/mavsdk_tests/test_mavlink_param_validation.py @@ -20,7 +20,7 @@ from typing import Any, Optional try: - from pymavlink import mavutil # type: ignore[import-not-found, import-untyped] + from pymavlink import mavutil # type: ignore[import-not-found] except ImportError: print("ERROR: pymavlink not installed. Run: pip install pymavlink") sys.exit(1) From 7942c65033c4ff7250e3a6ac32f3a699132cd1e8 Mon Sep 17 00:00:00 2001 From: Himaghna Date: Tue, 9 Jun 2026 21:41:50 -0400 Subject: [PATCH 06/14] fix(mavlink): wire check_params_for_vehicle, add int variant, drop non-vehicle check_params Signed-off-by: Himaghna --- src/modules/mavlink/mavlink_command_params.h | 196 +++++++------------ src/modules/mavlink/mavlink_mission.cpp | 24 ++- src/modules/mavlink/mavlink_receiver.cpp | 7 +- 3 files changed, 98 insertions(+), 129 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_params.h b/src/modules/mavlink/mavlink_command_params.h index 4a78aedadc69..54859730ff78 100644 --- a/src/modules/mavlink/mavlink_command_params.h +++ b/src/modules/mavlink/mavlink_command_params.h @@ -155,84 +155,64 @@ static inline bool param_is_zero(float v) return !std::isnan(v) && (bits & 0x7FFFFFFFu) == 0u; } -/** - * Check params 1–7 of an incoming mission item or command against the table. - * - * Pass 0.0f for any of p5–p7 to skip checking that param (treated as unset). - * For global-frame MISSION_ITEM_INT prefer check_params_int() which validates - * p5/p6 correctly as int32 against INT32_MAX. - * - * @param cmd MAV_CMD value - * @param for_mission true for mission items, false for COMMAND_LONG/INT - * @param p1–p7 raw float param values from the MAVLink message - * @param zero_sentinel_mask optional out: bitmask of unsupported params where - * the GCS sent 0.0 instead of NaN (bit 0=p1 … bit 6=p7) - * @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(uint16_t cmd, bool for_mission, - 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) +// INT32_MAX is the MAVLink sentinel for "int32 param not provided". +static inline bool int_param_is_unset(int32_t v) { - 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) { - const uint8_t mask = for_mission - ? SupportedCommandParams[mid].mission - : SupportedCommandParams[mid].command; - - const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; - - for (int i = 0; i < 7; ++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 v == INT32_MAX; +} - return 0; +// Vehicle type bitmask for per-vehicle parameter support. +// bit 0 = fixed-wing (FW), bit 1 = multicopter (MC), bit 2 = VTOL. +// 0xFF matches any vehicle type. +enum VehicleType : uint8_t { + VEHICLE_FW = (1u << 0), + VEHICLE_MC = (1u << 1), + VEHICLE_VTOL = (1u << 2), + VEHICLE_ANY = 0xFF, +}; - } else if (SupportedCommandParams[mid].cmd < cmd) { - lo = mid + 1; +// Maps PX4 vehicle_status_s fields to a VehicleType bitmask. +// vehicle_type: 1 = rotary-wing, 2 = fixed-wing (vehicle_status_s::VEHICLE_TYPE_*). +static inline uint8_t vehicle_type_bitmask(bool is_vtol, uint8_t vehicle_type) +{ + if (is_vtol) { return VEHICLE_VTOL; } - } else { - if (mid == 0) { break; } + if (vehicle_type == 2u) { return VEHICLE_FW; } - hi = mid - 1; - } - } + if (vehicle_type == 1u) { return VEHICLE_MC; } - return -1; // command not in table — no validation applied + return 0; } -// 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; -} +// 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. +// Example (not yet active): +// { 22, VEHICLE_FW, 0x04, 0x04 } // NAV_TAKEOFF: allow p3 (pitch) on FW only +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-6 = p1-p7) + uint8_t command; // additional allowed param bits (bits 0-6 = p1-p7) +}; + +// When adding vehicle-specific param differences, add entries here. +// Example: { 22, VEHICLE_FW, 0x04, 0x04 } // NAV_TAKEOFF: allow p3 (pitch) on FW only +static constexpr VehicleOverride VehicleParamOverrides[] = { +}; /** - * Variant of check_params for global-frame MISSION_ITEM_INT where p5 and p6 - * are raw int32 lat/lon fields. p7 (z/altitude) remains a float. + * 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 p5_int raw int32 x (latitude in 1e-7 deg, or INT32_MAX if unset) - * @param p6_int raw int32 y (longitude in 1e-7 deg, or INT32_MAX if unset) - * @param p7 float z (altitude); 0.0f to skip - * @return same as check_params + * @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_int(uint16_t cmd, bool for_mission, - 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) +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) { size_t lo = 0; size_t hi = SupportedCommandParamsCount - 1; @@ -241,28 +221,28 @@ static inline bool int_param_is_unset(int32_t v) const size_t mid = lo + (hi - lo) / 2; if (SupportedCommandParams[mid].cmd == cmd) { - const uint8_t mask = for_mission - ? SupportedCommandParams[mid].mission - : SupportedCommandParams[mid].command; + uint8_t mask = for_mission + ? SupportedCommandParams[mid].mission + : SupportedCommandParams[mid].command; - const float ps14[4] = {p1, p2, p3, p4}; + for (const auto &ov : VehicleParamOverrides) { + if (ov.cmd == cmd && (ov.vehicle_mask & vehicle_type)) { + mask |= for_mission ? ov.mission : ov.command; + } + } - for (int i = 0; i < 4; ++i) { + const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; + + for (int i = 0; i < 7; ++i) { if (!((mask >> i) & 1u)) { - if (!param_is_unset(ps14[i])) { return i + 1; } + if (!param_is_unset(ps[i])) { return i + 1; } - if (zero_sentinel_mask && param_is_zero(ps14[i])) { + if (zero_sentinel_mask && param_is_zero(ps[i])) { *zero_sentinel_mask |= (uint8_t)(1u << i); } } } - 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; } else if (SupportedCommandParams[mid].cmd < cmd) { @@ -278,46 +258,14 @@ static inline bool int_param_is_unset(int32_t v) return -1; } -// Vehicle type bitmask for per-vehicle parameter support. -// bit 0 = fixed-wing (FW), bit 1 = multicopter (MC), bit 2 = VTOL. -// 0xFF matches any vehicle type. -enum VehicleType : uint8_t { - VEHICLE_FW = (1u << 0), - VEHICLE_MC = (1u << 1), - VEHICLE_VTOL = (1u << 2), - VEHICLE_ANY = 0xFF, -}; - -// 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. -// Example (not yet active): -// { 22, VEHICLE_FW, 0x04, 0x04 } // NAV_TAKEOFF: allow p3 (pitch) on FW only -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-6 = p1-p7) - uint8_t command; // additional allowed param bits (bits 0-6 = p1-p7) -}; - -static constexpr VehicleOverride VehicleParamOverrides[] = { - // Populate as per-vehicle param differences are identified. -}; - /** - * Variant of check_params that applies vehicle-type-specific parameter overrides. - * - * Performs the same binary-search validation as check_params, then OR-extends the - * allowed-param mask with any matching VehicleParamOverrides entry for vehicle_type. - * Use this when the vehicle type is known at the call site; existing callers that do - * not have type information can continue to use check_params unchanged. - * - * @param vehicle_type bitmask of VehicleType (VEHICLE_FW / VEHICLE_MC / VEHICLE_VTOL) - * @return same as check_params + * 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_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) +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) { size_t lo = 0; size_t hi = SupportedCommandParamsCount - 1; @@ -336,18 +284,24 @@ static constexpr VehicleOverride VehicleParamOverrides[] = { } } - const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; + const float ps14[4] = {p1, p2, p3, p4}; - for (int i = 0; i < 7; ++i) { + for (int i = 0; i < 4; ++i) { if (!((mask >> i) & 1u)) { - if (!param_is_unset(ps[i])) { return i + 1; } + if (!param_is_unset(ps14[i])) { return i + 1; } - if (zero_sentinel_mask && param_is_zero(ps[i])) { + if (zero_sentinel_mask && param_is_zero(ps14[i])) { *zero_sentinel_mask |= (uint8_t)(1u << i); } } } + 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; } else if (SupportedCommandParams[mid].cmd < cmd) { diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 2560665e7a48..f9133a6ff5c6 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1444,24 +1444,29 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * } { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + const uint8_t vtype = mavlink_cmd_params::vehicle_type_bitmask(vehicle_status.is_vtol, + vehicle_status.vehicle_type); + 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(mavlink_mission_item->command, true, + bad = mavlink_cmd_params::check_params_int_for_vehicle(mavlink_mission_item->command, true, vtype, 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(mavlink_mission_item->command, true, - 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); + bad = mavlink_cmd_params::check_params_for_vehicle(mavlink_mission_item->command, true, vtype, + 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); } @@ -1601,8 +1606,13 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * // This is a mission item with no coordinates { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + const uint8_t vtype = mavlink_cmd_params::vehicle_type_bitmask(vehicle_status.is_vtol, + vehicle_status.vehicle_type); + uint8_t zero_mask = 0; - const int bad = mavlink_cmd_params::check_params(mavlink_mission_item->command, true, + const int bad = mavlink_cmd_params::check_params_for_vehicle(mavlink_mission_item->command, true, vtype, 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, diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1f31176247ee..4cf8b180281c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -672,8 +672,13 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const return; } + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + const uint8_t vtype = mavlink_cmd_params::vehicle_type_bitmask(vehicle_status.is_vtol, + vehicle_status.vehicle_type); + uint8_t zero_mask = 0; - const int command_invalid = mavlink_cmd_params::check_params(cmd_mavlink.command, false, + const int command_invalid = mavlink_cmd_params::check_params_for_vehicle(cmd_mavlink.command, false, vtype, vehicle_command.param1, vehicle_command.param2, vehicle_command.param3, vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7, From f2dec97ae59fc33d6ac5a04961973ebc52ad9c97 Mon Sep 17 00:00:00 2001 From: Himaghna Date: Tue, 9 Jun 2026 21:48:38 -0400 Subject: [PATCH 07/14] fix(mavlink): suppress clang unused-function on check_params_*_for_vehicle Signed-off-by: Himaghna --- src/modules/mavlink/mavlink_command_params.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_params.h b/src/modules/mavlink/mavlink_command_params.h index 54859730ff78..79c3f80abb2b 100644 --- a/src/modules/mavlink/mavlink_command_params.h +++ b/src/modules/mavlink/mavlink_command_params.h @@ -209,10 +209,10 @@ static constexpr VehicleOverride VehicleParamOverrides[] = { * 1–7 1-based index of the first offending param * -1 command not in table (no validation applied) */ -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) +[[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) { size_t lo = 0; size_t hi = SupportedCommandParamsCount - 1; @@ -262,10 +262,10 @@ static int check_params_for_vehicle(uint16_t cmd, bool for_mission, uint8_t vehi * 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. */ -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) +[[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) { size_t lo = 0; size_t hi = SupportedCommandParamsCount - 1; From ac4398ce45ed15e5b4bbecfd538e153f457b4b64 Mon Sep 17 00:00:00 2001 From: Himaghna Date: Tue, 9 Jun 2026 22:05:53 -0400 Subject: [PATCH 08/14] fix(mavlink): rename local vs to avoid shadow in handle_message_command_both Signed-off-by: Himaghna --- src/modules/mavlink/mavlink_receiver.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4cf8b180281c..3beb49372955 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -672,10 +672,9 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const return; } - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); - const uint8_t vtype = mavlink_cmd_params::vehicle_type_bitmask(vehicle_status.is_vtol, - vehicle_status.vehicle_type); + vehicle_status_s vs{}; + _vehicle_status_sub.copy(&vs); + const uint8_t vtype = mavlink_cmd_params::vehicle_type_bitmask(vs.is_vtol, vs.vehicle_type); uint8_t zero_mask = 0; const int command_invalid = mavlink_cmd_params::check_params_for_vehicle(cmd_mavlink.command, false, vtype, From 05781395f43a78feaf74237cc058a0a7f47a2823 Mon Sep 17 00:00:00 2001 From: Himaghna Date: Wed, 10 Jun 2026 10:33:46 -0400 Subject: [PATCH 09/14] fix(mavlink): cache vehicle_type_bitmask as member, update on vehicle_status change Signed-off-by: Himaghna --- src/modules/mavlink/mavlink_mission.cpp | 18 +++++------------- src/modules/mavlink/mavlink_mission.h | 1 + src/modules/mavlink/mavlink_receiver.cpp | 12 +++++++----- src/modules/mavlink/mavlink_receiver.h | 1 + 4 files changed, 14 insertions(+), 18 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index f9133a6ff5c6..7eabd01987ae 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1444,25 +1444,20 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * } { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); - const uint8_t vtype = mavlink_cmd_params::vehicle_type_bitmask(vehicle_status.is_vtol, - vehicle_status.vehicle_type); - 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, vtype, + 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, vtype, + 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, @@ -1606,13 +1601,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * // This is a mission item with no coordinates { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); - const uint8_t vtype = mavlink_cmd_params::vehicle_type_bitmask(vehicle_status.is_vtol, - vehicle_status.vehicle_type); - uint8_t zero_mask = 0; - const int bad = mavlink_cmd_params::check_params_for_vehicle(mavlink_mission_item->command, true, vtype, + const int 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, @@ -2004,6 +1994,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 3beb49372955..09b8c9b837c6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -672,12 +672,8 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const return; } - vehicle_status_s vs{}; - _vehicle_status_sub.copy(&vs); - const uint8_t vtype = mavlink_cmd_params::vehicle_type_bitmask(vs.is_vtol, vs.vehicle_type); - uint8_t zero_mask = 0; - const int command_invalid = mavlink_cmd_params::check_params_for_vehicle(cmd_mavlink.command, false, vtype, + 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, @@ -3699,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}; From b0fea9f995f35d27b040158620a3bfc40739e71a Mon Sep 17 00:00:00 2001 From: Himaghna Date: Fri, 12 Jun 2026 00:08:56 -0400 Subject: [PATCH 10/14] fix(mavlink): use named VEHICLE_TYPE_* consts, fix masks, drop WARN spam, fix int_mode cast Signed-off-by: Himaghna --- src/modules/mavlink/mavlink_command_params.h | 159 +++++++++---------- src/modules/mavlink/mavlink_mission.cpp | 27 +++- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 95 insertions(+), 93 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_params.h b/src/modules/mavlink/mavlink_command_params.h index 79c3f80abb2b..75535c875b72 100644 --- a/src/modules/mavlink/mavlink_command_params.h +++ b/src/modules/mavlink/mavlink_command_params.h @@ -42,8 +42,8 @@ * 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() is used so that p5/p6 - * are validated as int32 (INT32_MAX = unset) and p7 as float (altitude). + * 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 @@ -59,6 +59,7 @@ #include #include #include +#include namespace mavlink_cmd_params { @@ -71,7 +72,7 @@ struct Entry { // 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 and requires no MAVLink includes. +// 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, @@ -82,12 +83,12 @@ static constexpr Entry SupportedCommandParams[] = { { 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, 0x7A, 0x7A }, // NAV_LAND: p2:precision,p4:yaw; p5-7:lat/lon/alt - { 22, 0x78, 0x78 }, // NAV_TAKEOFF: p4:yaw; p5-7:lat/lon/alt + { 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, 0x78 }, // NAV_VTOL_TAKEOFF: p4:yaw; p5-7:lat/lon/alt - { 85, 0x78, 0x78 }, // NAV_VTOL_LAND: p4:yaw; p5-7:lat/lon/alt + { 84, 0x7C, 0x7C }, // NAV_VTOL_TAKEOFF: p3:approach_hdg,p4:yaw; p5-7:lat/lon/alt + { 85, 0x7F, 0x7F }, // NAV_VTOL_LAND: 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 @@ -162,57 +163,47 @@ static inline bool int_param_is_unset(int32_t v) } // Vehicle type bitmask for per-vehicle parameter support. -// bit 0 = fixed-wing (FW), bit 1 = multicopter (MC), bit 2 = VTOL. +// bit 0 = fixed-wing (FW), bit 1 = multicopter (MC), bit 2 = VTOL, bit 3 = rover. // 0xFF matches any vehicle type. enum VehicleType : uint8_t { - VEHICLE_FW = (1u << 0), - VEHICLE_MC = (1u << 1), - VEHICLE_VTOL = (1u << 2), - VEHICLE_ANY = 0xFF, + VEHICLE_FW = (1u << 0), + VEHICLE_MC = (1u << 1), + VEHICLE_VTOL = (1u << 2), + VEHICLE_ROVER = (1u << 3), + VEHICLE_ANY = 0xFF, }; -// Maps PX4 vehicle_status_s fields to a VehicleType bitmask. -// vehicle_type: 1 = rotary-wing, 2 = fixed-wing (vehicle_status_s::VEHICLE_TYPE_*). +// 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 == 2u) { return VEHICLE_FW; } + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { return VEHICLE_FW; } - if (vehicle_type == 1u) { return VEHICLE_MC; } + 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. -// Example (not yet active): -// { 22, VEHICLE_FW, 0x04, 0x04 } // NAV_TAKEOFF: allow p3 (pitch) on FW only 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-6 = p1-p7) - uint8_t command; // additional allowed param bits (bits 0-6 = p1-p7) + 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. -// Example: { 22, VEHICLE_FW, 0x04, 0x04 } // NAV_TAKEOFF: allow p3 (pitch) on FW only 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 }, }; -/** - * 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) +// 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; @@ -231,19 +222,7 @@ static constexpr VehicleOverride VehicleParamOverrides[] = { } } - const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; - - for (int i = 0; i < 7; ++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; + return mask; } else if (SupportedCommandParams[mid].cmd < cmd) { lo = mid + 1; @@ -259,62 +238,72 @@ static constexpr VehicleOverride VehicleParamOverrides[] = { } /** - * 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. + * 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_int_for_vehicle(uint16_t cmd, bool for_mission, uint8_t vehicle_type, +[[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, - int32_t p5_int, int32_t p6_int, float p7 = 0.0f, + float p5 = 0.0f, float p6 = 0.0f, float p7 = 0.0f, uint8_t *zero_sentinel_mask = nullptr) { - size_t lo = 0; - size_t hi = SupportedCommandParamsCount - 1; + const int mask_result = _find_mask(cmd, for_mission, vehicle_type); - while (lo <= hi) { - const size_t mid = lo + (hi - lo) / 2; + if (mask_result < 0) { return -1; } - 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; - } - } + const uint8_t mask = (uint8_t)mask_result; + const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; - const float ps14[4] = {p1, p2, p3, p4}; + for (int i = 0; i < 7; ++i) { + if (!((mask >> i) & 1u)) { + if (!param_is_unset(ps[i])) { return i + 1; } - for (int i = 0; i < 4; ++i) { - if (!((mask >> i) & 1u)) { - if (!param_is_unset(ps14[i])) { return i + 1; } - - if (zero_sentinel_mask && param_is_zero(ps14[i])) { - *zero_sentinel_mask |= (uint8_t)(1u << i); - } - } + if (zero_sentinel_mask && param_is_zero(ps[i])) { + *zero_sentinel_mask |= (uint8_t)(1u << i); } + } + } - if (!((mask >> 4) & 1u) && !int_param_is_unset(p5_int)) { return 5; } - - if (!((mask >> 5) & 1u) && !int_param_is_unset(p6_int)) { return 6; } + return 0; +} - if (!((mask >> 6) & 1u) && !param_is_unset(p7)) { return 7; } +/** + * 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); - return 0; + if (mask_result < 0) { return -1; } - } else if (SupportedCommandParams[mid].cmd < cmd) { - lo = mid + 1; + const uint8_t mask = (uint8_t)mask_result; + const float ps14[4] = {p1, p2, p3, p4}; - } else { - if (mid == 0) { break; } + for (int i = 0; i < 4; ++i) { + if (!((mask >> i) & 1u)) { + if (!param_is_unset(ps14[i])) { return i + 1; } - hi = mid - 1; + if (zero_sentinel_mask && param_is_zero(ps14[i])) { + *zero_sentinel_mask |= (uint8_t)(1u << i); + } } } - return -1; + 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 7eabd01987ae..468681283517 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1466,7 +1466,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * if (bad > 0) { return MAV_MISSION_INVALID_PARAM1 + (bad - 1); } - if (bad < 0) { PX4_WARN("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.h", (unsigned)mavlink_mission_item->command); } + if (bad < 0) { PX4_DEBUG("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.h", (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); } } @@ -1602,15 +1602,28 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * { uint8_t zero_mask = 0; - const int 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); + 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, + (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_WARN("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.h", (unsigned)mavlink_mission_item->command); } + if (bad < 0) { PX4_DEBUG("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.h", (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); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 09b8c9b837c6..37cad7e8954f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -685,7 +685,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const return; } - if (command_invalid < 0) { PX4_WARN("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.h", (unsigned)cmd_mavlink.command); } + if (command_invalid < 0) { PX4_DEBUG("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.h", (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); } From d00a17e7330fc5f0028d225f4639ac8c32853c6d Mon Sep 17 00:00:00 2001 From: Himaghna Date: Fri, 12 Jun 2026 00:25:22 -0400 Subject: [PATCH 11/14] fix(mavlink): normalize INT32_MAX to 0 for MAV_FRAME_MISSION p5/p6 in int_mode MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit For MAV_FRAME_MISSION items, x/y are generic p5/p6 params (not lat/lon), so GCS tools send 0 for unused params — not INT32_MAX. Using check_params_int_for_vehicle here caused int_param_is_unset(0)=false, which rejected valid DO_LAND_START and similar items with x=y=0. Fix by normalizing INT32_MAX to 0.0f before calling check_params_for_vehicle, so both the MISSION_ITEM_INT sentinel and the conventional float zero are treated as unset. This restores acceptance of zero-param items like DO_LAND_START while still correctly rejecting items that carry non-zero, non-sentinel values in unsupported param slots. Signed-off-by: Himaghna --- src/modules/mavlink/mavlink_mission.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 468681283517..96b11d5f66e7 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1607,11 +1607,15 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * 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, + // x/y are p5/p6 generic params (not lat/lon) for non-position frames. + // Normalize INT32_MAX (MISSION_ITEM_INT "unused" sentinel) to 0.0f so + // both the int sentinel and the conventional float zero are treated as unset. + const float p5 = mavlink_cmd_params::int_param_is_unset(item_int->x) ? 0.0f : (float)item_int->x; + const float p6 = mavlink_cmd_params::int_param_is_unset(item_int->y) ? 0.0f : (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, - item_int->x, item_int->y, - mavlink_mission_item->z, &zero_mask); + 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, From f912eaf0d495b54919b95476efcaee4ac73f9c4f Mon Sep 17 00:00:00 2001 From: Himaghna Date: Sun, 21 Jun 2026 13:56:29 -0400 Subject: [PATCH 12/14] fix(mavlink): address review feedback on VTOL masks, NaN sentinel, dead enum, and RTL test frame --- src/modules/mavlink/mavlink_command_params.h | 6 +- src/modules/mavlink/mavlink_mission.cpp | 8 +- .../test_mavlink_param_validation.py | 96 +++++++++++++++++-- 3 files changed, 94 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_params.h b/src/modules/mavlink/mavlink_command_params.h index 75535c875b72..2a18d1e980d9 100644 --- a/src/modules/mavlink/mavlink_command_params.h +++ b/src/modules/mavlink/mavlink_command_params.h @@ -87,8 +87,8 @@ static constexpr Entry SupportedCommandParams[] = { { 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, 0x7C, 0x7C }, // NAV_VTOL_TAKEOFF: p3:approach_hdg,p4:yaw; p5-7:lat/lon/alt - { 85, 0x7F, 0x7F }, // NAV_VTOL_LAND: p1:options,p2:approach_hdg,p3:loiter_r,p4:yaw; 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 @@ -164,13 +164,11 @@ static inline bool int_param_is_unset(int32_t v) // Vehicle type bitmask for per-vehicle parameter support. // bit 0 = fixed-wing (FW), bit 1 = multicopter (MC), bit 2 = VTOL, bit 3 = rover. -// 0xFF matches any vehicle type. enum VehicleType : uint8_t { VEHICLE_FW = (1u << 0), VEHICLE_MC = (1u << 1), VEHICLE_VTOL = (1u << 2), VEHICLE_ROVER = (1u << 3), - VEHICLE_ANY = 0xFF, }; // Maps vehicle_status_s fields to a VehicleType bitmask. diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 96b11d5f66e7..e2ab16319fcc 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1608,10 +1608,10 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * 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 0.0f so - // both the int sentinel and the conventional float zero are treated as unset. - const float p5 = mavlink_cmd_params::int_param_is_unset(item_int->x) ? 0.0f : (float)item_int->x; - const float p6 = mavlink_cmd_params::int_param_is_unset(item_int->y) ? 0.0f : (float)item_int->y; + // 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, diff --git a/test/mavsdk_tests/test_mavlink_param_validation.py b/test/mavsdk_tests/test_mavlink_param_validation.py index 19de113f1504..eba4aa38d86f 100644 --- a/test/mavsdk_tests/test_mavlink_param_validation.py +++ b/test/mavsdk_tests/test_mavlink_param_validation.py @@ -47,6 +47,8 @@ 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 @@ -211,22 +213,24 @@ def run_mission_tests(mav: Any, timeout: float) -> None: result, MAV_MISSION_INVALID_PARAM3, ) - # 3. NAV_RTL with param1 set (mask 0x00, no params) + # 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_GLOBAL_INT, - p1=1.0, p2=NAN, p3=NAN, p4=NAN, - x=LAT, y=LON, z=ALT, current=1), + _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) + # 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_GLOBAL_INT, - p1=NAN, p2=NAN, p3=NAN, p4=NAN, - x=LAT, y=LON, z=ALT, current=1), + _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", @@ -267,6 +271,52 @@ def run_mission_tests(mav: Any, timeout: float) -> None: 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 ===") @@ -312,6 +362,36 @@ def run_command_tests(mav: Any, timeout: float) -> None: 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 From ffe87129efef89cbbaaa69f17b510009f00a2743 Mon Sep 17 00:00:00 2001 From: Himaghna Date: Fri, 26 Jun 2026 14:14:35 -0400 Subject: [PATCH 13/14] fix(mavlink): move param validation test out of mavsdk_tests, rename header to .hpp --- ...{mavlink_command_params.h => mavlink_command_params.hpp} | 2 +- src/modules/mavlink/mavlink_mission.cpp | 6 +++--- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- test/{mavsdk_tests => }/test_mavlink_param_validation.py | 0 4 files changed, 6 insertions(+), 6 deletions(-) rename src/modules/mavlink/{mavlink_command_params.h => mavlink_command_params.hpp} (99%) rename test/{mavsdk_tests => }/test_mavlink_param_validation.py (100%) diff --git a/src/modules/mavlink/mavlink_command_params.h b/src/modules/mavlink/mavlink_command_params.hpp similarity index 99% rename from src/modules/mavlink/mavlink_command_params.h rename to src/modules/mavlink/mavlink_command_params.hpp index 2a18d1e980d9..e097b3de64a5 100644 --- a/src/modules/mavlink/mavlink_command_params.h +++ b/src/modules/mavlink/mavlink_command_params.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mavlink_command_params.h + * @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) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index e2ab16319fcc..6a1c59a8fe96 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -42,7 +42,7 @@ #include "mavlink_mission.h" #include "mavlink_main.h" -#include "mavlink_command_params.h" +#include "mavlink_command_params.hpp" #include #include @@ -1466,7 +1466,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * 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.h", (unsigned)mavlink_mission_item->command); } + 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); } } @@ -1627,7 +1627,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * 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.h", (unsigned)mavlink_mission_item->command); } + 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); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 37cad7e8954f..0c5a65c81a7b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -58,7 +58,7 @@ #endif #include "mavlink_command_sender.h" -#include "mavlink_command_params.h" +#include "mavlink_command_params.hpp" #include "mavlink_main.h" #include "mavlink_receiver.h" @@ -685,7 +685,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const return; } - if (command_invalid < 0) { PX4_DEBUG("MAV_CMD %u not in param validation table; add entry to mavlink_command_params.h", (unsigned)cmd_mavlink.command); } + 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); } diff --git a/test/mavsdk_tests/test_mavlink_param_validation.py b/test/test_mavlink_param_validation.py similarity index 100% rename from test/mavsdk_tests/test_mavlink_param_validation.py rename to test/test_mavlink_param_validation.py From 6183c80da01d468096009f3c6eb311e860088135 Mon Sep 17 00:00:00 2001 From: Himaghna Date: Fri, 26 Jun 2026 21:11:47 -0400 Subject: [PATCH 14/14] fix(mavlink): dedupe param-scan loop shared by check_params_for_vehicle variants --- .../mavlink/mavlink_command_params.hpp | 41 +++++++++---------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_params.hpp b/src/modules/mavlink/mavlink_command_params.hpp index e097b3de64a5..3e4b0e1effb2 100644 --- a/src/modules/mavlink/mavlink_command_params.hpp +++ b/src/modules/mavlink/mavlink_command_params.hpp @@ -235,6 +235,23 @@ static int _find_mask(uint16_t cmd, bool for_mission, uint8_t vehicle_type) 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. @@ -253,20 +270,8 @@ static int _find_mask(uint16_t cmd, bool for_mission, uint8_t vehicle_type) if (mask_result < 0) { return -1; } - const uint8_t mask = (uint8_t)mask_result; const float ps[7] = {p1, p2, p3, p4, p5, p6, p7}; - - for (int i = 0; i < 7; ++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; + return _scan_params((uint8_t)mask_result, ps, 7, zero_sentinel_mask); } /** @@ -285,15 +290,9 @@ static int _find_mask(uint16_t cmd, bool for_mission, uint8_t vehicle_type) const uint8_t mask = (uint8_t)mask_result; const float ps14[4] = {p1, p2, p3, p4}; - for (int i = 0; i < 4; ++i) { - if (!((mask >> i) & 1u)) { - if (!param_is_unset(ps14[i])) { return i + 1; } + const int bad = _scan_params(mask, ps14, 4, zero_sentinel_mask); - if (zero_sentinel_mask && param_is_zero(ps14[i])) { - *zero_sentinel_mask |= (uint8_t)(1u << i); - } - } - } + if (bad != 0) { return bad; } if (!((mask >> 4) & 1u) && !int_param_is_unset(p5_int)) { return 5; }