diff --git a/docs/cmd_range_checking.md b/docs/cmd_range_checking.md new file mode 100644 index 000000000..5efcdf16a --- /dev/null +++ b/docs/cmd_range_checking.md @@ -0,0 +1,109 @@ +# MAV_CMD parameter range checking (`mav_cmd_helpers.h`) + +MAVLink command parameters (p1–p7) carry numeric values whose valid ranges are defined in the MAVLink XML — for example, a heading param may only accept 0–360°, or a speed param may require a non-negative value. +Validating these ranges at the MAVLink layer lets a vehicle or GCS reject malformed commands an mission items early, before they reach the flight stack, reducing the risk of undefined behaviour caused by out-of-range inputs. + +This PR provides efficient range checking in a generated mavgen_c header `mav_cmd_helpers.h`. +These can be used by any flight stack to check inputs. + +In addition to explicit range checking, it also provides methods for checking that lat/lon values are in valid ranges. + +## Generating the header + +The header is generated using mavgen as normal. + +```sh +python3 generator/mavgen.py \ + --lang C \ + --output /path/to/output \ + message_definitions/v1.0/common.xml +``` + +The `mav_cmd_helpers.h` in this case would be generated to the root of each of the generated dialect folders. +Include the result directly — it is a single self-contained `#pragma once` header with no library dependencies (only ``, ``, and ``). + +## API + +| Function | Description | +| ----------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `check_range(cmd, p1…p7)` | Validate all seven params against XML-defined bounds. Returns `0` (ok, including when the command has no range data), or `1–7` (1-based index of first failing param). Geographic range is **not** checked here. | +| `lat_in_range_int(lat)` | `1` if `lat` (int32 degE7, COMMAND_INT / MISSION_ITEM_INT) is in range for a latitude, else `0`. Call after `coord_invalid_int` to check coordinates. | +| `lat_in_range_float(lat)` | `1` if `lat` (float degrees, COMMAND_LONG) is in range for a latitude, `0` if out of range or NaN. Call after `param_invalid` to check coordinates. | +| `lon_in_range_int(lon)` | `1` if `lon` (int32 degE7) is in range for a longitude, else `0`. | +| `lon_in_range_float(lon)` | `1` if `lon` (float degrees) is in range for a longitude, `0` if out of range or NaN. | +| `has_location(cmd)` | `1` if the command carries lat/lon/alt, `0` if not, `-1` if unknown. | +| `is_destination(cmd)` | `1` if the command is a waypoint destination, `0` if not, `-1` if unknown. | +| `param_invalid(param_val)` | Non-zero if the float `param_val` is an invalid/default value (NaN or ±0.0 by default). Invalid params are skipped by `check_range`. | +| `coord_invalid_int(coord)` | Non-zero if the int32 degE7 `coord` is the `INT32_MAX` "use current position" sentinel. | + +### Using the methods + +The methods are intended to be used in command handlers and during mission upload to reject MAV_CMD with passed values that are out of range. +The `check_range()` method returns `0` if every bounded param is in range or has no range. Only NaN params are treated as "not set" and skipped; a literal `0` and ±Inf *are* range-checked — so a param whose minimum is greater than `0` will reject `0`, and an infinite value fails against any bound on the violated side. This prefers a false positive (which can be relaxed by updating the XML ranges) over letting a bad value through. Otherwise it returns the 1-based index of the first out-of-range param. +The `lat_in_range_*()` and `lon_in_range_*()` helpers can further be used to check that lat/lon values aren't bigger than valid lat/lon ranges. + +This code shows how you might check a mission item. +The result is either 0, or the param number of the first out of range param. + +```c +#include "common/mav_cmd_helpers.h" + +/* Returns the 1-based index of the first out-of-range param, or 0 if all ok. */ +int check_mission_item(const mavlink_mission_item_int_t *item) +{ + // Passing x/y as floats to check_range is fine: location params (x/y) have no + // XML bounds so are never evaluated there; p1-p4/p7 are always float anyway. + // The int32 lat/lon are range-checked separately below with the _int helpers. + int r = check_range(item->command, + item->param1, item->param2, item->param3, item->param4, + (float)item->x, (float)item->y, item->z); + if (r != 0) return r; + + if (has_location(item->command) == 1) { + if (!coord_invalid_int(item->x) && !lat_in_range_int(item->x)) return 5; + if (!coord_invalid_int(item->y) && !lon_in_range_int(item->y)) return 6; + } + + return 0; +} +``` + +For a `COMMAND_LONG` (all params are plain floats): + +```c +/* Returns the 1-based index of the first out-of-range param, or 0 if all ok. */ +int check_command_long(const mavlink_command_long_t *cmd) +{ + int r = check_range(cmd->command, + cmd->param1, cmd->param2, cmd->param3, cmd->param4, + cmd->param5, cmd->param6, cmd->param7); + if (r != 0) return r; + + if (has_location(cmd->command) == 1) { + if (!param_invalid(cmd->param5) && !lat_in_range_float(cmd->param5)) return 5; + if (!param_invalid(cmd->param6) && !lon_in_range_float(cmd->param6)) return 6; + } + + return 0; +} +``` + +For a `COMMAND_INT` (x/y are `int32_t` × 1e7, checked directly with the `_int` helpers): + +```c +/* Returns the 1-based index of the first out-of-range param, or 0 if all ok. */ +int check_command_int(const mavlink_command_int_t *cmd) +{ + int r = check_range(cmd->command, + cmd->param1, cmd->param2, cmd->param3, cmd->param4, + (float)cmd->x, (float)cmd->y, cmd->z); + if (r != 0) return r; + + if (has_location(cmd->command) == 1) { + if (!coord_invalid_int(cmd->x) && !lat_in_range_int(cmd->x)) return 5; + if (!coord_invalid_int(cmd->y) && !lon_in_range_int(cmd->y)) return 6; + } + + return 0; +} +``` diff --git a/generator/mavgen_c.py b/generator/mavgen_c.py index c20646fcb..8d392c931 100644 --- a/generator/mavgen_c.py +++ b/generator/mavgen_c.py @@ -7,7 +7,7 @@ ''' import os -from . import mavparse, mavtemplate +from . import mavparse, mavtemplate, mavgen_c_cmd_helpers t = mavtemplate.MAVTemplate() @@ -766,6 +766,10 @@ def generate(basename, xml_list): for idx in range(len(xml_list)): xml = xml_list[idx] - xml.xml_hash = hash(xml.basename) + xml.xml_hash = hash(xml.basename) generate_one(basename, xml) + directory = os.path.join(basename, xml.basename) + output_path = os.path.join(directory, "mav_cmd_helpers.h") + if mavgen_c_cmd_helpers.generate(xml.filename, output_path): + print("Generated %s" % output_path) copy_fixed_headers(basename, xml_list[0]) diff --git a/generator/mavgen_c_cmd_helpers.py b/generator/mavgen_c_cmd_helpers.py new file mode 100644 index 000000000..aa8270e07 --- /dev/null +++ b/generator/mavgen_c_cmd_helpers.py @@ -0,0 +1,379 @@ +#!/usr/bin/env python3 +"""Generate mav_cmd_helpers.h — per-MAV_CMD range-check tables and helpers. + +Outputs a header with: + - param_bounds[] explicit min/max from XML minValue/maxValue attributes + - cmd_flags_table[] hasLocation and isDestination flags from XML + - Helper functions: is_float_param_invalid, lat_in_range, lon_in_range, + cmd_flags, has_location, is_destination, check_range + +All commands with bounds or flags defined in the XML are included in the tables. +""" + +import argparse +import math +import os +import sys + +# Allow package-relative imports when invoked as a script. +# The grandparent of this file is the directory that contains the +# "pymavlink" package (e.g. the repo root or site-packages parent). +if __name__ == "__main__" and __package__ is None: + _here = os.path.dirname(os.path.abspath(__file__)) # generator/ + _pymavlink = os.path.dirname(_here) # pymavlink/ + _root = os.path.dirname(_pymavlink) # / + if _root not in sys.path: + sys.path.insert(0, _root) + +from pymavlink.generator import mavparse + + +def load_with_includes(xml_path): + """Parse xml_path and all transitively included files, merge enums into root.""" + base = os.path.dirname(os.path.abspath(xml_path)) + root = mavparse.MAVXML(xml_path, wire_protocol_version=mavparse.PROTOCOL_2_0) + all_xml = [root] + seen = {os.path.abspath(xml_path)} + queue = [os.path.join(base, f) for f in root.include] + while queue: + path = os.path.abspath(queue.pop(0)) + if path in seen: + continue + seen.add(path) + try: + x = mavparse.MAVXML(path, wire_protocol_version=mavparse.PROTOCOL_2_0) + all_xml.append(x) + queue += [os.path.join(os.path.dirname(path), f) for f in x.include] + except Exception: + pass + mavparse.merge_enums(all_xml) + return root + + +def float_c(v): + """Format a Python float as a C float literal, using NAN for nan values.""" + if math.isnan(v): + return "(float)NAN" + s = repr(float(v)) + # Ensure there's always a decimal point for clarity + if "." not in s and "e" not in s: + s += ".0" + return s + "f" + + +def generate(xml_path, output_path): + root = load_with_includes(xml_path) + + mav_cmd = next((e for e in root.enum if e.name == "MAV_CMD"), None) + if mav_cmd is None: + # This dialect (and its includes) defines no MAV_CMD enum, so there is + # nothing to range-check. Skip generating the header rather than failing + # the whole mavgen run. + return False + + # Collect all MAV_CMD entries, sorted by cmd value, excluding marker entries: + # the synthetic MAV_CMD_ENUM_END (end_marker) and the *_LAST NOP entries that only + # mark the upper bound of a command group. Neither is a real, sendable command, so + # neither belongs in the validation tables. + def _is_marker(e): + return e.end_marker or e.name.endswith("_LAST") + + entries = sorted((e for e in mav_cmd.entry if not _is_marker(e)), + key=lambda e: int(e.value)) + + # Build param bounds table — only params with at least one bound defined + bounds = [] # list of (cmd, param_1based, lo_float, hi_float) + for entry in entries: + cmd = int(entry.value) + for param in entry.param: + if param.reserved: + continue + lo_s = (param.minValue or "").strip() + hi_s = (param.maxValue or "").strip() + if lo_s or hi_s: + lo = float(lo_s) if lo_s else float("nan") + hi = float(hi_s) if hi_s else float("nan") + bounds.append((cmd, int(param.index), lo, hi)) + + # Build cmd flags table + flags = [] # list of (cmd, flags_byte, short_name) + for entry in entries: + cmd = int(entry.value) + f = 0 + if getattr(entry, "has_location", False): + f |= 1 # CMD_FLAG_HAS_LOCATION + if getattr(entry, "is_destination", False): + f |= 2 # CMD_FLAG_IS_DESTINATION + short = entry.name.replace("MAV_CMD_", "") + flags.append((cmd, f, short)) + + with open(output_path, "w") as out: + _write(out, bounds, flags, os.path.basename(xml_path)) + + return True + + +_HEADER_TOP = """\ +/* AUTO-GENERATED by mavgen (mavgen_c_cmd_helpers.py) from {xml} — do not edit. + * Regenerate with mavgen whenever the MAVLink XML definitions change. + */ +#pragma once + +#include +#include +#include + +#ifdef __cplusplus +extern "C" {{ +#endif + +/** + * @brief Tests if a float command parameter is set to an invalid/default value (NaN or ±0.0). + * + * NaN and ±0.0 are commonly used as "not in use" defaults for float parameters. + * Define MAV_CMD_STRICT_NAN_INVALID to reject ±0.0 and require NaN only. + * + * This is for the float-encoded params (p1–p4, p7, and COMMAND_LONG p5/p6). For the + * int32 (degE7) coordinate fields of COMMAND_INT / MISSION_ITEM_INT, use coord_invalid_int(). + * + * @param param_val Parameter value to test. + * @return Non-zero if param_val is invalid, zero otherwise. + */ +static inline int param_invalid(float param_val) +{{ +\tuint32_t bits; +\tmemcpy(&bits, ¶m_val, sizeof(bits)); +\tif ((bits & 0x7F800000u) == 0x7F800000u) return 1; /* NaN — always invalid */ +#ifndef MAV_CMD_STRICT_NAN_INVALID +\tif (!(bits & 0x7FFFFFFFu)) return 1; /* ±0.0 — invalid by default */ +#endif +\treturn 0; +}} + +/** + * @brief Tests if an int32 (degE7) coordinate is set to the "use current position" sentinel. + * + * COMMAND_INT / MISSION_ITEM_INT use INT32_MAX in the x/y (lat/lon) fields to mean + * "use the current position". + * + * @param coord Coordinate value (latitude or longitude), int32 degrees × 1e7. + * @return Non-zero if coord is the INT32_MAX sentinel, zero otherwise. + */ +static inline int coord_invalid_int(int32_t coord) +{{ +\treturn coord == INT32_MAX; +}} + +/** + * @brief Tests if lat is a valid latitude, COMMAND_INT / MISSION_ITEM_INT encoding (int32 degE7). + * + * @param lat Latitude, int32 degrees × 1e7 (range ±9×10⁸ for ±90°). + * @return 1 if in range, 0 if out of range. + */ +static inline int lat_in_range_int(int32_t lat) +{{ +\treturn lat >= -900000000 && lat <= 900000000; +}} + +/** + * @brief Tests if lat is a valid latitude, COMMAND_LONG encoding (float degrees). + * + * @param lat Latitude in float degrees (range ±90). + * @return 1 if in range, 0 if out of range or NaN. + */ +static inline int lat_in_range_float(float lat) +{{ +\treturn lat >= -90.0f && lat <= 90.0f; +}} + +/** + * @brief Tests if lon is a valid longitude, COMMAND_INT / MISSION_ITEM_INT encoding (int32 degE7). + * + * @param lon Longitude, int32 degrees × 1e7 (range ±1.8×10⁹ for ±180°). + * @return 1 if in range, 0 if out of range. + */ +static inline int lon_in_range_int(int32_t lon) +{{ +\treturn lon >= -1800000000 && lon <= 1800000000; +}} + +/** + * @brief Tests if lon is a valid longitude, COMMAND_LONG encoding (float degrees). + * + * @param lon Longitude in float degrees (range ±180). + * @return 1 if in range, 0 if out of range or NaN. + */ +static inline int lon_in_range_float(float lon) +{{ +\treturn lon >= -180.0f && lon <= 180.0f; +}} + +""" + +_TABLES_MID = """\ + +/* Per-command attribute flags from XML hasLocation / isDestination. */ +struct CmdFlags {{ uint16_t cmd; uint8_t flags; }}; + +#define CMD_FLAG_HAS_LOCATION (1u << 0u) +#define CMD_FLAG_IS_DESTINATION (1u << 1u) + +""" + +_HEADER_BOTTOM = """\ + +/** + * @brief Binary-search lookup for per-command attribute flags. + * + * @param cmd MAV_CMD command ID. + * @return Flags byte (CMD_FLAG_HAS_LOCATION | CMD_FLAG_IS_DESTINATION), or -1 if unknown. + */ +static inline int cmd_flags(uint16_t cmd) +{{ +\tunsigned lo = 0u, hi = cmd_flags_count; +\twhile (lo < hi) {{ +\t\tunsigned mid = lo + (hi - lo) / 2u; +\t\tif (cmd_flags_table[mid].cmd == cmd) return (int)cmd_flags_table[mid].flags; +\t\telse if (cmd_flags_table[mid].cmd < cmd) lo = mid + 1u; +\t\telse hi = mid; +\t}} +\treturn -1; +}} + +/** + * @brief Test if a command carries lat/lon/alt. + * + * @param cmd MAV_CMD command ID. + * @return 1 if the command has location, 0 if not, -1 if unknown. + */ +static inline int has_location(uint16_t cmd) +{{ +\tconst int f = cmd_flags(cmd); +\treturn f < 0 ? -1 : ((f & (int)CMD_FLAG_HAS_LOCATION) != 0); +}} + +/** + * @brief Test if a command is a waypoint destination. + * + * @param cmd MAV_CMD command ID. + * @return 1 if the command is a destination, 0 if not, -1 if unknown. + */ +static inline int is_destination(uint16_t cmd) +{{ +\tconst int f = cmd_flags(cmd); +\treturn f < 0 ? -1 : ((f & (int)CMD_FLAG_IS_DESTINATION) != 0); +}} + +/* Internal: non-zero if v is NaN (exponent all ones, non-zero mantissa). Inf is NOT NaN. + * Bit-manipulation avoids isnan(). Bounds use NaN as the "unbounded" sentinel, so this + * also answers "is this bound set?" — bounds are only ever finite or NaN, never Inf. */ +static inline int _is_nan(float v) +{{ +\tuint32_t bits; +\tmemcpy(&bits, &v, sizeof(bits)); +\treturn (bits & 0x7F800000u) == 0x7F800000u && (bits & 0x007FFFFFu) != 0u; +}} + +/** + * @brief Validate MAVLink command parameters against XML-defined bounds. + * + * Only NaN params are treated as "not set" and skipped. A literal 0 IS range-checked, + * so a param whose XML minimum is greater than 0 will reject 0; likewise ±Inf is checked + * and fails against any bound on the violated side. (We prefer a false positive here — + * ranges can be relaxed later — over letting a bad value through.) + * + * @param cmd MAV_CMD command ID. + * @param p1,p2,p3,p4,p5,p6,p7 Command parameters 1–7. + * @return 0 if every bounded param is in range or unset; otherwise 1–7, the 1-based + * index of the first failing parameter. + */ +static inline int check_range(uint16_t cmd, +\tfloat p1, float p2, float p3, float p4, +\tfloat p5, float p6, float p7) +{{ +\tconst float params[7] = {{p1, p2, p3, p4, p5, p6, p7}}; + +\t/* Binary search: find any entry for this cmd in param_bounds. */ +\tunsigned lo = 0u, hi = param_bounds_count; +\twhile (lo < hi) {{ +\t\tconst unsigned mid = lo + (hi - lo) / 2u; +\t\tif (param_bounds[mid].cmd < cmd) {{ lo = mid + 1u; continue; }} +\t\telse if (param_bounds[mid].cmd > cmd) {{ hi = mid; continue; }} +\t\t/* Found. Back up to first entry for this cmd. */ +\t\tunsigned start = mid; +\t\twhile (start > 0u && param_bounds[start - 1u].cmd == cmd) {{ --start; }} +\t\t/* Walk all entries for this cmd. */ +\t\tfor (unsigned i = start; +\t\t i < param_bounds_count && param_bounds[i].cmd == cmd; ++i) {{ +\t\t\tconst unsigned pidx = (unsigned)param_bounds[i].param - 1u; +\t\t\t/* Defensive: params are 1–7 in valid XML, but guard against a malformed +\t\t\t * or extended table entry indexing past params[7] (pidx wraps if param==0). */ +\t\t\tif (pidx >= 7u) {{ continue; }} +\t\t\tconst float param_val = params[pidx]; +\t\t\t/* Skip only NaN "not set" params; a real 0 and ±Inf ARE range-checked below. */ +\t\t\tif (_is_nan(param_val)) {{ continue; }} +\t\t\tif (!_is_nan(param_bounds[i].lo) && param_val < param_bounds[i].lo) +\t\t\t\t{{ return (int)param_bounds[i].param; }} +\t\t\tif (!_is_nan(param_bounds[i].hi) && param_val > param_bounds[i].hi) +\t\t\t\t{{ return (int)param_bounds[i].param; }} +\t\t}} +\t\tbreak; +\t}} + +\treturn 0; +}} + +#ifdef __cplusplus +}} /* extern "C" */ +#endif +""" + + +def _write(out, bounds, flags, basename): + out.write(_HEADER_TOP.format(xml="%s and its includes" % basename)) + + # --- param_bounds table --- + out.write("/* Explicit param bounds from XML minValue / maxValue.\n" + " * (float)NAN for lo or hi means that side is unbounded. */\n") + out.write("struct ParamBound { uint16_t cmd; uint8_t param; float lo; float hi; };\n\n") + out.write("static const struct ParamBound param_bounds[] = {\n") + for cmd, pidx, lo, hi in bounds: + out.write( + "\t{{ {:5d}, {:d}, {:>14s}, {:>14s} }},\n".format( + cmd, pidx, float_c(lo), float_c(hi) + ) + ) + out.write("};\n") + out.write("static const unsigned param_bounds_count =\n" + "\tsizeof(param_bounds) / sizeof(param_bounds[0]);\n") + + out.write(_TABLES_MID.format()) + + # --- cmd_flags_table --- + out.write("static const struct CmdFlags cmd_flags_table[] = {\n") + for cmd, f, short in flags: + parts = [] + if f & 1: + parts.append("CMD_FLAG_HAS_LOCATION") + if f & 2: + parts.append("CMD_FLAG_IS_DESTINATION") + flag_str = " | ".join(parts) if parts else "0" + out.write("\t{{ {:5d}, {:s} }}, /* {:s} */\n".format(cmd, flag_str, short)) + out.write("};\n") + out.write("static const unsigned cmd_flags_count =\n" + "\tsizeof(cmd_flags_table) / sizeof(cmd_flags_table[0]);\n") + + out.write(_HEADER_BOTTOM.format()) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Generate MAVLink command range-check helper tables" + ) + parser.add_argument("xml", help="Root MAVLink XML dialect file (e.g. common.xml)") + parser.add_argument("--output", required=True, help="Output .h file path") + args = parser.parse_args() + if generate(args.xml, args.output): + print("Generated", args.output) + else: + print("Skipped %s: no MAV_CMD enum in %s" % (args.output, args.xml)) diff --git a/generator/mavparse.py b/generator/mavparse.py index 64356aa8d..14e6732c6 100644 --- a/generator/mavparse.py +++ b/generator/mavparse.py @@ -171,7 +171,7 @@ def set_description(self, description): self.description = description class MAVEnumEntry(object): - def __init__(self, name, value, description='', wip=False, end_marker=False, autovalue=False, origin_file='', origin_line=0, has_location=False): + def __init__(self, name, value, description='', wip=False, end_marker=False, autovalue=False, origin_file='', origin_line=0, has_location=False, is_destination=False): self.name = name self.value = value self.deprecated = None @@ -183,6 +183,7 @@ def __init__(self, name, value, description='', wip=False, end_marker=False, aut self.origin_file = origin_file self.origin_line = origin_line self.has_location = has_location + self.is_destination = is_destination class MAVEnum(object): def __init__(self, name, linenumber, description='', bitmask=False): @@ -308,6 +309,14 @@ def start_element(name, attrs): if type(has_location) != bool: raise MAVParseError("invalid has_location value %s" % has_location) + is_destination = attrs.get('isDestination', False) + if is_destination == 'true': + is_destination = True + elif is_destination == 'false': + is_destination = False + if type(is_destination) != bool: + raise MAVParseError("invalid is_destination value %s" % is_destination) + # check bitmask value if self.enum[-1].bitmask: # values should always be a power of 2. Py3.10 @@ -316,7 +325,7 @@ def start_element(name, attrs): print(f"{attrs['name']} has invalid values (bitmask must have powers of 2)") # append the new entry - self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value, '', False, False, autovalue, self.filename, p.CurrentLineNumber, has_location=has_location)) + self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value, '', False, False, autovalue, self.filename, p.CurrentLineNumber, has_location=has_location, is_destination=is_destination)) elif in_element == "mavlink.enums.enum.entry.wip": self.enum[-1].entry[-1].wip = True elif in_element == "mavlink.enums.enum.entry.param":