diff --git a/docs/Mavlink.md b/docs/Mavlink.md new file mode 100644 index 00000000000..b201daf255c --- /dev/null +++ b/docs/Mavlink.md @@ -0,0 +1,273 @@ +# MAVLink INAV Implementation + +INAV has a selective but broad MAVLink implementation. It is still not a drop-in MAVLink autopilot stack like ArduPilot or PX4, but it now covers much more than simple telemetry: multiport routing, mission upload/download, waypoint reached notifications, guided/navigation control, request-response services, RC override for MAVLink serial RX, high-latency mode, MSP-over-MAVLink tunnel, and native MLRS receiver integration. + +INAV supports up to 4 concurrent MAVLink telemetry ports (`MAX_MAVLINK_PORTS`), one endpoint per serial port configured with `FUNCTION_TELEMETRY_MAVLINK`. + +INAV builds against the checked-in generated `storm32` MAVLink headers/dialect bundle, which includes the native mLRS messages used by the implementation. The STorM32 dialect is a superset of the ArduPilot and common message sets. + +## What INAV currently supports + +- Multiport MAVLink telemetry on up to 4 serial ports, with per-port stream rates, radio type, and high-latency mode. +- Learned route discovery and forwarding between MAVLink ports based on `(sysid, compid) -> ingress port`. +- Stateful mission upload, download, clear, retry, persistence, `MISSION_CURRENT`, and `MISSION_ITEM_REACHED`. +- Guided / GCS-nav control via `MAV_CMD_DO_REPOSITION`, altitude changes, yaw changes, and selected `SET_POSITION_TARGET_*` messages. +- Request-response services including `PING`, `TIMESYNC`, `REQUEST_DATA_STREAM`, message interval control, version discovery, and indexed mode discovery. +- `RC_CHANNELS_OVERRIDE` for MAVLink serial RX setups. +- Per-port `HIGH_LATENCY2` mode. +- MSP-over-MAVLink tunnel using `TUNNEL` payload type `0x8001`. +- Native MLRS receiver integration using `MLRS_RADIO_LINK_STATS`, `MLRS_RADIO_LINK_INFORMATION`, and `MLRS_RADIO_LINK_FLOW_CONTROL`. + +## Important differences and omissions + +- **No MAVLink parameter API**: INAV sends a single stub parameter and otherwise ignores parameter traffic. Configure the aircraft through the INAV Configurator or CLI instead. +- **Selective command support**: INAV implements a useful subset of MAVLink commands and ACKs unsupported commands as `UNSUPPORTED`. +- **Mission handling is partial**: uploads are rejected while armed except for legacy guided waypoint writes, mission frames are validated per command, and MSP mission parity gaps remain. +- **Mode reporting is approximate**: `custom_mode` is mapped to ArduPilot-style modes for compatibility and does not represent every INAV state exactly. +- **Single local component identity**: INAV always originates as `MAV_COMP_ID_AUTOPILOT1`; attached radios, GCSes, and companions are always remote components, never local per-port FC identities. +- **Flow control is per-port and opportunistic**: INAV uses remote TX buffer information from `RADIO_STATUS.txbuf`, or from `MLRS_RADIO_LINK_FLOW_CONTROL.txbuf` on MLRS links. Without flow-control input it falls back to blind 20 ms pacing. +- **Half-duplex etiquette still applies**: on a MAVLink serial RX port configured for `serialrx_halfduplex`, INAV waits one telemetry tick after a received frame before transmitting. + +### Usage guidance + +- If you rely on RC via MAVLink, set `receiver_type = SERIAL` and `serialrx_provider = MAVLINK`. The serial port carrying MAVLink RC override traffic must also be configured for MAVLink telemetry so the MAVLink parser is active on that link. +- If MAVLink RX and telemetry intentionally share one half-duplex wire, enable `serialrx_halfduplex` for that setup. +- Leave `mavlink_version = 2` unless you intentionally need MAVLink1 compatibility. Native MLRS messages, MSP tunnel, high-latency mode, `AUTOPILOT_VERSION`, and `PROTOCOL_VERSION` all depend on MAVLink2 behavior. +- SITL builds explicitly include MAVLink telemetry and MAVLink serial RX so this receiver path can be exercised in simulation. +- To reduce bandwidth, lower stream rates for groups you do not need, or disable them entirely by setting the rate to 0. +- If a GCS or companion needs telemetry on ports 2..4, explicitly request streams (`REQUEST_DATA_STREAM` or `MAV_CMD_SET_MESSAGE_INTERVAL`) because only heartbeat is enabled by default. +- If you depend on directed forwarding between links, ensure each remote endpoint transmits at least one frame early so route learning is populated. + +### Relevant CLI settings + +- `mavlink_sysid` - system ID used in every outbound packet (default 1); most inbound handlers only act on packets targeted to this system ID. +- `mavlink_autopilot_type` - heartbeat autopilot ID (`GENERIC` or `ARDUPILOT`). +- `mavlink_version` - MAVLink version to use (`2` by default, `1` only when forced for compatibility). +- Stream rates (Hz): each group is polled up to 50 Hz; a rate of 0 disables the group. These CLI rates exist for port 1 only; ports 2..4 have no configurable stream-rate settings and are controlled at runtime (see below). + - `mavlink_port1_ext_status_rate` + - `mavlink_port1_rc_chan_rate` + - `mavlink_port1_pos_rate` + - `mavlink_port1_extra1_rate` + - `mavlink_port1_extra2_rate` + - `mavlink_port1_extra3_rate` +- Port 1 uses configured CLI rates (`mavlink_port1_*_rate`). +- Ports 2..4 start with heartbeat only (1 Hz), all other streams disabled. +- `mavlink_port{1-4}_min_txbuffer` - minimum remote TX buffer level before sending when per-port flow-control information is available. +- `mavlink_port{1-4}_radio_type` - selects `GENERIC`, `ELRS`, `SIK`, or `MLRS`. `GENERIC` / `ELRS` / `SIK` use `RADIO_STATUS` interpretation; `MLRS` uses native `MLRS_RADIO_LINK_*` traffic on the RX-sharing MAVLink port. +- `mavlink_port{1-4}_high_latency` - turns on MAVLink `HIGH_LATENCY2` mode on that port. + +## Local identity, compatibility, and advertised capabilities + +- INAV always transmits as MAVLink component `MAV_COMP_ID_AUTOPILOT1`. +- The FC does not create local per-port MAVLink identities. Attached devices are learned as remote components from incoming traffic. +- `AUTOPILOT_VERSION` advertises ArduPilot-compatible flight software version `4.7.0`. +- When MAVLink2 is active, `AUTOPILOT_VERSION` advertises these protocol capabilities: + - `MAV_PROTOCOL_CAPABILITY_MAVLINK2` + - `MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT` + - `MAV_PROTOCOL_CAPABILITY_MISSION_INT` + - `MAV_PROTOCOL_CAPABILITY_COMMAND_INT` + - `MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED` + - `MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT` + +## Datastream groups and defaults + +Default rates (Hz) are shown; adjust with the CLI keys above for port 1. +Ports 2..N use a secondary startup profile (heartbeat at 1 Hz, other streams disabled). + +| Datastream group | Messages | Default rate | +| --- | --- | --- | +| `EXTENDED_STATUS` | `SYS_STATUS` | 2 Hz | +| `RC_CHANNELS` | `RC_CHANNELS_RAW` (v1) / `RC_CHANNELS` (v2) | 1 Hz | +| `POSITION` | `GPS_RAW_INT`, `GLOBAL_POSITION_INT`, `GPS_GLOBAL_ORIGIN` | 2 Hz | +| `EXTRA1` | `ATTITUDE` | 2 Hz | +| `EXTRA2` | `VFR_HUD` | 2 Hz | +| `HEARTBEAT` | `HEARTBEAT` | 1 Hz (independent of stream groups) | +| `EXT_SYS_STATE` | `EXTENDED_SYS_STATE` | 1 Hz (defaults to `mavlink_port1_extra3_rate`) | +| `EXTRA3` | `BATTERY_STATUS`, `SCALED_PRESSURE`, `SYSTEM_TIME`, `STATUSTEXT` (when present and not suppressed as an unchanged repeat) | 1 Hz | + +### Routing, forwarding, and local handling + +- INAV learns routes from incoming traffic as `(sysid, compid) -> ingress port`. +- Broadcast messages are forwarded to all other MAVLink ports except `RADIO_STATUS` and `MLRS_RADIO_LINK_FLOW_CONTROL`, which stay local to the ingress port. +- Targeted messages are forwarded only to ports with a learned route for that target. +- Practical caveat: the first targeted message to a never-seen endpoint may not forward until that endpoint has sent at least one MAVLink frame. +- INAV's local FC identity is always `(mavlink_sysid, MAV_COMP_ID_AUTOPILOT1)`. +- Traffic from the local system ID but a different component ID is treated as a remote component and can be learned into the route table. +- Local/system broadcasts (`target_system = 0` or local system ID with `target_component = 0`) are fanned out to all local ports only for: + - `REQUEST_DATA_STREAM` + - `MAV_CMD_SET_MESSAGE_INTERVAL` + - `MAV_CMD_CONTROL_HIGH_LATENCY` +- Other incoming commands/messages are handled once on the resolved local ingress path, but broadcast-targeted control requests still execute locally. + +### Reconnect and status re-announcement + +INAV tracks each remote peer's presence from its `HEARTBEAT` frames, keyed per `(sysid, compid)` route entry rather than per port. A peer counts as having just (re)connected on a port when INAV sees its first heartbeat, a heartbeat after a gap of more than 5 seconds, or a heartbeat that has moved to a different port (failover). On such a (re)connect INAV: + +- resets that port's `STATUSTEXT` de-spam state, so the next OSD/system message is sent immediately instead of waiting out the repeat interval, and +- re-sends the current arming-disable reason to that port, independent of whether the global arming-disable flags have changed, rate-limited to once per 10 seconds per port. + +If the route table is full when a new peer first appears, reconnect detection for that peer degrades gracefully to the broadcast-only behavior (no per-peer re-announcement). + +## Native MLRS receiver integration + +When `mavlink_port{1-4}_radio_type = MLRS`, INAV uses native receiver-emitted MLRS messages rather than treating `RADIO_STATUS` as the real MLRS data source. + +- INAV expects `MLRS_RADIO_LINK_STATS`, `MLRS_RADIO_LINK_INFORMATION`, and `MLRS_RADIO_LINK_FLOW_CONTROL`. +- These messages are accepted only from `MAV_COMP_ID_TELEMETRY_RADIO`. +- `MLRS_RADIO_LINK_STATS` and `MLRS_RADIO_LINK_INFORMATION` must target the local system. `target_component` may be `0` or `MAV_COMP_ID_AUTOPILOT1`. +- INAV stores MLRS runtime state per ingress MAVLink port and clears that state with the normal port lifecycle. +- Only the ingress port that is also the active MAVLink serial RX port may update the global `rxLinkStatistics`. +- `MLRS_RADIO_LINK_STATS` feeds: + - RC link quality into `uplinkLQ` + - serial link quality into `downlinkLQ` + - RSSI, SNR, and active antenna into the receiver stats model +- `MLRS_RADIO_LINK_INFORMATION` feeds: + - mode and band strings + - TX/RX power + - receive sensitivity metadata +- `MLRS_RADIO_LINK_FLOW_CONTROL` applies only to the ingress port's pacing / TX-buffer runtime state and is not forwarded to other MAVLink ports. +- `RADIO_STATUS` remains the generic / legacy radio path for non-MLRS links (`GENERIC`, `ELRS`, `SIK`) and should not be treated as the real MLRS integration path. + +## Supported outgoing messages + +Messages are organized into MAVLink datastream groups. Each group sends one message per trigger at the configured rate. + +- `SYS_STATUS`: advertises detected sensors (gyro/accel/compass, baro, pitot, GPS, optical flow, rangefinder, RC, blackbox) and whether they are healthy. Includes main loop load, battery voltage/current/percentage, and logging capability. +- `RC_CHANNELS_RAW` (v1) / `RC_CHANNELS` (v2): up to 18 input channels plus RSSI mapped to MAVLink units. +- `GPS_RAW_INT`: GNSS fix quality, HDOP/VDOP, velocity, satellite count, and receiver-native ellipsoid altitude and accuracy extensions when available. +- `GLOBAL_POSITION_INT`: GPS position plus INAV altitude and velocity estimates. +- `GPS_GLOBAL_ORIGIN`: current home position. +- `ATTITUDE`: roll, pitch, yaw, and angular rates. +- `VFR_HUD`: airspeed (if a healthy pitot is available), ground speed, throttle, altitude, and climb rate. +- `HEARTBEAT`: arming state plus ArduPilot-style `custom_mode` mapping from INAV flight modes. +- `EXTENDED_SYS_STATE`: landed-state reporting. +- `BATTERY_STATUS`: per-cell voltages (cells 11-14 in `voltages_ext`), current draw, consumed mAh/Wh, and remaining percentage when available. +- `SCALED_PRESSURE`: baro pressure and temperature data. +- `SYSTEM_TIME`: boot time plus RTC Unix time when the RTC is valid. +- `STATUSTEXT`: pending OSD/system messages, changed arming-disable reasons, and flight-mode transitions. OSD/system messages carry a mapped severity (NOTICE by default, WARNING when the source OSD element is inverted, CRITICAL when it blinks); arming-disable and mode notices are always sent at NOTICE severity. Repeated OSD-derived status text is suppressed per MAVLink port; changed text is sent immediately, while unchanged notice/warning/critical messages are re-announced at progressively shorter severity-based intervals (30 s / 10 s / 5 s). Changed arming-disable reasons are broadcast to every enabled port as `Arming disabled: `. INAV flight-mode transitions are broadcast as notices in the form `Notice: INAV: Entering `, with explicit `Notice: INAV: Entering GCS NAV mode` / `Notice: INAV: Exiting GCS NAV mode` transitions while GCS-assisted navigation engages or ends. See [Reconnect and status re-announcement](#reconnect-and-status-re-announcement) for how these are re-sent when a peer (re)connects. +- `MISSION_CURRENT`: broadcast at 1 Hz with mission count, current item, execution mode, and mission state. +- `MISSION_ITEM_REACHED`: sent when navigation reports a mission waypoint reached and broadcast to all enabled MAVLink ports. +- `AUTOPILOT_VERSION`: on request, advertises ArduPilot-compatible version `4.7.0` and the capabilities listed above (MAVLink2 only). +- `PROTOCOL_VERSION`: on request, reports the configured MAVLink protocol version (MAVLink2 only). +- `MESSAGE_INTERVAL`: reply payload for `MAV_CMD_GET_MESSAGE_INTERVAL`. +- `HOME_POSITION`: on request, when a home fix exists. +- `AVAILABLE_MODES`, `AVAILABLE_MODES_MONITOR`, `CURRENT_MODE`: optional MAVLink Standard Modes microservice messages, available only when built with `USE_MAVLINK_STANDARD_MODES`. + +## Supported incoming messages + +- `HEARTBEAT`: used to detect ADS-B participants when `type` is `MAV_TYPE_ADSB`. +- `PING`: broadcast requests are echoed to the requesting system/component. +- `TIMESYNC`: broadcast or locally targeted requests receive the local boot time in nanoseconds. +- `MISSION_COUNT`: starts an upload transaction. Stored INAV waypoints remain capped at `NAV_MAX_WAYPOINTS`; the upload transaction also allows QGC planned-home and non-storage command items. The owning system/component and ingress port are retained for the transaction. +- `MISSION_ITEM` / `MISSION_ITEM_INT`: stores mission waypoints; rejects unsupported frames / sequence errors. Upload while armed is rejected except legacy guided waypoint writes. +- `MISSION_REQUEST_LIST`, `MISSION_REQUEST`, `MISSION_REQUEST_INT`: stateful mission download with partner checks and one-item retransmission support. +- `MISSION_ACK`: completes an active mission download. +- `MISSION_CLEAR_ALL`: clears the runtime and saved mission. +- `COMMAND_LONG` / `COMMAND_INT`: command transport for supported `MAV_CMD_*` handlers. +- `REQUEST_DATA_STREAM`: legacy stream-rate control per stream group. +- `SET_POSITION_TARGET_GLOBAL_INT`: writes the GCS-guided waypoint when the frame is supported; altitude-only requests are also accepted when X/Y are masked out and GCS navigation is valid. +- `SET_POSITION_TARGET_LOCAL_NED`: accepts altitude-only requests in `MAV_FRAME_LOCAL_OFFSET_NED` when X/Y are zero or ignored and GCS navigation is valid. +- `RC_CHANNELS_OVERRIDE`: passes channel values to the MAVLink serial receiver backend. +- `MLRS_RADIO_LINK_STATS`: native MLRS per-port link stats from `MAV_COMP_ID_TELEMETRY_RADIO`, with local target-system / target-component checks. +- `MLRS_RADIO_LINK_INFORMATION`: native MLRS per-port link metadata from `MAV_COMP_ID_TELEMETRY_RADIO`, with local target-system / target-component checks. +- `MLRS_RADIO_LINK_FLOW_CONTROL`: native MLRS per-port flow-control input from `MAV_COMP_ID_TELEMETRY_RADIO`; applied only to the ingress port runtime. +- `RADIO_STATUS`: updates remote TX buffer level and generic / legacy link stats according to `mavlink_port{1-4}_radio_type`. This remains the non-MLRS radio path. +- `ADSB_VEHICLE`: populates the internal traffic list when ADS-B is enabled. +- `PARAM_REQUEST_LIST`: elicits a stub `PARAM_VALUE` response so ground stations stop requesting parameters. +- `TUNNEL`: accepts private payload type `0x8001` for MSP-over-MAVLink on MAVLink2 links. + +## Supported commands + +INAV implements a selective but useful subset of the MAVLink Command protocol. Unsupported commands are ACKed as `UNSUPPORTED`. + +- `MAV_CMD_COMPONENT_ARM_DISARM`: arms through the normal INAV arming checks or disarms with `DISARM_SWITCH`. The ACK is accepted only when the requested armed state is reached. +- `MAV_CMD_NAV_RETURN_TO_LAUNCH`: enters the normal INAV RTH mode path while armed by adding a temporary `BOXNAVRTH` mode source to the normal RC mode selector. RTH altitude, safehome, landing, and fixed-wing autoland behavior remain controlled by the existing INAV configuration. This does not set the failsafe/geozone forced-RTH latch; a later pilot RC flight-mode change or disarm clears the temporary source. +- `MAV_CMD_DO_SET_MODE`: supports the ArduPilot-style RTL/RTH custom-mode request and routes it to the same normal RTH mode path as `MAV_CMD_NAV_RETURN_TO_LAUNCH`. It also accepts the ArduPilot Loiter/PosHold pause modes (`PLANE_MODE_LOITER`, `COPTER_MODE_LOITER`, `COPTER_MODE_POSHOLD`, `COPTER_MODE_BRAKE`) and enters normal INAV PosHold at the current position and altitude. Other mode changes remain unsupported. +- `MAV_CMD_NAV_LAND`: while armed with usable navigation estimates, creates a transient LAND waypoint at the current position and enters the same normal landing path used by a mission `NAV_WP_ACTION_LAND`. It does not use emergency landing and does not modify the uploaded mission. Command location fields are ignored; mission items retain their supplied landing position. +- `MAV_CMD_DO_SET_HOME`: writes the existing INAV waypoint `0` home through `setWaypoint(0, ...)`. `param1 = 1` uses the current GNSS position; `param1 = 0` uses the supplied global location. The existing WP#0 gates still apply: armed state, usable position estimate, valid GPS origin, and GCS-assisted navigation enabled. +- `MAV_CMD_DO_REPOSITION`: sets the Follow Me / GCS-nav waypoint when GCS nav is valid. Accepts `MAV_FRAME_GLOBAL`, `MAV_FRAME_GLOBAL_INT`, `MAV_FRAME_GLOBAL_RELATIVE_ALT`, and `MAV_FRAME_GLOBAL_RELATIVE_ALT_INT`; otherwise `UNSUPPORTED`. `param3` is the optional fixed-wing PosHold loiter-radius override in meters; `0` clears the temporary override back to `nav_fw_loiter_radius`, and `NaN` leaves it unchanged. `param4`, when finite and in the range `0`–`360`, sets a target heading for the reposition point; other values leave the heading unset. +- `MAV_CMD_DO_CHANGE_ALTITUDE`: changes the current altitude target. `param1` is the target altitude in meters and `param2` is interpreted as the MAVLink frame (`MAV_FRAME_GLOBAL`, `MAV_FRAME_GLOBAL_INT`, `MAV_FRAME_GLOBAL_RELATIVE_ALT`, `MAV_FRAME_GLOBAL_RELATIVE_ALT_INT`); unsupported frames are rejected. +- `MAV_CMD_CONDITION_YAW`: changes the current heading target when the active navigation state has yaw control. Accepts absolute heading (`param4 = 0`) and relative turns (`param4 != 0`); turn rate is ignored. +- `MAV_CMD_SET_MESSAGE_INTERVAL` / `MAV_CMD_GET_MESSAGE_INTERVAL`: adjust or query per-message periodic output for `HEARTBEAT`, `SYS_STATUS`, `EXTENDED_SYS_STATE`, RC channels, `GPS_RAW_INT`, `GLOBAL_POSITION_INT`, `GPS_GLOBAL_ORIGIN`, `ATTITUDE`, `VFR_HUD`, `BATTERY_STATUS`, `SCALED_PRESSURE`, and `SYSTEM_TIME`. `REQUEST_DATA_STREAM` still controls the legacy base stream groups; `SET_MESSAGE_INTERVAL` overrides individual messages on top. +- `MAV_CMD_GET_HOME_POSITION`: replies with `HOME_POSITION` when a home fix exists. +- `MAV_CMD_REQUEST_MESSAGE`: emits one selected message or `UNSUPPORTED`. `MESSAGE_INTERVAL` uses `param2` as the queried message ID. When built with `USE_MAVLINK_STANDARD_MODES`, `AVAILABLE_MODES` uses `param2` as its one-based index and `AVAILABLE_MODES_MONITOR` is also supported. +- `MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES`: returns `AUTOPILOT_VERSION` (MAVLink2 only; MAVLink1 returns `UNSUPPORTED`) advertising ArduPilot-compatible version `4.7.0` and the capabilities listed above. +- `MAV_CMD_REQUEST_PROTOCOL_VERSION`: returns `PROTOCOL_VERSION` (MAVLink2 only; MAVLink1 returns `UNSUPPORTED`). +- `MAV_CMD_CONTROL_HIGH_LATENCY`: enables or disables `HIGH_LATENCY2` scheduling on the ingress MAVLink port (`param1 = 0` or `1`). Enabling is rejected on MAVLink1 links. + +`MAV_CMD_NAV_TAKEOFF` has an explicit command stub and currently returns `UNSUPPORTED`. + +## Mode mappings (INAV -> MAVLink / ArduPilot) + +`custom_mode` is derived from active INAV telemetry flight mode (`getFlightModeForTelemetry()`), then mapped per vehicle type. +The default ArduPilot-compatible path reports modes through `HEARTBEAT.custom_mode`. The MAVLink Standard Modes microservice is separate and disabled unless the firmware is built with `USE_MAVLINK_STANDARD_MODES`; if enabled, GUIDED is reported as a named non-standard mode because MAVLink common standard modes do not define a GUIDED enum. + +- **Multirotor profiles** + - ACRO / ACRO AIR -> **ACRO** + - ANGLE / HORIZON / ANGLE HOLD -> **STABILIZE** + - ALT HOLD -> **ALT_HOLD** + - POS HOLD -> **GUIDED** (if GCS valid), otherwise **POSHOLD** + - RTH -> **RTL** + - MISSION -> **AUTO** + - LAUNCH -> **THROW** + - FAILSAFE -> **RTL** (RTH / other phases) or **LAND** (landing phase) + - Any other unmapped mode falls back to **STABILIZE** +- **Fixed-wing profiles** + - MANUAL -> **MANUAL** + - ACRO / ACRO AIR -> **ACRO** + - ANGLE -> **FBWA** + - HORIZON / ANGLE HOLD -> **STABILIZE** + - ALT HOLD -> **FBWB** + - POS HOLD -> **GUIDED** (if GCS valid), otherwise **LOITER** + - RTH -> **RTL** + - MISSION -> **AUTO** + - CRUISE -> **CRUISE** + - LAUNCH -> **TAKEOFF** + - FAILSAFE -> **RTL** (RTH / other phases) or **AUTOLAND** (landing phase) + - Any other unmapped mode falls back to **MANUAL** + +## MAVLink missions + +INAV supports MAVLink mission upload, download, clear, live mission-state reporting, and waypoint-reached notifications. Uploads retry the outstanding request every 1.5 seconds and abort after five unsuccessful retries. Downloads time out after five seconds of inactivity. Only the system/component and ingress port that started a transfer may continue it. Completed uploads and clears update nonvolatile waypoint storage on targets that provide it. Mission downloads always reply with `MISSION_ITEM_INT`, including in response to a legacy float `MISSION_REQUEST`. + +Mission upload is staged before it touches the live INAV waypoint list. The MAVLink stream is translated into a temporary INAV mission, validated, and committed only after the full upload succeeds, so rejected uploads do not leave a half-written mission in the FC. QGC planned home item `0` is skipped because INAV stores home separately; MAVLink sequence `1` becomes INAV waypoint `1`. + +The upload translator handles MAVLink mission items that are modifiers rather than standalone INAV waypoints. `MAV_CMD_NAV_WAYPOINT` stores a normal waypoint, or `POSHOLD_TIME` when hold time is set. `MAV_CMD_NAV_LOITER_TIME` stores `POSHOLD_TIME`; `MAV_CMD_NAV_LAND` stores `LAND`; `MAV_CMD_DO_JUMP` stores `JUMP` with its target remapped after translation. `MAV_CMD_NAV_RETURN_TO_LAUNCH` stores `RTH` (with the land-after-RTH flag and, for coordinate frames, an altitude); `MAV_CMD_DO_SET_ROI` (with `MAV_ROI_LOCATION`) stores `SET_POI`; `MAV_CMD_CONDITION_YAW` stores `SET_HEAD`. `MAV_CMD_DO_CHANGE_SPEED` sets a pending leg speed for following geographic waypoints. `MAV_CMD_CONDITION_DELAY`, `MAV_CMD_CONDITION_CHANGE_ALT`, and `MAV_CMD_DO_CHANGE_ALTITUDE` modify the previous applicable geographic waypoint instead of consuming an INAV waypoint slot. + +The implementation works with common MAVLink mission planners such as QGC for simple mission flows. However, the differences between MAVLink missions and INAV's fuller MSP navigation model mean MAVLink still cannot represent every INAV mission feature. Use MultiWii Planner or the INAV Configurator when you need full MSP mission semantics. + +Warning: a mission downloaded over MAVLink is a best-effort reconstruction from INAV's stored MSP waypoint list, not a reliable canonical mission source. Downloaded items can differ from the original uploaded plan in item count, sequence numbers, command shape, modifier placement, and MSP-specific fields. Keep the original mission plan outside the FC and verify important missions through MSP / INAV-native tooling. + +## MSP mission parity gaps (MAV <-> MSP) + +- WAYPOINT: MSP->MAV sends lat/lon/alt but drops leg speed `p1` and all user-action bits in `p3` (only alt-mode bit drives frame). MAV->MSP stores lat/lon/alt, can apply pending leg speed into `p1`, and keeps only alt-mode bit in `p3`; user bits are lost. +- POSHOLD_TIME / LOITER_TIME: loiter time `p1` OK; pending leg speed can be stored in `p2` on upload. User-action bits in `p3` are discarded both directions. +- LAND: lat/lon/alt OK. As with WAYPOINT, a pending `DO_CHANGE_SPEED` leg speed can be written into `p1` on upload, but leg speed is dropped on download. Ground elevation `p2` and user-action bits in `p3` are cleared in both directions (only the alt-mode bit is retained from frame on upload). +- RTH: the land-after-RTH flag in `p1` is captured on upload (`p1 = 1` when `MAV_CMD_NAV_RETURN_TO_LAUNCH param1 > 0`) but dropped on download (always sent as `0`), so it does not survive a round trip; user-action bits dropped; alt is sent only if the MAVLink frame is a coordinate frame, and returns with the alt-mode bit set on upload. +- JUMP: target and repeat count OK. +- SET_POI: lat/lon/alt OK; `param1` is fixed to `MAV_ROI_LOCATION`; user-action bits in `p3` are dropped (alt-mode bit respected on upload). +- SET_HEAD: heading `p1` OK; user-action bits in `p3` are not represented. +- Net effect: actions and positions OK, but MSP-specific fields (leg speed, LAND elevation adjustment, RTH land flag, user-action bits in `p3`) do not survive a MAV -> MSP -> MAV round trip — some are captured on upload, but downloads drop them — so MAVLink missions cannot fully represent INAV's native MSP waypoint model (see the `wp` field definitions in [Navigation.md](Navigation.md#cli-command-wp-to-manage-waypoints)). + +## MSP over MAVLink tunnel + +This feature uses the MAVLink [Tunnel service](https://mavlink.io/en/services/tunnel.html) to let the INAV Configurator use MSP over an existing MAVLink telemetry link, typically a radio link where there is no separate wireless MSP device. +**It is not intended as a general-purpose serial tunnel, and it is not a replacement for normal MAVLink control / telemetry traffic.** +CLI mode is unavailable in MSP-over-MAVLink. + +- INAV accepts `TUNNEL` messages with private payload type `0x8001` as an MSP byte stream carried over MAVLink2. +- `target_system` must match `mavlink_sysid`. +- `target_component` may be `0` or `MAV_COMP_ID_AUTOPILOT1`. +- `target_component = 0` is handled on the ingress MAVLink port only; it is not fanned out to other local MAVLink ports. +- MSP replies are sent back to the requester as one or more `TUNNEL` messages on that same ingress port. +- MSP framing is preserved end-to-end: MSPv1 requests get MSPv1 replies, and MSPv2 requests get MSPv2 replies. +- Reboot (`MSP_REBOOT`) is supported over the tunnel. Serial passthrough and ESC 4way passthrough are rejected before execution. + +## High latency mode + +High-latency mode uses the MAVLink [High Latency service](https://mavlink.io/en/services/high_latency.html) to replace normal scheduled telemetry on one port with periodic `HIGH_LATENCY2` summaries for very low-bandwidth or intermittent links. + +- High latency mode is per-port, controlled by `mavlink_port{1-4}_high_latency` or by `MAV_CMD_CONTROL_HIGH_LATENCY` received on that port. +- It requires MAVLink2. MAVLink1 cannot enable or carry `HIGH_LATENCY2`. +- When enabled on a port, normal stream scheduling on that port is replaced by `HIGH_LATENCY2` at 5 second intervals. +- This is intended for slow and high-latency telemetry such as cellular, satellite, or LoRa, not for normal rich telemetry, mission planning, or configurator use. diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index a6b546c54cf..a81a7fbadc7 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -119,9 +119,12 @@ for complete documentation on using JavaScript to program your flight controller | 54 | Mag calibration | Trigger a magnetometer calibration. | | 55 | Set Gimbal Sensitivity | Scales `Operand A` from [`-16` : `15`] | 56 | Override Minimum Ground Speed | When active, sets the minimum ground speed to the value specified in `Operand A` [m/s]. Minimum allowed value is set in `nav_min_ground_speed`. Maximum value is `150` | +| 57 | Set Altitude Target | Sets the navigation altitude target. `Operand A` selects the altitude datum and `Operand B` supplies the target altitude in centimetres. | | 58 | Trigonometry: ACos | Computes ACOS of (`Operand A` / `Operand B`) using the fast approximation. If `Operand B` is `0`, `1000` is used. Input is clamped to [-1, 1] and the result is returned in degrees. | | 59 | Trigonometry: ASin | Computes ASIN of (`Operand A` / `Operand B`) using the fast approximation. If `Operand B` is `0`, `1000` is used. Input is clamped to [-1, 1] and the result is returned in degrees. | | 60 | Trigonometry: ATan2 | Computes ATAN2 using `Operand A` as Y and `Operand B` as X with the fast approximation. Returns a signed angle in degrees in `(-180, 180]`. | +| 62 | Activate RTH | While armed, activates normal return-to-home mode through the same mode selector path as RC RTH. Returns true when RTH mode is accepted. | +| 63 | Activate Landing | While armed with usable navigation estimates, commands a normal landing at the current position through the waypoint LAND path. This is not emergency landing. | ### Operands diff --git a/docs/Settings.md b/docs/Settings.md index 243314fc6d8..ded7825f65c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2905,9 +2905,9 @@ Autopilot type to advertise for MAVLink telemetry --- -### mavlink_ext_status_rate +### mavlink_port1_ext_status_rate -Rate of the extended status message for MAVLink telemetry +Rate of the extended status message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2915,9 +2915,9 @@ Rate of the extended status message for MAVLink telemetry --- -### mavlink_extra1_rate +### mavlink_port1_extra1_rate -Rate of the extra1 message for MAVLink telemetry +Rate of the extra1 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2925,9 +2925,9 @@ Rate of the extra1 message for MAVLink telemetry --- -### mavlink_extra2_rate +### mavlink_port1_extra2_rate -Rate of the extra2 message for MAVLink telemetry +Rate of the extra2 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2935,9 +2935,9 @@ Rate of the extra2 message for MAVLink telemetry --- -### mavlink_extra3_rate +### mavlink_port1_extra3_rate -Rate of the extra3 message for MAVLink telemetry +Rate of the extra3 message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2945,9 +2945,19 @@ Rate of the extra3 message for MAVLink telemetry --- -### mavlink_min_txbuffer +### mavlink_port1_high_latency -Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits. +Enable MAVLink high-latency mode on port 1 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port1_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 1. Requires RADIO_STATUS messages. | Default | Min | Max | | --- | --- | --- | @@ -2955,9 +2965,9 @@ Minimum percent of TX buffer space free, before attempting to transmit telemetry --- -### mavlink_pos_rate +### mavlink_port1_pos_rate -Rate of the position message for MAVLink telemetry +Rate of the position message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2965,21 +2975,22 @@ Rate of the position message for MAVLink telemetry --- -### mavlink_radio_type +### mavlink_port1_radio_type -Mavlink radio type. Affects how RSSI and LQ are reported on OSD. +MAVLink radio type for port 1. Affects RSSI and LQ reporting on OSD. | Allowed Values | | | --- | --- | | GENERIC | Default | | ELRS | | | SIK | | +| MLRS | | --- -### mavlink_rc_chan_rate +### mavlink_port1_rc_chan_rate -Rate of the RC channels message for MAVLink telemetry +Rate of the RC channels message for MAVLink telemetry on port 1 | Default | Min | Max | | --- | --- | --- | @@ -2987,6 +2998,105 @@ Rate of the RC channels message for MAVLink telemetry --- +### mavlink_port2_high_latency + +Enable MAVLink high-latency mode on port 2 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port2_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 2. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port2_radio_type + +MAVLink radio type for port 2. Affects RSSI and LQ reporting on OSD. + +| Allowed Values | | +| --- | --- | +| GENERIC | Default | +| ELRS | | +| SIK | | +| MLRS | | + +--- + +### mavlink_port3_high_latency + +Enable MAVLink high-latency mode on port 3 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port3_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 3. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port3_radio_type + +MAVLink radio type for port 3. Affects RSSI and LQ reporting on OSD. + +| Allowed Values | | +| --- | --- | +| GENERIC | Default | +| ELRS | | +| SIK | | +| MLRS | | + +--- + +### mavlink_port4_high_latency + +Enable MAVLink high-latency mode on port 4 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port4_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 4. Requires RADIO_STATUS messages. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 100 | + +--- + +### mavlink_port4_radio_type + +MAVLink radio type for port 4. Affects RSSI and LQ reporting on OSD. + +| Allowed Values | | +| --- | --- | +| GENERIC | Default | +| ELRS | | +| SIK | | +| MLRS | | + +--- + ### mavlink_sysid MAVLink System ID diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index fafcf5d4762..9a5df73b174 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -452,6 +452,10 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: [8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) [8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index) [8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading) +[8740 - MSP2_INAV_ACTIVATE_LANDING](#msp2_inav_activate_landing) +[8741 - MSP2_INAV_ACTIVATE_RTH](#msp2_inav_activate_rth) +[8743 - MSP2_INAV_ARM_DISARM](#msp2_inav_arm_disarm) +[8744 - MSP2_INAV_TIMESYNC](#msp2_inav_timesync) [8752 - MSP2_INAV_SET_AUX_RC](#msp2_inav_set_aux_rc) [12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) [12289 - MSP2_RX_BIND](#msp2_rx_bind) @@ -4603,7 +4607,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Request Payload:** |Field|C Type|Size (Bytes)|Units|Description| |---|---|---|---|---| -| `altitudeDatum` | `uint8_t` | 1 | [geoAltitudeDatumFlag_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-geoaltitudedatumflag_e) | Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM` (default), `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` (not implemented yet) | +| `altitudeDatum` | `uint8_t` | 1 | [geoAltitudeDatumFlag_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-geoaltitudedatumflag_e) | Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM` (default), `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` and `NAV_WP_RELATIVE_DATUM` (not implemented yet) | | `altitudeTarget` | `int32_t` | 4 | cm | Desired altitude target according to reference datum | **Reply Payload:** **None** @@ -4683,10 +4687,11 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude coordinate | | `altitudeTarget` | `int32_t` | 4 | cm | Desired altitude target according to reference datum (0 keeps current altitude) | | `altitudeDatum` | `uint8_t` | 1 | [geoAltitudeDatumFlag_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-geoaltitudedatumflag_e) | Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM`, `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` (not implemented yet) | +| `loiterRadius` | `int32_t` | 4 | cm | Optional temporary fixed-wing PosHold loiter radius override. Appended field; omit to leave unchanged. `0` clears the override and uses `navConfig()->fw.loiter_radius`. | **Reply Payload:** **None** -**Notes:** Uses the GCSNAV/offboard path; rejected when GCSNAV is not active. Rejects `NAV_WP_TERRAIN_DATUM`; other datums are converted to local NEU and applied through `setDesiredPosition()`. Altitude of 0 leaves current Z unchanged. +**Notes:** Uses the GCSNAV/offboard path; rejected when GCSNAV is not active. Rejects `NAV_WP_TERRAIN_DATUM`; other datums are converted to local NEU and applied through `setDesiredPosition()`. Altitude of 0 leaves current Z unchanged. Existing 13-byte payloads are still accepted; 17-byte payloads append `loiterRadius`, where `0` clears the temporary override and non-zero values are centimeters. ## `MSP2_INAV_NAV_TARGET (8731 / 0x221b)` **Description:** Returns the current navigation desired global target (lat/lon/alt, heading, climb rate). @@ -4701,8 +4706,9 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `altitudeTarget` | `int32_t` | 4 | cm | Desired altitude target (takeoff datum, cm) as used by altitude/position hold | | `headingTarget` | `uint16_t` | 2 | degrees | Current heading-hold target (`getHeadingHoldTarget()`), wrapped to 0–359.99 | | `climbRate` | `int16_t` | 2 | cm/s | Desired climb rate demand (`posControl.desiredState.climbRateDemand`) | +| `loiterRadius` | `uint32_t` | 4 | cm | Temporary fixed-wing PosHold loiter radius override. `0` means no override; the configured `navConfig()->fw.loiter_radius` is used. | -**Notes:** Altitude target is reported in the takeoff datum frame (local Z). Heading is sourced from the heading-hold target. Intended for monitoring the active navigation desired target (Goto/Followme/RTH/Safehome). +**Notes:** Altitude target is reported in the takeoff datum frame (local Z). Heading is sourced from the heading-hold target. Intended for monitoring the active navigation desired target (Goto/Followme/RTH/Safehome). The appended `loiterRadius` reports the temporary override only; `0` means the configured default is active. ## `MSP2_INAV_FULL_LOCAL_POSE (8736 / 0x2220)` **Description:** Provides estimates of current attitude, local NEU position, and velocity. @@ -4748,6 +4754,48 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Notes:** Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle. +## `MSP2_INAV_ACTIVATE_LANDING (8740 / 0x2224)` +**Description:** Commands an immediate normal landing at the current position. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Requires the aircraft to be armed with usable position, altitude, and heading estimates. Creates a transient LAND waypoint at the current position without changing the uploaded mission, then enters the normal `NAV_STATE_WAYPOINT_RTH_LAND` path. This is not emergency landing. + +## `MSP2_INAV_ACTIVATE_RTH (8741 / 0x2225)` +**Description:** Commands the aircraft to execute its configured return-to-home sequence. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Requires the aircraft to be armed. Enters normal return-to-home mode through the same mode selector path as RC RTH, without setting the failsafe/geozone forced-RTH latch. + +## `MSP2_INAV_ARM_DISARM (8743 / 0x2227)` +**Description:** Arms or disarms the flight controller using the normal FC arming path. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `arm` | `uint8_t` | 1 | Boolean | Requested armed state: 0 disarms, 1 arms through the normal arming checks. | + +**Reply Payload:** **None** + +**Notes:** Returns an error for values other than 0 or 1, or when the requested armed state is not reached. + +## `MSP2_INAV_TIMESYNC (8744 / 0x2228)` +**Description:** Returns the local monotonic boot time in nanoseconds. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `timeNs` | `uint64_t` | 8 | ns | Monotonic flight-controller boot time, calculated as `(uint64_t)micros() * 1000`. | + +**Notes:** The value is little-endian like other MSP integer fields and uses the same boot-time clock returned by MAVLink `TIMESYNC`. + ## `MSP2_INAV_SET_AUX_RC (8752 / 0x2230)` **Description:** Bandwidth-efficient auxiliary RC channel update. Sets CH13-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough. diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index 52241112fd3..cb17f8d8899 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -1383,7 +1383,8 @@ "_source": "inav/src/main/navigation/navigation.h", "NAV_WP_TAKEOFF_DATUM": "0", "NAV_WP_MSL_DATUM": "1", - "NAV_WP_TERRAIN_DATUM": "2" + "NAV_WP_TERRAIN_DATUM": "2", + "NAV_WP_RELATIVE_DATUM": "3" }, "geoOriginResetMode_e": { "_source": "inav/src/main/navigation/navigation.h", @@ -2094,7 +2095,10 @@ "LOGIC_CONDITION_ACOS": "58", "LOGIC_CONDITION_ASIN": "59", "LOGIC_CONDITION_ATAN2": "60", - "LOGIC_CONDITION_LAST": "61" + "LOGIC_CONDITION_DISABLE_AUTOSPEED_AIRSPEED": "61", + "LOGIC_CONDITION_ACTIVATE_RTH": "62", + "LOGIC_CONDITION_ACTIVATE_LANDING": "63", + "LOGIC_CONDITION_LAST": "64" }, "logicWaypointOperands_e": { "_source": "inav/src/main/programming/logic_condition.h", @@ -2385,12 +2389,43 @@ "MF_SUSPEND_TRACKBACK": "(1 << 1)", "MF_TURTLE_MODE": "(1 << 2)" }, + "nav_control_type_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_GPS_ATTI": "0", + "NAV_GPS_CRUISE": "1" + }, + "nav_heading_control_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_HEADING_CONTROL_NONE": "0", + "NAV_HEADING_CONTROL_AUTO": "1", + "NAV_HEADING_CONTROL_MANUAL": "2" + }, + "nav_loiter_type_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_LOITER_RIGHT": "0", + "NAV_LOITER_LEFT": "1", + "NAV_LOITER_YAW": "2" + }, "nav_reset_type_e": { "_source": "inav/src/main/navigation/navigation.h", "NAV_RESET_NEVER": "0", "NAV_RESET_ON_FIRST_ARM": "1", "NAV_RESET_ON_EACH_ARM": "2" }, + "nav_rth_alt_profile_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_RTH_NO_ALT": "0", + "NAV_RTH_EXTRA_ALT": "1", + "NAV_RTH_CONST_ALT": "2", + "NAV_RTH_MAX_ALT": "3", + "NAV_RTH_AT_LEAST_ALT": "4", + "NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT": "5" + }, + "nav_rth_climb_profile_e": { + "_source": "inav/src/main/navigation/navigation.h", + "NAV_RTH_CLIMB_STAGE_AT_LEAST": "0", + "NAV_RTH_CLIMB_STAGE_EXTRA": "1" + }, "navAGLEstimateQuality_e": { "_source": "inav/src/main/navigation/navigation_pos_estimator_private.h", "SURFACE_QUAL_LOW": "0", @@ -2448,11 +2483,12 @@ "NAV_FSM_EVENT_SWITCH_TO_MIXERAT": "14", "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING": "15", "NAV_FSM_EVENT_SWITCH_TO_SEND_TO": "16", - "NAV_FSM_EVENT_STATE_SPECIFIC_1": "17", - "NAV_FSM_EVENT_STATE_SPECIFIC_2": "18", - "NAV_FSM_EVENT_STATE_SPECIFIC_3": "19", - "NAV_FSM_EVENT_STATE_SPECIFIC_4": "20", - "NAV_FSM_EVENT_STATE_SPECIFIC_5": "21", + "NAV_FSM_EVENT_SWITCH_TO_LANDING": "17", + "NAV_FSM_EVENT_STATE_SPECIFIC_1": "18", + "NAV_FSM_EVENT_STATE_SPECIFIC_2": "19", + "NAV_FSM_EVENT_STATE_SPECIFIC_3": "20", + "NAV_FSM_EVENT_STATE_SPECIFIC_4": "21", + "NAV_FSM_EVENT_STATE_SPECIFIC_5": "22", "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT": "NAV_FSM_EVENT_STATE_SPECIFIC_1", "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED": "NAV_FSM_EVENT_STATE_SPECIFIC_2", "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME": "NAV_FSM_EVENT_STATE_SPECIFIC_1", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index 275145df9f6..17180bfef2b 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -191,7 +191,12 @@ - [mspSDCardState_e](#enum-mspsdcardstate_e) - [multi_function_e](#enum-multi_function_e) - [multiFunctionFlags_e](#enum-multifunctionflags_e) +- [nav_control_type_e](#enum-nav_control_type_e) +- [nav_heading_control_e](#enum-nav_heading_control_e) +- [nav_loiter_type_e](#enum-nav_loiter_type_e) - [nav_reset_type_e](#enum-nav_reset_type_e) +- [nav_rth_alt_profile_e](#enum-nav_rth_alt_profile_e) +- [nav_rth_climb_profile_e](#enum-nav_rth_climb_profile_e) - [navAGLEstimateQuality_e](#enum-navaglestimatequality_e) - [navArmingBlocker_e](#enum-navarmingblocker_e) - [navDefaultAltitudeSensor_e](#enum-navdefaultaltitudesensor_e) @@ -2246,6 +2251,7 @@ | `NAV_WP_TAKEOFF_DATUM` | 0 | | | `NAV_WP_MSL_DATUM` | 1 | | | `NAV_WP_TERRAIN_DATUM` | 2 | | +| `NAV_WP_RELATIVE_DATUM` | 3 | | --- ## `geoOriginResetMode_e` @@ -3200,7 +3206,10 @@ | `LOGIC_CONDITION_ACOS` | 58 | | | `LOGIC_CONDITION_ASIN` | 59 | | | `LOGIC_CONDITION_ATAN2` | 60 | | -| `LOGIC_CONDITION_LAST` | 61 | | +| `LOGIC_CONDITION_DISABLE_AUTOSPEED_AIRSPEED` | 61 | | +| `LOGIC_CONDITION_ACTIVATE_RTH` | 62 | | +| `LOGIC_CONDITION_ACTIVATE_LANDING` | 63 | | +| `LOGIC_CONDITION_LAST` | 64 | | --- ## `logicWaypointOperands_e` @@ -3607,6 +3616,38 @@ | `MF_SUSPEND_TRACKBACK` | (1 << 1) | | | `MF_TURTLE_MODE` | (1 << 2) | | +--- +## `nav_control_type_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_GPS_ATTI` | 0 | | +| `NAV_GPS_CRUISE` | 1 | | + +--- +## `nav_heading_control_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_HEADING_CONTROL_NONE` | 0 | | +| `NAV_HEADING_CONTROL_AUTO` | 1 | | +| `NAV_HEADING_CONTROL_MANUAL` | 2 | | + +--- +## `nav_loiter_type_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_LOITER_RIGHT` | 0 | | +| `NAV_LOITER_LEFT` | 1 | | +| `NAV_LOITER_YAW` | 2 | | + --- ## `nav_reset_type_e` @@ -3618,6 +3659,30 @@ | `NAV_RESET_ON_FIRST_ARM` | 1 | | | `NAV_RESET_ON_EACH_ARM` | 2 | | +--- +## `nav_rth_alt_profile_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_RTH_NO_ALT` | 0 | | +| `NAV_RTH_EXTRA_ALT` | 1 | | +| `NAV_RTH_CONST_ALT` | 2 | | +| `NAV_RTH_MAX_ALT` | 3 | | +| `NAV_RTH_AT_LEAST_ALT` | 4 | | +| `NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT` | 5 | | + +--- +## `nav_rth_climb_profile_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_RTH_CLIMB_STAGE_AT_LEAST` | 0 | | +| `NAV_RTH_CLIMB_STAGE_EXTRA` | 1 | | + --- ## `navAGLEstimateQuality_e` @@ -3710,11 +3775,12 @@ | `NAV_FSM_EVENT_SWITCH_TO_MIXERAT` | 14 | | | `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING` | 15 | | | `NAV_FSM_EVENT_SWITCH_TO_SEND_TO` | 16 | | -| `NAV_FSM_EVENT_STATE_SPECIFIC_1` | 17 | | -| `NAV_FSM_EVENT_STATE_SPECIFIC_2` | 18 | | -| `NAV_FSM_EVENT_STATE_SPECIFIC_3` | 19 | | -| `NAV_FSM_EVENT_STATE_SPECIFIC_4` | 20 | | -| `NAV_FSM_EVENT_STATE_SPECIFIC_5` | 21 | | +| `NAV_FSM_EVENT_SWITCH_TO_LANDING` | 17 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_1` | 18 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_2` | 19 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_3` | 20 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_4` | 21 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_5` | 22 | | | `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | | `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED` | NAV_FSM_EVENT_STATE_SPECIFIC_2 | | | `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index e8f64d0cb93..06f97d7210e 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -1,7 +1,7 @@ { "version": { "major": 2, - "minor": 0, + "minor": 1, "patch": 0 }, "messages": { @@ -11015,7 +11015,7 @@ { "name": "altitudeDatum", "ctype": "uint8_t", - "desc": "Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM` (default), `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` (not implemented yet)", + "desc": "Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM` (default), `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` and `NAV_WP_RELATIVE_DATUM` (not implemented yet)", "units": "", "enum": "geoAltitudeDatumFlag_e" }, @@ -11224,11 +11224,17 @@ "desc": "Altitude reference datum flag (`geoAltitudeDatumFlag_e`): `NAV_WP_TAKEOFF_DATUM`, `NAV_WP_MSL_DATUM`, `NAV_WP_TERRAIN_DATUM` (not implemented yet)", "units": "", "enum": "geoAltitudeDatumFlag_e" + }, + { + "name": "loiterRadius", + "ctype": "int32_t", + "desc": "Optional temporary fixed-wing PosHold loiter radius override. Appended field; omit to leave unchanged. `0` clears the override and uses `navConfig()->fw.loiter_radius`.", + "units": "cm" } ] }, "reply": null, - "notes": "Uses the GCSNAV/offboard path; rejected when GCSNAV is not active. Rejects `NAV_WP_TERRAIN_DATUM`; other datums are converted to local NEU and applied through `setDesiredPosition()`. Altitude of 0 leaves current Z unchanged.", + "notes": "Uses the GCSNAV/offboard path; rejected when GCSNAV is not active. Rejects `NAV_WP_TERRAIN_DATUM`; other datums are converted to local NEU and applied through `setDesiredPosition()`. Altitude of 0 leaves current Z unchanged. Existing 13-byte payloads are still accepted; 17-byte payloads append `loiterRadius`, where `0` clears the temporary override and non-zero values are centimeters.", "description": "Sets desired GCS Nav position with global coordinates (WP 254/GOTO)." }, "MSP2_INAV_NAV_TARGET": { @@ -11266,10 +11272,16 @@ "ctype": "int16_t", "desc": "Desired climb rate demand (`posControl.desiredState.climbRateDemand`)", "units": "cm/s" + }, + { + "name": "loiterRadius", + "ctype": "uint32_t", + "desc": "Temporary fixed-wing PosHold loiter radius override. `0` means no override; the configured `navConfig()->fw.loiter_radius` is used.", + "units": "cm" } ] }, - "notes": "Altitude target is reported in the takeoff datum frame (local Z). Heading is sourced from the heading-hold target. Intended for monitoring the active navigation desired target (Goto/Followme/RTH/Safehome).", + "notes": "Altitude target is reported in the takeoff datum frame (local Z). Heading is sourced from the heading-hold target. Intended for monitoring the active navigation desired target (Goto/Followme/RTH/Safehome). The appended `loiterRadius` reports the temporary override only; `0` means the configured default is active.", "description": "Returns the current navigation desired global target (lat/lon/alt, heading, climb rate)." }, "MSP2_INAV_FULL_LOCAL_POSE": { @@ -11371,6 +11383,56 @@ "notes": "Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle.", "description": "Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading." }, + "MSP2_INAV_ACTIVATE_LANDING": { + "code": 8740, + "mspv": 2, + "request": null, + "reply": null, + "notes": "Requires the aircraft to be armed with usable position, altitude, and heading estimates. Creates a transient LAND waypoint at the current position without changing the uploaded mission, then enters the normal `NAV_STATE_WAYPOINT_RTH_LAND` path. This is not emergency landing.", + "description": "Commands an immediate normal landing at the current position." + }, + "MSP2_INAV_ACTIVATE_RTH": { + "code": 8741, + "mspv": 2, + "request": null, + "reply": null, + "notes": "Requires the aircraft to be armed. Enters normal return-to-home mode through the same mode selector path as RC RTH, without setting the failsafe/geozone forced-RTH latch.", + "description": "Commands the aircraft to execute its configured return-to-home sequence." + }, + "MSP2_INAV_ARM_DISARM": { + "code": 8743, + "mspv": 2, + "request": { + "payload": [ + { + "name": "arm", + "ctype": "uint8_t", + "desc": "Requested armed state: 0 disarms, 1 arms through the normal arming checks.", + "units": "Boolean" + } + ] + }, + "reply": null, + "notes": "Returns an error for values other than 0 or 1, or when the requested armed state is not reached.", + "description": "Arms or disarms the flight controller using the normal FC arming path." + }, + "MSP2_INAV_TIMESYNC": { + "code": 8744, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "timeNs", + "ctype": "uint64_t", + "desc": "Monotonic flight-controller boot time, calculated as `(uint64_t)micros() * 1000`.", + "units": "ns" + } + ] + }, + "notes": "The value is little-endian like other MSP integer fields and uses the same boot-time clock returned by MAVLink `TIMESYNC`.", + "description": "Returns the local monotonic boot time in nanoseconds." + }, "MSP2_INAV_SET_AUX_RC": { "code": 8752, "mspv": 2, @@ -11448,4 +11510,4 @@ "description": "Initiates binding for MSP receivers (mLRS)." } } -} \ No newline at end of file +} diff --git a/lib/main/MAVLink/common/mavlink_msg_global_position_int.h b/lib/main/MAVLink/common/mavlink_msg_global_position_int.h deleted file mode 100755 index 84482700408..00000000000 --- a/lib/main/MAVLink/common/mavlink_msg_global_position_int.h +++ /dev/null @@ -1,484 +0,0 @@ -#pragma once -// MESSAGE GLOBAL_POSITION_INT PACKING - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 33 - - -typedef struct __mavlink_global_position_int_t { - uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ - int32_t lat; /*< [degE7] Latitude, expressed*/ - int32_t lon; /*< [degE7] Longitude, expressed*/ - int32_t alt; /*< [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL.*/ - int32_t relative_alt; /*< [mm] Altitude above home*/ - int16_t vx; /*< [cm/s] Ground X Speed (Latitude, positive north)*/ - int16_t vy; /*< [cm/s] Ground Y Speed (Longitude, positive east)*/ - int16_t vz; /*< [cm/s] Ground Z Speed (Altitude, positive down)*/ - uint16_t hdg; /*< [cdeg] Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/ -} mavlink_global_position_int_t; - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 28 -#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN 28 -#define MAVLINK_MSG_ID_33_LEN 28 -#define MAVLINK_MSG_ID_33_MIN_LEN 28 - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC 104 -#define MAVLINK_MSG_ID_33_CRC 104 - - - -#if MAVLINK_COMMAND_24BIT -#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT { \ - 33, \ - "GLOBAL_POSITION_INT", \ - 9, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_global_position_int_t, time_boot_ms) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_int_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_int_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_global_position_int_t, alt) }, \ - { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_global_position_int_t, relative_alt) }, \ - { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_global_position_int_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_global_position_int_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_global_position_int_t, vz) }, \ - { "hdg", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_global_position_int_t, hdg) }, \ - } \ -} -#else -#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT { \ - "GLOBAL_POSITION_INT", \ - 9, \ - { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_global_position_int_t, time_boot_ms) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_int_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_int_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_global_position_int_t, alt) }, \ - { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_global_position_int_t, relative_alt) }, \ - { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_global_position_int_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_global_position_int_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_global_position_int_t, vz) }, \ - { "hdg", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_global_position_int_t, hdg) }, \ - } \ -} -#endif - -/** - * @brief Pack a global_position_int message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param lat [degE7] Latitude, expressed - * @param lon [degE7] Longitude, expressed - * @param alt [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. - * @param relative_alt [mm] Altitude above home - * @param vx [cm/s] Ground X Speed (Latitude, positive north) - * @param vy [cm/s] Ground Y Speed (Longitude, positive east) - * @param vz [cm/s] Ground Z Speed (Altitude, positive down) - * @param hdg [cdeg] Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, lon); - _mav_put_int32_t(buf, 12, alt); - _mav_put_int32_t(buf, 16, relative_alt); - _mav_put_int16_t(buf, 20, vx); - _mav_put_int16_t(buf, 22, vy); - _mav_put_int16_t(buf, 24, vz); - _mav_put_uint16_t(buf, 26, hdg); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#else - mavlink_global_position_int_t packet; - packet.time_boot_ms = time_boot_ms; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.relative_alt = relative_alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.hdg = hdg; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -} - -/** - * @brief Pack a global_position_int message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param status MAVLink status structure - * @param msg The MAVLink message to compress the data into - * - * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param lat [degE7] Latitude, expressed - * @param lon [degE7] Longitude, expressed - * @param alt [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. - * @param relative_alt [mm] Altitude above home - * @param vx [cm/s] Ground X Speed (Latitude, positive north) - * @param vy [cm/s] Ground Y Speed (Longitude, positive east) - * @param vz [cm/s] Ground Z Speed (Altitude, positive down) - * @param hdg [cdeg] Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_int_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, - uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, lon); - _mav_put_int32_t(buf, 12, alt); - _mav_put_int32_t(buf, 16, relative_alt); - _mav_put_int16_t(buf, 20, vx); - _mav_put_int16_t(buf, 22, vy); - _mav_put_int16_t(buf, 24, vz); - _mav_put_uint16_t(buf, 26, hdg); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#else - mavlink_global_position_int_t packet; - packet.time_boot_ms = time_boot_ms; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.relative_alt = relative_alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.hdg = hdg; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#else - return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif -} - -/** - * @brief Pack a global_position_int message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param lat [degE7] Latitude, expressed - * @param lon [degE7] Longitude, expressed - * @param alt [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. - * @param relative_alt [mm] Altitude above home - * @param vx [cm/s] Ground X Speed (Latitude, positive north) - * @param vy [cm/s] Ground Y Speed (Longitude, positive east) - * @param vz [cm/s] Ground Z Speed (Altitude, positive down) - * @param hdg [cdeg] Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time_boot_ms,int32_t lat,int32_t lon,int32_t alt,int32_t relative_alt,int16_t vx,int16_t vy,int16_t vz,uint16_t hdg) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, lon); - _mav_put_int32_t(buf, 12, alt); - _mav_put_int32_t(buf, 16, relative_alt); - _mav_put_int16_t(buf, 20, vx); - _mav_put_int16_t(buf, 22, vy); - _mav_put_int16_t(buf, 24, vz); - _mav_put_uint16_t(buf, 26, hdg); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#else - mavlink_global_position_int_t packet; - packet.time_boot_ms = time_boot_ms; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.relative_alt = relative_alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.hdg = hdg; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -} - -/** - * @brief Encode a global_position_int struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param global_position_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) -{ - return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); -} - -/** - * @brief Encode a global_position_int struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param global_position_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) -{ - return mavlink_msg_global_position_int_pack_chan(system_id, component_id, chan, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); -} - -/** - * @brief Encode a global_position_int struct with provided status structure - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param status MAVLink status structure - * @param msg The MAVLink message to compress the data into - * @param global_position_int C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_int_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) -{ - return mavlink_msg_global_position_int_pack_status(system_id, component_id, _status, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); -} - -/** - * @brief Send a global_position_int message - * @param chan MAVLink channel to send the message - * - * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param lat [degE7] Latitude, expressed - * @param lon [degE7] Longitude, expressed - * @param alt [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. - * @param relative_alt [mm] Altitude above home - * @param vx [cm/s] Ground X Speed (Latitude, positive north) - * @param vy [cm/s] Ground Y Speed (Longitude, positive east) - * @param vz [cm/s] Ground Z Speed (Altitude, positive down) - * @param hdg [cdeg] Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, lon); - _mav_put_int32_t(buf, 12, alt); - _mav_put_int32_t(buf, 16, relative_alt); - _mav_put_int16_t(buf, 20, vx); - _mav_put_int16_t(buf, 22, vy); - _mav_put_int16_t(buf, 24, vz); - _mav_put_uint16_t(buf, 26, hdg); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#else - mavlink_global_position_int_t packet; - packet.time_boot_ms = time_boot_ms; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.relative_alt = relative_alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - packet.hdg = hdg; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#endif -} - -/** - * @brief Send a global_position_int message - * @param chan MAVLink channel to send the message - * @param struct The MAVLink struct to serialize - */ -static inline void mavlink_msg_global_position_int_send_struct(mavlink_channel_t chan, const mavlink_global_position_int_t* global_position_int) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_global_position_int_send(chan, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)global_position_int, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#endif -} - -#if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This variant of _send() can be used to save stack space by reusing - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_global_position_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, lon); - _mav_put_int32_t(buf, 12, alt); - _mav_put_int32_t(buf, 16, relative_alt); - _mav_put_int16_t(buf, 20, vx); - _mav_put_int16_t(buf, 22, vy); - _mav_put_int16_t(buf, 24, vz); - _mav_put_uint16_t(buf, 26, hdg); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#else - mavlink_global_position_int_t *packet = (mavlink_global_position_int_t *)msgbuf; - packet->time_boot_ms = time_boot_ms; - packet->lat = lat; - packet->lon = lon; - packet->alt = alt; - packet->relative_alt = relative_alt; - packet->vx = vx; - packet->vy = vy; - packet->vz = vz; - packet->hdg = hdg; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); -#endif -} -#endif - -#endif - -// MESSAGE GLOBAL_POSITION_INT UNPACKING - - -/** - * @brief Get field time_boot_ms from global_position_int message - * - * @return [ms] Timestamp (time since system boot). - */ -static inline uint32_t mavlink_msg_global_position_int_get_time_boot_ms(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field lat from global_position_int message - * - * @return [degE7] Latitude, expressed - */ -static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field lon from global_position_int message - * - * @return [degE7] Longitude, expressed - */ -static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Get field alt from global_position_int message - * - * @return [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. - */ -static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 12); -} - -/** - * @brief Get field relative_alt from global_position_int message - * - * @return [mm] Altitude above home - */ -static inline int32_t mavlink_msg_global_position_int_get_relative_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 16); -} - -/** - * @brief Get field vx from global_position_int message - * - * @return [cm/s] Ground X Speed (Latitude, positive north) - */ -static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 20); -} - -/** - * @brief Get field vy from global_position_int message - * - * @return [cm/s] Ground Y Speed (Longitude, positive east) - */ -static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 22); -} - -/** - * @brief Get field vz from global_position_int message - * - * @return [cm/s] Ground Z Speed (Altitude, positive down) - */ -static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 24); -} - -/** - * @brief Get field hdg from global_position_int message - * - * @return [cdeg] Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - */ -static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 26); -} - -/** - * @brief Decode a global_position_int message into a struct - * - * @param msg The message to decode - * @param global_position_int C-struct to decode the message contents into - */ -static inline void mavlink_msg_global_position_int_decode(const mavlink_message_t* msg, mavlink_global_position_int_t* global_position_int) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - global_position_int->time_boot_ms = mavlink_msg_global_position_int_get_time_boot_ms(msg); - global_position_int->lat = mavlink_msg_global_position_int_get_lat(msg); - global_position_int->lon = mavlink_msg_global_position_int_get_lon(msg); - global_position_int->alt = mavlink_msg_global_position_int_get_alt(msg); - global_position_int->relative_alt = mavlink_msg_global_position_int_get_relative_alt(msg); - global_position_int->vx = mavlink_msg_global_position_int_get_vx(msg); - global_position_int->vy = mavlink_msg_global_position_int_get_vy(msg); - global_position_int->vz = mavlink_msg_global_position_int_get_vz(msg); - global_position_int->hdg = mavlink_msg_global_position_int_get_hdg(msg); -#else - uint8_t len = msg->len < MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN? msg->len : MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN; - memset(global_position_int, 0, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); - memcpy(global_position_int, _MAV_PAYLOAD(msg), len); -#endif -} diff --git a/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h b/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h deleted file mode 100644 index efc35ab8eb0..00000000000 --- a/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h +++ /dev/null @@ -1,363 +0,0 @@ -#pragma once -// MESSAGE PROTOCOL_VERSION PACKING - -#define MAVLINK_MSG_ID_PROTOCOL_VERSION 300 - - -typedef struct __mavlink_protocol_version_t { - uint16_t version; /*< Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc.*/ - uint16_t min_version; /*< Minimum MAVLink version supported*/ - uint16_t max_version; /*< Maximum MAVLink version supported (set to the same value as version by default)*/ - uint8_t spec_version_hash[8]; /*< The first 8 bytes (not characters printed in hex!) of the git hash.*/ - uint8_t library_version_hash[8]; /*< The first 8 bytes (not characters printed in hex!) of the git hash.*/ -} mavlink_protocol_version_t; - -#define MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN 22 -#define MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN 22 -#define MAVLINK_MSG_ID_300_LEN 22 -#define MAVLINK_MSG_ID_300_MIN_LEN 22 - -#define MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC 217 -#define MAVLINK_MSG_ID_300_CRC 217 - -#define MAVLINK_MSG_PROTOCOL_VERSION_FIELD_SPEC_VERSION_HASH_LEN 8 -#define MAVLINK_MSG_PROTOCOL_VERSION_FIELD_LIBRARY_VERSION_HASH_LEN 8 - -#if MAVLINK_COMMAND_24BIT -#define MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION { \ - 300, \ - "PROTOCOL_VERSION", \ - 5, \ - { { "version", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_protocol_version_t, version) }, \ - { "min_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_protocol_version_t, min_version) }, \ - { "max_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_protocol_version_t, max_version) }, \ - { "spec_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 6, offsetof(mavlink_protocol_version_t, spec_version_hash) }, \ - { "library_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 14, offsetof(mavlink_protocol_version_t, library_version_hash) }, \ - } \ -} -#else -#define MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION { \ - "PROTOCOL_VERSION", \ - 5, \ - { { "version", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_protocol_version_t, version) }, \ - { "min_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_protocol_version_t, min_version) }, \ - { "max_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_protocol_version_t, max_version) }, \ - { "spec_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 6, offsetof(mavlink_protocol_version_t, spec_version_hash) }, \ - { "library_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 14, offsetof(mavlink_protocol_version_t, library_version_hash) }, \ - } \ -} -#endif - -/** - * @brief Pack a protocol_version message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. - * @param min_version Minimum MAVLink version supported - * @param max_version Maximum MAVLink version supported (set to the same value as version by default) - * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. - * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_protocol_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; - _mav_put_uint16_t(buf, 0, version); - _mav_put_uint16_t(buf, 2, min_version); - _mav_put_uint16_t(buf, 4, max_version); - _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); - _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); -#else - mavlink_protocol_version_t packet; - packet.version = version; - packet.min_version = min_version; - packet.max_version = max_version; - mav_array_assign_uint8_t(packet.spec_version_hash, spec_version_hash, 8); - mav_array_assign_uint8_t(packet.library_version_hash, library_version_hash, 8); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PROTOCOL_VERSION; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); -} - -/** - * @brief Pack a protocol_version message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param status MAVLink status structure - * @param msg The MAVLink message to compress the data into - * - * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. - * @param min_version Minimum MAVLink version supported - * @param max_version Maximum MAVLink version supported (set to the same value as version by default) - * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. - * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_protocol_version_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, - uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; - _mav_put_uint16_t(buf, 0, version); - _mav_put_uint16_t(buf, 2, min_version); - _mav_put_uint16_t(buf, 4, max_version); - _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); - _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); -#else - mavlink_protocol_version_t packet; - packet.version = version; - packet.min_version = min_version; - packet.max_version = max_version; - mav_array_memcpy(packet.spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); - mav_array_memcpy(packet.library_version_hash, library_version_hash, sizeof(uint8_t)*8); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PROTOCOL_VERSION; -#if MAVLINK_CRC_EXTRA - return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); -#else - return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); -#endif -} - -/** - * @brief Pack a protocol_version message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. - * @param min_version Minimum MAVLink version supported - * @param max_version Maximum MAVLink version supported (set to the same value as version by default) - * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. - * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_protocol_version_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t version,uint16_t min_version,uint16_t max_version,const uint8_t *spec_version_hash,const uint8_t *library_version_hash) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; - _mav_put_uint16_t(buf, 0, version); - _mav_put_uint16_t(buf, 2, min_version); - _mav_put_uint16_t(buf, 4, max_version); - _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); - _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); -#else - mavlink_protocol_version_t packet; - packet.version = version; - packet.min_version = min_version; - packet.max_version = max_version; - mav_array_assign_uint8_t(packet.spec_version_hash, spec_version_hash, 8); - mav_array_assign_uint8_t(packet.library_version_hash, library_version_hash, 8); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PROTOCOL_VERSION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); -} - -/** - * @brief Encode a protocol_version struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param protocol_version C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_protocol_version_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_protocol_version_t* protocol_version) -{ - return mavlink_msg_protocol_version_pack(system_id, component_id, msg, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); -} - -/** - * @brief Encode a protocol_version struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param protocol_version C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_protocol_version_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_protocol_version_t* protocol_version) -{ - return mavlink_msg_protocol_version_pack_chan(system_id, component_id, chan, msg, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); -} - -/** - * @brief Encode a protocol_version struct with provided status structure - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param status MAVLink status structure - * @param msg The MAVLink message to compress the data into - * @param protocol_version C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_protocol_version_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_protocol_version_t* protocol_version) -{ - return mavlink_msg_protocol_version_pack_status(system_id, component_id, _status, msg, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); -} - -/** - * @brief Send a protocol_version message - * @param chan MAVLink channel to send the message - * - * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. - * @param min_version Minimum MAVLink version supported - * @param max_version Maximum MAVLink version supported (set to the same value as version by default) - * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. - * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_protocol_version_send(mavlink_channel_t chan, uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; - _mav_put_uint16_t(buf, 0, version); - _mav_put_uint16_t(buf, 2, min_version); - _mav_put_uint16_t(buf, 4, max_version); - _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); - _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); -#else - mavlink_protocol_version_t packet; - packet.version = version; - packet.min_version = min_version; - packet.max_version = max_version; - mav_array_assign_uint8_t(packet.spec_version_hash, spec_version_hash, 8); - mav_array_assign_uint8_t(packet.library_version_hash, library_version_hash, 8); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)&packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); -#endif -} - -/** - * @brief Send a protocol_version message - * @param chan MAVLink channel to send the message - * @param struct The MAVLink struct to serialize - */ -static inline void mavlink_msg_protocol_version_send_struct(mavlink_channel_t chan, const mavlink_protocol_version_t* protocol_version) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_protocol_version_send(chan, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)protocol_version, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); -#endif -} - -#if MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This variant of _send() can be used to save stack space by reusing - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_protocol_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_uint16_t(buf, 0, version); - _mav_put_uint16_t(buf, 2, min_version); - _mav_put_uint16_t(buf, 4, max_version); - _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); - _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); -#else - mavlink_protocol_version_t *packet = (mavlink_protocol_version_t *)msgbuf; - packet->version = version; - packet->min_version = min_version; - packet->max_version = max_version; - mav_array_assign_uint8_t(packet->spec_version_hash, spec_version_hash, 8); - mav_array_assign_uint8_t(packet->library_version_hash, library_version_hash, 8); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); -#endif -} -#endif - -#endif - -// MESSAGE PROTOCOL_VERSION UNPACKING - - -/** - * @brief Get field version from protocol_version message - * - * @return Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. - */ -static inline uint16_t mavlink_msg_protocol_version_get_version(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Get field min_version from protocol_version message - * - * @return Minimum MAVLink version supported - */ -static inline uint16_t mavlink_msg_protocol_version_get_min_version(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 2); -} - -/** - * @brief Get field max_version from protocol_version message - * - * @return Maximum MAVLink version supported (set to the same value as version by default) - */ -static inline uint16_t mavlink_msg_protocol_version_get_max_version(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 4); -} - -/** - * @brief Get field spec_version_hash from protocol_version message - * - * @return The first 8 bytes (not characters printed in hex!) of the git hash. - */ -static inline uint16_t mavlink_msg_protocol_version_get_spec_version_hash(const mavlink_message_t* msg, uint8_t *spec_version_hash) -{ - return _MAV_RETURN_uint8_t_array(msg, spec_version_hash, 8, 6); -} - -/** - * @brief Get field library_version_hash from protocol_version message - * - * @return The first 8 bytes (not characters printed in hex!) of the git hash. - */ -static inline uint16_t mavlink_msg_protocol_version_get_library_version_hash(const mavlink_message_t* msg, uint8_t *library_version_hash) -{ - return _MAV_RETURN_uint8_t_array(msg, library_version_hash, 8, 14); -} - -/** - * @brief Decode a protocol_version message into a struct - * - * @param msg The message to decode - * @param protocol_version C-struct to decode the message contents into - */ -static inline void mavlink_msg_protocol_version_decode(const mavlink_message_t* msg, mavlink_protocol_version_t* protocol_version) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - protocol_version->version = mavlink_msg_protocol_version_get_version(msg); - protocol_version->min_version = mavlink_msg_protocol_version_get_min_version(msg); - protocol_version->max_version = mavlink_msg_protocol_version_get_max_version(msg); - mavlink_msg_protocol_version_get_spec_version_hash(msg, protocol_version->spec_version_hash); - mavlink_msg_protocol_version_get_library_version_hash(msg, protocol_version->library_version_hash); -#else - uint8_t len = msg->len < MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN? msg->len : MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN; - memset(protocol_version, 0, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); - memcpy(protocol_version, _MAV_PAYLOAD(msg), len); -#endif -} diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3223aca497e..0fc7ecf1070 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -619,6 +619,17 @@ void tryArm(void) } } +bool fcSetArmState(bool arm) +{ + if (arm) { + tryArm(); + } else { + disarm(DISARM_SWITCH); + } + + return ARMING_FLAG(ARMED) == arm; +} + #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) void releaseSharedTelemetryPorts(void) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 02a5c889a65..3e5ba12714b 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -39,6 +39,7 @@ void handleInflightCalibrationStickPosition(void); void disarm(disarmReason_t disarmReason); timeUs_t getLastDisarmTimeUs(void); void tryArm(void); +bool fcSetArmState(bool arm); disarmReason_t getDisarmReason(void); bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm); @@ -48,4 +49,4 @@ float getFlightTime(void); void resetFlightTime(void); float getArmTime(void); void fcReboot(bool bootLoader); -bool isMspConfigActive(bool isActive); \ No newline at end of file +bool isMspConfigActive(bool isActive); diff --git a/src/main/fc/fc_mavlink.c b/src/main/fc/fc_mavlink.c index de527b67614..95f54cc50ff 100644 --- a/src/main/fc/fc_mavlink.c +++ b/src/main/fc/fc_mavlink.c @@ -10,6 +10,139 @@ #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) +static void mavlinkResetTunnelState(uint8_t ingressPortIndex) +{ + resetMspPort(&mavTunnelMspPorts[ingressPortIndex], NULL); + mavTunnelRemoteSystemIds[ingressPortIndex] = 0; + mavTunnelRemoteComponentIds[ingressPortIndex] = 0; +} + +static void mavlinkSendTunnelReply(uint8_t targetSystem, uint8_t targetComponent, const uint8_t *payload, uint8_t payloadLength) +{ + uint8_t tunnelPayload[MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN] = { 0 }; + memcpy(tunnelPayload, payload, payloadLength); + + mavlink_msg_tunnel_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + targetSystem, + targetComponent, + MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP, + payloadLength, + tunnelPayload); + mavlinkSendMessage(); +} + +static void mavlinkSendTunnelMspReply(uint8_t targetSystem, uint8_t targetComponent, mspPacket_t *reply, uint8_t *replyPayloadHead, mspVersion_e mspVersion) +{ + sbufSwitchToReader(&reply->buf, replyPayloadHead); + + const int frameLength = mspSerialEncodePacket(reply, mspVersion, mavTunnelFrameBuf, sizeof(mavTunnelFrameBuf)); + for (int offset = 0; offset < frameLength; offset += MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN) { + const uint8_t chunkLength = MIN(MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN, frameLength - offset); + mavlinkSendTunnelReply( + targetSystem, + targetComponent, + mavTunnelFrameBuf + offset, + chunkLength); + } +} + +static bool handleIncoming_TUNNEL(uint8_t ingressPortIndex) +{ + if (mavlinkGetProtocolVersion() == 1) { + return false; + } + + mavlink_tunnel_t msg; + mavlink_msg_tunnel_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.payload_type != MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP) { + return false; + } + + if (msg.target_system != mavSystemId) { + return false; + } + + if (msg.target_component != 0 && msg.target_component != mavComponentId) { + return false; + } + + mspPort_t *mspPort = &mavTunnelMspPorts[ingressPortIndex]; + const timeMs_t now = millis(); + if (msg.payload_length > MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN) { + mavlinkResetTunnelState(ingressPortIndex); + return false; + } + + if (mspPort->c_state != MSP_IDLE && + ((now - mspPort->lastActivityMs) > MAVLINK_TUNNEL_MSP_TIMEOUT_MS || + mavTunnelRemoteSystemIds[ingressPortIndex] != mavlinkContext.recvMsg.sysid || + mavTunnelRemoteComponentIds[ingressPortIndex] != mavlinkContext.recvMsg.compid)) { + mavlinkResetTunnelState(ingressPortIndex); + } + + mavTunnelRemoteSystemIds[ingressPortIndex] = mavlinkContext.recvMsg.sysid; + mavTunnelRemoteComponentIds[ingressPortIndex] = mavlinkContext.recvMsg.compid; + mspPort->lastActivityMs = now; + + bool handled = false; + for (uint8_t payloadIndex = 0; payloadIndex < msg.payload_length; payloadIndex++) { + if (!mspSerialProcessReceivedByte(mspPort, msg.payload[payloadIndex])) { + continue; + } + + handled = true; + if (mspPort->c_state != MSP_COMMAND_RECEIVED) { + continue; + } + + mspPacket_t reply = { + .buf = { .ptr = mavTunnelReplyPayloadBuf, .end = ARRAYEND(mavTunnelReplyPayloadBuf), }, + .cmd = -1, + .flags = 0, + .result = 0, + }; + uint8_t *replyPayloadHead = reply.buf.ptr; + + if (mspPort->cmdMSP == MSP_SET_PASSTHROUGH) { + reply.cmd = MSP_SET_PASSTHROUGH; + reply.result = MSP_RESULT_ERROR; + mavlinkSendTunnelMspReply(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, &reply, replyPayloadHead, mspPort->mspVersion); + mavlinkResetTunnelState(ingressPortIndex); + break; + } + + mspPostProcessFnPtr mspPostProcessFn = NULL; + const uint16_t command = mspPort->cmdMSP; + mspResult_e status = mspSerialProcessCommand(mspPort, mspFcProcessCommand, &reply, &mspPostProcessFn); + + if (mspPostProcessFn && command != MSP_REBOOT) { + sbufInit(&reply.buf, mavTunnelReplyPayloadBuf, ARRAYEND(mavTunnelReplyPayloadBuf)); + reply.result = MSP_RESULT_ERROR; + mspPostProcessFn = NULL; + status = MSP_RESULT_ERROR; + } + + if (status != MSP_RESULT_NO_REPLY) { + mavlinkSendTunnelMspReply(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, &reply, replyPayloadHead, mspPort->mspVersion); + } + + mavlinkResetTunnelState(ingressPortIndex); + + if (mspPostProcessFn) { + waitForSerialPortToFinishTransmitting(mavPortStates[ingressPortIndex].port); + mspPostProcessFn(mavPortStates[ingressPortIndex].port); + } + + break; + } + + return handled; +} + static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { mavlink_rc_channels_override_t msg; mavlink_msg_rc_channels_override_decode(&mavlinkContext.recvMsg, &msg); @@ -260,21 +393,25 @@ mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIn return mavlinkHandleIncomingMissionItemInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: return mavlinkHandleIncomingMissionRequestList() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_COMMAND_LONG: + return mavlinkHandleIncomingCommandLong() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_COMMAND_INT: + return mavlinkHandleIncomingCommandInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST: return mavlinkHandleIncomingMissionRequest() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_REQUEST_INT: return mavlinkHandleIncomingMissionRequestInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_MISSION_ACK: return mavlinkHandleIncomingMissionAck() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; - case MAVLINK_MSG_ID_COMMAND_LONG: - return mavlinkHandleIncomingCommandLong() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; - case MAVLINK_MSG_ID_COMMAND_INT: - return mavlinkHandleIncomingCommandInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: return mavlinkHandleIncomingRequestDataStream() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: handleIncoming_RC_CHANNELS_OVERRIDE(); return MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + return mavlinkHandleIncomingSetPositionTargetLocalNed() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: + return mavlinkHandleIncomingSetPositionTargetGlobalInt() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; #ifdef USE_ADSB case MAVLINK_MSG_ID_ADSB_VEHICLE: return handleIncoming_ADSB_VEHICLE() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; @@ -288,6 +425,8 @@ mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIn case MAVLINK_MSG_ID_RADIO_STATUS: handleIncoming_RADIO_STATUS(); return MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY; + case MAVLINK_MSG_ID_TUNNEL: + return handleIncoming_TUNNEL(ingressPortIndex) ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; default: return MAVLINK_FC_DISPATCH_NOT_HANDLED; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 69f3f4171a6..317224edf2d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -744,6 +744,14 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; + case MSP2_INAV_TIMESYNC: + { + const uint64_t timeNs = (uint64_t)micros() * 1000ULL; + sbufWriteU32(dst, (uint32_t)timeNs); + sbufWriteU32(dst, (uint32_t)(timeNs >> 32)); + } + break; + case MSP_SONAR_ALTITUDE: #ifdef USE_RANGEFINDER sbufWriteU32(dst, rangefinderGetLatestAltitude()); @@ -3968,6 +3976,27 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } return MSP_RESULT_ERROR; + case MSP2_INAV_ACTIVATE_LANDING: + if (dataSize == 0 && activateForcedLanding()) { + break; + } + return MSP_RESULT_ERROR; + + case MSP2_INAV_ACTIVATE_RTH: + if (dataSize == 0 && activateRTHMode()) { + break; + } + return MSP_RESULT_ERROR; + + case MSP2_INAV_ARM_DISARM: + if (dataSize == 1) { + uint8_t arm; + if (sbufReadU8Safe(&arm, src) && arm <= 1 && fcSetArmState(arm)) { + break; + } + } + return MSP_RESULT_ERROR; + default: return MSP_RESULT_ERROR; } @@ -4794,7 +4823,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; case MSP2_INAV_SET_GLOBAL_TARGET: - if (dataSize != (3 * sizeof(int32_t) + sizeof(uint8_t)) || !isGCSValid()) { + if ((dataSize != (3 * sizeof(int32_t) + sizeof(uint8_t)) && dataSize != (4 * sizeof(int32_t) + sizeof(uint8_t))) || !isGCSValid()) { *ret = MSP_RESULT_ERROR; break; } @@ -4806,8 +4835,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu targetLlh.alt = (int32_t)sbufReadU32(src); const geoAltitudeDatumFlag_e datumFlag = (geoAltitudeDatumFlag_e)sbufReadU8(src); + const bool hasLoiterRadius = dataSize == (4 * sizeof(int32_t) + sizeof(uint8_t)); + const int32_t loiterRadius = hasLoiterRadius ? (int32_t)sbufReadU32(src) : 0; - if (datumFlag == NAV_WP_TERRAIN_DATUM) { + if (datumFlag == NAV_WP_TERRAIN_DATUM || loiterRadius < 0) { *ret = MSP_RESULT_ERROR; break; } @@ -4825,6 +4856,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } setDesiredPosition(&targetPos, posControl.desiredState.yaw, updateMask); + if (hasLoiterRadius) { + navigationSetLoiterRadiusOverride((uint32_t)loiterRadius); + } *ret = MSP_RESULT_ACK; } break; @@ -4846,6 +4880,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu const uint16_t headingTarget = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(getHeadingHoldTarget()))); sbufWriteU16(dst, headingTarget); sbufWriteU16(dst, posControl.desiredState.climbRateDemand); + sbufWriteU32(dst, navigationGetLoiterRadiusOverride()); *ret = MSP_RESULT_ACK; } break; diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index b593bddcc6a..c69613b0bb1 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -42,6 +42,10 @@ static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT]; static bool isUsingNAVModes = false; boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e +static boxBitmask_t rcModeRawActivationMask; +static boxBitmask_t rcModeActivationOverrideMask; +static boxBitmask_t rcModeActivationOverrideSnapshot; +static bool rcModeActivationOverrideActive; // TODO(alberto): It looks like we can now safely remove this assert, since everything // but BB is able to handle more than 32 boxes and all the definitions use @@ -141,9 +145,84 @@ bool IS_RC_MODE_ACTIVE(boxId_e boxId) return bitArrayGet(rcModeActivationMask.bits, boxId); } +static boxBitmask_t rcModeFlightModeMask(const boxBitmask_t *source) +{ + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + + const boxId_e modeBoxes[] = { + BOXANGLE, + BOXHORIZON, + BOXNAVALTHOLD, + BOXHEADINGHOLD, + BOXNAVRTH, + BOXNAVPOSHOLD, + BOXMANUAL, + BOXNAVLAUNCH, + BOXNAVWP, + BOXTURNASSIST, + BOXSURFACE, + BOXNAVCOURSEHOLD, + BOXBRAKING, + BOXNAVCRUISE, + BOXSOARING, + BOXANGLEHOLD, + }; + + for (unsigned i = 0; i < ARRAYLEN(modeBoxes); i++) { + if (bitArrayGet(source->bits, modeBoxes[i])) { + bitArraySet(mask.bits, modeBoxes[i]); + } + } + + return mask; +} + +static void rcModeUpdateEffectiveActivationMask(void) +{ + rcModeActivationMask = rcModeRawActivationMask; + + if (rcModeActivationOverrideActive && !ARMING_FLAG(ARMED)) { + BITARRAY_CLR_ALL(rcModeActivationOverrideMask.bits); + rcModeActivationOverrideActive = false; + } + + if (!rcModeActivationOverrideActive) { + return; + } + + const boxBitmask_t currentFlightModeMask = rcModeFlightModeMask(&rcModeRawActivationMask); + if (memcmp(¤tFlightModeMask, &rcModeActivationOverrideSnapshot, sizeof(currentFlightModeMask)) != 0) { + BITARRAY_CLR_ALL(rcModeActivationOverrideMask.bits); + rcModeActivationOverrideActive = false; + return; + } + + for (unsigned i = 0; i < ARRAYLEN(rcModeActivationMask.bits); i++) { + rcModeActivationMask.bits[i] |= rcModeActivationOverrideMask.bits[i]; + } +} + void rcModeUpdate(boxBitmask_t *newState) { - rcModeActivationMask = *newState; + rcModeRawActivationMask = *newState; + rcModeUpdateEffectiveActivationMask(); +} + +void rcModeSetActivationOverride(boxId_e boxId) +{ + rcModeActivationOverrideSnapshot = rcModeFlightModeMask(&rcModeRawActivationMask); + BITARRAY_CLR_ALL(rcModeActivationOverrideMask.bits); + bitArraySet(rcModeActivationOverrideMask.bits, boxId); + rcModeActivationOverrideActive = true; + rcModeUpdateEffectiveActivationMask(); +} + +void rcModeClearActivationOverride(boxId_e boxId) +{ + bitArrayClr(rcModeActivationOverrideMask.bits, boxId); + rcModeActivationOverrideActive = BITARRAY_FIND_FIRST_SET(rcModeActivationOverrideMask.bits, 0) >= 0; + rcModeUpdateEffectiveActivationMask(); } bool isModeActivationConditionPresent(boxId_e modeId) diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 48e4357a1a5..8dec6c61b70 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -136,6 +136,8 @@ PG_DECLARE(modeActivationOperatorConfig_t, modeActivationOperatorConfig); bool IS_RC_MODE_ACTIVE(boxId_e boxId); void rcModeUpdate(boxBitmask_t *newState); +void rcModeSetActivationOverride(boxId_e boxId); +void rcModeClearActivationOverride(boxId_e boxId); bool isModeActivationConditionPresent(boxId_e modeId); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 500140a3bbd..f9dab593580 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2623,6 +2623,8 @@ static bool osdDrawSingleElement(uint8_t item) p = "TURT"; else if (FLIGHT_MODE(NAV_RTH_MODE)) p = isWaypointMissionRTHActive() ? "WRTH" : "RTH "; + else if (isGCSValid()) + p = "GCSN"; else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE)) p = "LOTR"; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) diff --git a/src/main/mavlink/mavlink_command.c b/src/main/mavlink/mavlink_command.c index c4b4755ec5c..9a8f4ab8a60 100644 --- a/src/main/mavlink/mavlink_command.c +++ b/src/main/mavlink/mavlink_command.c @@ -58,13 +58,109 @@ static bool handleIncoming_COMMAND( float longitudeDeg, float altitudeMeters) { - UNUSED(param3); - if (!mavlinkIsLocalTarget(targetSystem, targetComponent)) { return false; } switch (command) { + case MAV_CMD_COMPONENT_ARM_DISARM: + if (param1 != 0.0f && param1 != 1.0f) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + mavlinkSendCommandAck(command, fcSetArmState(param1 == 1.0f) ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + if (!ARMING_FLAG(ARMED)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + mavlinkSendCommandAck(command, activateRTHMode() ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + case MAV_CMD_DO_SET_MODE: + { + uint32_t modeFlags; + uint32_t customMode; + if (!mavlinkCommandParamToUint32(param1, UINT8_MAX, &modeFlags) || + !mavlinkCommandParamToUint32(param2, UINT8_MAX, &customMode) || + (modeFlags & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) == 0) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + const bool fixedWing = mavlinkIsFixedWingVehicle(); + const uint8_t rthMode = fixedWing ? PLANE_MODE_RTL : COPTER_MODE_RTL; + const bool posHoldMode = fixedWing ? + (customMode == PLANE_MODE_LOITER) : + (customMode == COPTER_MODE_LOITER || customMode == COPTER_MODE_POSHOLD || customMode == COPTER_MODE_BRAKE); + + if (customMode != rthMode && !posHoldMode) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + if (!ARMING_FLAG(ARMED)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + if (customMode == rthMode) { + mavlinkSendCommandAck(command, activateRTHMode() ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + mavlinkSendCommandAck(command, activatePositionHoldMode() ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_NAV_LAND: + mavlinkSendCommandAck(command, activateForcedLanding() ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + case MAV_CMD_NAV_TAKEOFF: + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + case MAV_CMD_DO_SET_HOME: + { + if ((param1 != 0.0f && param1 != 1.0f) || + !mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + if (!navCanSetHome()) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + navWaypoint_t wp = {0}; + wp.action = NAV_WP_ACTION_WAYPOINT; + if (param1 == 1.0f) { + wp.lat = gpsSol.llh.lat; + wp.lon = gpsSol.llh.lon; + wp.alt = gpsSol.llh.alt - posControl.gpsOrigin.alt; + } else { + if (!isfinite(latitudeDeg) || latitudeDeg < -90.0f || latitudeDeg > 90.0f || + !isfinite(longitudeDeg) || longitudeDeg < -180.0f || longitudeDeg > 180.0f || + !isfinite(altitudeMeters) || + altitudeMeters < (float)INT32_MIN / 100.0f || + altitudeMeters > (float)INT32_MAX / 100.0f) { + mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); + return true; + } + wp.lat = (int32_t)lrintf(latitudeDeg * 1e7f); + wp.lon = (int32_t)lrintf(longitudeDeg * 1e7f); + wp.alt = (int32_t)lrintf(altitudeMeters * 100.0f); + if (mavlinkFrameUsesAbsoluteAltitude(frame)) { + wp.alt -= posControl.gpsOrigin.alt; + } + } + setWaypoint(0, &wp); + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + return true; + } case MAV_CMD_DO_REPOSITION: if (!mavlinkFrameIsSupported(frame, MAV_FRAME_SUPPORTED_GLOBAL | @@ -85,6 +181,16 @@ static bool handleIncoming_COMMAND( } if (isGCSValid()) { + if (isfinite(param3)) { + const float maxLoiterRadiusMeters = (float)(UINT32_MAX / 100U); + + if (param3 < 0.0f || param3 > maxLoiterRadiusMeters) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + navigationSetLoiterRadiusOverride((uint32_t)lrintf(METERS_TO_CENTIMETERS(param3))); + } + navWaypoint_t wp = {0}; wp.action = NAV_WP_ACTION_WAYPOINT; wp.lat = (int32_t)(latitudeDeg * 1e7f); @@ -117,6 +223,38 @@ static bool handleIncoming_COMMAND( mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); return true; } + case MAV_CMD_CONDITION_YAW: + { + if (!isfinite(param1) || fabsf(param1) > 360.0f || !isfinite(param3) || !isfinite(param4)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + if (!(navGetCurrentStateFlags() & NAV_CTL_YAW)) { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + return true; + } + + int32_t targetHeadingCd = wrap_36000((int32_t)lrintf(param1 * 100.0f)); + + if (param4 != 0.0f) { + const int32_t currentHeadingCd = STATE(AIRPLANE) ? posControl.actualState.cog : posControl.actualState.yaw; + const int32_t headingChangeCd = (int32_t)lrintf(fabsf(param1) * 100.0f); + + if (param3 < 0.0f) { + targetHeadingCd = wrap_36000(currentHeadingCd - headingChangeCd); + } else { + targetHeadingCd = wrap_36000(currentHeadingCd + headingChangeCd); + } + } + + updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(targetHeadingCd)); + posControl.desiredState.yaw = targetHeadingCd; + posControl.cruise.course = targetHeadingCd; + posControl.cruise.previousCourse = targetHeadingCd; + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + return true; + } case MAV_CMD_SET_MESSAGE_INTERVAL: { mavlinkPeriodicMessage_e periodicMessage; diff --git a/src/main/mavlink/mavlink_guided.c b/src/main/mavlink/mavlink_guided.c index 0658b70694f..1281ab7fee8 100644 --- a/src/main/mavlink/mavlink_guided.c +++ b/src/main/mavlink/mavlink_guided.c @@ -59,4 +59,101 @@ MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters #endif } +static bool mavlinkIsLocalTarget(uint8_t targetSystem, uint8_t targetComponent) +{ + if (targetSystem != 0 && targetSystem != mavSystemId) { + return false; + } + + if (targetComponent != 0 && targetComponent != mavComponentId) { + return false; + } + + return true; +} + +bool mavlinkHandleIncomingSetPositionTargetGlobalInt(void) +{ + mavlink_set_position_target_global_int_t msg; + mavlink_msg_set_position_target_global_int_decode(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + return false; + } + + uint8_t frame = msg.coordinate_frame; + if (!mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + return true; + } + + const uint16_t typeMask = msg.type_mask; + const bool xIgnored = (typeMask & POSITION_TARGET_TYPEMASK_X_IGNORE) != 0; + const bool yIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Y_IGNORE) != 0; + const bool zIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Z_IGNORE) != 0; + + // Altitude-only SET_POSITION_TARGET_GLOBAL_INT mirrors MAV_CMD_DO_CHANGE_ALTITUDE semantics. + if (xIgnored && yIgnored && !zIgnored) { + if (isGCSValid()) { + mavlinkSetAltitudeTargetFromFrame(frame, msg.alt); + } + return true; + } + + if (xIgnored || yIgnored) { + return true; + } + + if (isGCSValid()) { + navWaypoint_t wp = {0}; + wp.action = NAV_WP_ACTION_WAYPOINT; + wp.lat = msg.lat_int; + wp.lon = msg.lon_int; + wp.alt = zIgnored ? 0 : (int32_t)(msg.alt * 100.0f); + wp.p1 = 0; + wp.p2 = 0; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + wp.flag = 0; + + setWaypoint(255, &wp); + } + + return true; +} + +bool mavlinkHandleIncomingSetPositionTargetLocalNed(void) +{ + mavlink_set_position_target_local_ned_t msg; + mavlink_msg_set_position_target_local_ned_decode(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + return false; + } + + if (msg.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) { + return true; + } + + const uint16_t typeMask = msg.type_mask; + const bool xIgnored = (typeMask & POSITION_TARGET_TYPEMASK_X_IGNORE) != 0; + const bool yIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Y_IGNORE) != 0; + const bool zIgnored = (typeMask & POSITION_TARGET_TYPEMASK_Z_IGNORE) != 0; + + if (!isGCSValid() || zIgnored) { + return true; + } + + if ((!xIgnored && fabsf(msg.x) > 0.01f) || (!yIgnored && fabsf(msg.y) > 0.01f)) { + return true; + } + + const int32_t targetAltitudeCm = (int32_t)lrintf((float)getEstimatedActualPosition(Z) - (msg.z * 100.0f)); + navigationSetAltitudeTargetWithDatum(NAV_WP_TAKEOFF_DATUM, targetAltitudeCm); + + return true; +} + #endif diff --git a/src/main/mavlink/mavlink_internal.h b/src/main/mavlink/mavlink_internal.h index 8dae16494da..ab6e00116c3 100644 --- a/src/main/mavlink/mavlink_internal.h +++ b/src/main/mavlink/mavlink_internal.h @@ -74,6 +74,9 @@ #include "scheduler/scheduler.h" +#define MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP 0x8001 +#define MAVLINK_TUNNEL_MSP_TIMEOUT_MS 1000 +#define MAVLINK_TUNNEL_MSP_FRAMEBUF_SIZE (MSP_PORT_OUTBUF_SIZE + 16) #define MAVLINK_MISSION_UPLOAD_RETRY_MS 1500 #define MAVLINK_MISSION_UPLOAD_MAX_RETRIES 5 #define MAVLINK_MISSION_DOWNLOAD_TIMEOUT_MS 5000 @@ -85,6 +88,9 @@ typedef struct mavlinkContext_s { uint8_t portCount; mavlinkRouteEntry_t routeTable[MAVLINK_MAX_ROUTES]; uint8_t routeCount; + mspPort_t tunnelMspPorts[MAX_MAVLINK_PORTS]; + uint8_t tunnelRemoteSystemIds[MAX_MAVLINK_PORTS]; + uint8_t tunnelRemoteComponentIds[MAX_MAVLINK_PORTS]; uint8_t sendMask; mavlinkPortRuntime_t *activePort; const mavlinkTelemetryPortConfig_t *activeConfig; @@ -93,6 +99,8 @@ typedef struct mavlinkContext_s { uint8_t systemId; uint8_t autopilotType; uint8_t componentId; + uint8_t tunnelReplyPayloadBuf[MSP_PORT_OUTBUF_SIZE]; + uint8_t tunnelFrameBuf[MAVLINK_TUNNEL_MSP_FRAMEBUF_SIZE]; uint8_t recvPortIndex; mavlinkMissionTransfer_t missionTransfer; timeMs_t lastMissionCurrentMs; @@ -110,6 +118,9 @@ extern mavlinkContext_t mavlinkContext; #define mavPortCount (mavlinkContext.portCount) #define mavRouteTable (mavlinkContext.routeTable) #define mavRouteCount (mavlinkContext.routeCount) +#define mavTunnelMspPorts (mavlinkContext.tunnelMspPorts) +#define mavTunnelRemoteSystemIds (mavlinkContext.tunnelRemoteSystemIds) +#define mavTunnelRemoteComponentIds (mavlinkContext.tunnelRemoteComponentIds) #define mavSendMask (mavlinkContext.sendMask) #define mavActivePort (mavlinkContext.activePort) #define mavActiveConfig (mavlinkContext.activeConfig) @@ -117,6 +128,8 @@ extern mavlinkContext_t mavlinkContext; #define mavSystemId (mavlinkContext.systemId) #define mavAutopilotType (mavlinkContext.autopilotType) #define mavComponentId (mavlinkContext.componentId) +#define mavTunnelReplyPayloadBuf (mavlinkContext.tunnelReplyPayloadBuf) +#define mavTunnelFrameBuf (mavlinkContext.tunnelFrameBuf) #define mavRecvPortIndex (mavlinkContext.recvPortIndex) #define mavMissionTransfer (mavlinkContext.missionTransfer) diff --git a/src/main/mavlink/mavlink_ports.c b/src/main/mavlink/mavlink_ports.c index f2c66fb1941..296e71cc6aa 100644 --- a/src/main/mavlink/mavlink_ports.c +++ b/src/main/mavlink/mavlink_ports.c @@ -22,13 +22,16 @@ static void resetMAVLinkPortRuntimeState(uint8_t portIndex) state->lastStatusTextSeverity = 0; state->firstStatusTextMs = 0; state->lastStatusTextMs = 0; - state->lastRemoteHeartbeatMs = 0; + state->lastArmingSnapshotMs = 0; memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue)); memset(state->mavMessageOverrideIntervalsUs, 0, sizeof(state->mavMessageOverrideIntervalsUs)); memset(state->mavMessageNextDue, 0, sizeof(state->mavMessageNextDue)); memset(&state->mavRecvStatus, 0, sizeof(state->mavRecvStatus)); memset(&state->mavRecvMsg, 0, sizeof(state->mavRecvMsg)); memset(&state->mlrs, 0, sizeof(state->mlrs)); + resetMspPort(&mavTunnelMspPorts[portIndex], NULL); + mavTunnelRemoteSystemIds[portIndex] = 0; + mavTunnelRemoteComponentIds[portIndex] = 0; } void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) diff --git a/src/main/mavlink/mavlink_routing.c b/src/main/mavlink/mavlink_routing.c index 09144962fe4..cf83bb0eb08 100644 --- a/src/main/mavlink/mavlink_routing.c +++ b/src/main/mavlink/mavlink_routing.c @@ -31,9 +31,23 @@ void mavlinkLearnRoute(uint8_t ingressPortIndex) mavRouteTable[mavRouteCount].sysid = mavlinkContext.recvMsg.sysid; mavRouteTable[mavRouteCount].compid = mavlinkContext.recvMsg.compid; mavRouteTable[mavRouteCount].ingressPortIndex = ingressPortIndex; + mavRouteTable[mavRouteCount].lastHeartbeatPortIndex = 0; + mavRouteTable[mavRouteCount].lastHeartbeatMs = 0; mavRouteCount++; } +mavlinkRouteEntry_t *mavlinkFindRoute(uint8_t sysid, uint8_t compid) +{ + for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) { + mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex]; + if (route->sysid == sysid && route->compid == compid) { + return route; + } + } + + return NULL; +} + void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent) { *targetSystem = -1; diff --git a/src/main/mavlink/mavlink_routing.h b/src/main/mavlink/mavlink_routing.h index ae63f372416..9809af875f4 100644 --- a/src/main/mavlink/mavlink_routing.h +++ b/src/main/mavlink/mavlink_routing.h @@ -7,6 +7,7 @@ bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid); void mavlinkLearnRoute(uint8_t ingressPortIndex); +mavlinkRouteEntry_t *mavlinkFindRoute(uint8_t sysid, uint8_t compid); void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent); void mavlinkForwardMessage(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent); int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t targetComponent, uint8_t ingressPortIndex); diff --git a/src/main/mavlink/mavlink_runtime.c b/src/main/mavlink/mavlink_runtime.c index 94551728ef4..6fce0de2422 100644 --- a/src/main/mavlink/mavlink_runtime.c +++ b/src/main/mavlink/mavlink_runtime.c @@ -126,7 +126,6 @@ void mavlinkRuntimeCheckState(void) configureMAVLinkTelemetryPort(portIndex); if (state->telemetryEnabled) { configureMAVLinkStreamRates(portIndex); - mavlinkPortReconnected(portIndex); } } else { freeMAVLinkTelemetryPortByIndex(portIndex); diff --git a/src/main/mavlink/mavlink_streams.c b/src/main/mavlink/mavlink_streams.c index b1fd5327d63..62d8ec287f2 100644 --- a/src/main/mavlink/mavlink_streams.c +++ b/src/main/mavlink/mavlink_streams.c @@ -3,6 +3,7 @@ #include "common/time.h" #include "mavlink/mavlink_modes.h" +#include "mavlink/mavlink_routing.h" #include "mavlink/mavlink_runtime.h" #include "mavlink/mavlink_streams.h" @@ -24,10 +25,15 @@ const uint8_t mavSecondaryRates[MAVLINK_STREAM_COUNT] = { #define MAVLINK_STATUS_TEXT_WARNING_REPEAT_MS 10000 #define MAVLINK_STATUS_TEXT_CRITICAL_REPEAT_MS 5000 -// GCS heartbeats nominally arrive at 1 Hz; a gap this long means the peer on -// this port went away and whatever comes back needs fresh one-shot state. +// GCS heartbeats nominally arrive at 1 Hz; a gap this long means the peer +// went away and needs fresh one-shot state when it returns. #define MAVLINK_HEARTBEAT_RECONNECT_GAP_MS 5000 +// Peers heartbeating slower than the gap threshold trigger a reconnect on +// every beat; this floor caps the arming-snapshot rate per port regardless +// of peer behavior. +#define MAVLINK_ARMING_SNAPSHOT_MIN_INTERVAL_MS 10000 + static const char * const mavlinkInavFlightModeNames[FLM_COUNT] = { [FLM_MANUAL] = "MANUAL", [FLM_ACRO] = "ACRO", @@ -1215,15 +1221,24 @@ bool mavlinkHandleIncomingHeartbeat(void) mavlink_heartbeat_t msg; mavlink_msg_heartbeat_decode(&mavlinkContext.recvMsg, &msg); - // A framed HEARTBEAT is the protocol's presence signal. First one ever on - // this port, or one arriving after a gap, means a peer just (re)connected. - mavlinkPortRuntime_t *ingressState = &mavPortStates[mavRecvPortIndex]; - const timeMs_t nowMs = millis(); - const bool firstHeartbeat = ingressState->lastRemoteHeartbeatMs == 0; - const bool heartbeatGap = nowMs - ingressState->lastRemoteHeartbeatMs >= MAVLINK_HEARTBEAT_RECONNECT_GAP_MS; - ingressState->lastRemoteHeartbeatMs = nowMs; - if (firstHeartbeat || heartbeatGap) { - mavlinkPortReconnected(mavRecvPortIndex); + // A framed HEARTBEAT is the protocol's presence signal. Track it per peer + // (route table entry) rather than per port, so a steady peer cannot mask a + // newly joining one behind the same port, and a peer moving to another + // port (failover) registers as a reconnect there. mavlinkLearnRoute() ran + // before dispatch, so the sender already has a route entry unless the + // table is full - in which case reconnect detection degrades gracefully + // to the pre-existing broadcast-only behavior for that peer. + mavlinkRouteEntry_t *route = mavlinkFindRoute(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid); + if (route) { + const timeMs_t nowMs = millis(); + const bool firstHeartbeat = route->lastHeartbeatMs == 0; + const bool heartbeatGap = nowMs - route->lastHeartbeatMs >= MAVLINK_HEARTBEAT_RECONNECT_GAP_MS; + const bool portChanged = !firstHeartbeat && route->lastHeartbeatPortIndex != mavRecvPortIndex; + route->lastHeartbeatMs = nowMs; + route->lastHeartbeatPortIndex = mavRecvPortIndex; + if (firstHeartbeat || heartbeatGap || portChanged) { + mavlinkPortReconnected(mavRecvPortIndex); + } } switch (msg.type) { @@ -1346,6 +1361,14 @@ void mavlinkSendArmingStatusTextToPort(uint8_t portIndex) return; } + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + const timeMs_t nowMs = millis(); + if (state->lastArmingSnapshotMs != 0 && + nowMs - state->lastArmingSnapshotMs < MAVLINK_ARMING_SNAPSHOT_MIN_INTERVAL_MS) { + return; + } + state->lastArmingSnapshotMs = nowMs; + char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; mavlinkBuildArmingDisabledText(text, sizeof(text), disableFlags); @@ -1366,8 +1389,8 @@ void mavlinkSendArmingStatusTextToPort(uint8_t portIndex) #endif } -// Fired when a port is judged to have just (re)connected: shared-port enable -// transition, or a remote HEARTBEAT after a gap on an always-open port. +// Fired when a peer is judged to have just (re)connected on this port: its +// first HEARTBEAT, one after a gap, or one after moving from another port. void mavlinkPortReconnected(uint8_t portIndex) { if (portIndex >= mavPortCount) { diff --git a/src/main/mavlink/mavlink_types.h b/src/main/mavlink/mavlink_types.h index fded8e6292f..213dc90c2b7 100644 --- a/src/main/mavlink/mavlink_types.h +++ b/src/main/mavlink/mavlink_types.h @@ -71,6 +71,11 @@ typedef struct mavlinkRouteEntry_s { uint8_t sysid; uint8_t compid; uint8_t ingressPortIndex; + // Reconnect detection state, updated only on HEARTBEAT from this peer. + // ingressPortIndex above is refreshed by every routed message, so it + // cannot be used to detect the port a heartbeat last arrived on. + uint8_t lastHeartbeatPortIndex; + timeMs_t lastHeartbeatMs; } mavlinkRouteEntry_t; typedef enum { @@ -145,7 +150,7 @@ typedef struct mavlinkPortRuntime_s { uint8_t lastStatusTextSeverity; timeMs_t firstStatusTextMs; timeMs_t lastStatusTextMs; - timeMs_t lastRemoteHeartbeatMs; + timeMs_t lastArmingSnapshotMs; uint8_t txSeq; uint32_t txDroppedFrames; mavlink_message_t mavRecvMsg; diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 180597c4527..0ea2cbaa939 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -146,5 +146,9 @@ #define MSP2_INAV_SET_WP_INDEX 0x2221 //in message jump to waypoint N during active WP mission; payload: U8 wp_index (0-based, relative to mission start) #define MSP2_INAV_SET_CRUISE_HEADING 0x2223 //in message set heading while in Cruise/Course Hold mode; payload: I32 heading_centidegrees (0-35999) +#define MSP2_INAV_ACTIVATE_LANDING 0x2224 //in message land at the current position through the normal waypoint LAND path; payload: none +#define MSP2_INAV_ACTIVATE_RTH 0x2225 //in message trigger normal return-to-home mode; payload: none +#define MSP2_INAV_ARM_DISARM 0x2227 //in message set armed state; payload: U8 arm +#define MSP2_INAV_TIMESYNC 0x2228 //out message local monotonic boot time in nanoseconds; payload: U64 time_ns -#define MSP2_INAV_SET_AUX_RC 0x2230 \ No newline at end of file +#define MSP2_INAV_SET_AUX_RC 0x2230 diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 32aac7a7bd5..02e36004c89 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -259,6 +259,11 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c) return true; } +bool mspSerialProcessReceivedByte(mspPort_t *mspPort, uint8_t c) +{ + return mspSerialProcessReceivedData(mspPort, c); +} + static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int len) { while (len-- > 0) { @@ -391,6 +396,98 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e msp return mspSerialSendFrame(msp, hdrBuf, hdrLen, sbufPtr(&packet->buf), dataLen, crcBuf, crcLen); } +int mspSerialEncodePacket(mspPacket_t *packet, mspVersion_e mspVersion, uint8_t *frameBuf, int frameBufSize) +{ + static const uint8_t mspMagic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER; + const int dataLen = sbufBytesRemaining(&packet->buf); + uint8_t hdrBuf[16] = { '$', mspMagic[mspVersion], packet->result == MSP_RESULT_ERROR ? '!' : '>'}; + uint8_t crcBuf[2]; + int hdrLen = 3; + int crcLen = 0; + + if (mspVersion == MSP_V1) { + mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen]; + hdrLen += sizeof(mspHeaderV1_t); + hdrV1->cmd = packet->cmd; + + if (dataLen >= JUMBO_FRAME_SIZE_LIMIT) { + mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen]; + hdrLen += sizeof(mspHeaderJUMBO_t); + + hdrV1->size = JUMBO_FRAME_SIZE_LIMIT; + hdrJUMBO->size = dataLen; + } + else { + hdrV1->size = dataLen; + } + + crcBuf[crcLen] = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS); + crcBuf[crcLen] = mspSerialChecksumBuf(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen); + crcLen++; + } + else if (mspVersion == MSP_V2_OVER_V1) { + mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen]; + + hdrLen += sizeof(mspHeaderV1_t); + + mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen]; + hdrLen += sizeof(mspHeaderV2_t); + + const int v1PayloadSize = sizeof(mspHeaderV2_t) + dataLen + 1; + hdrV1->cmd = MSP_V2_FRAME_ID; + + if (v1PayloadSize >= JUMBO_FRAME_SIZE_LIMIT) { + mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen]; + hdrLen += sizeof(mspHeaderJUMBO_t); + + hdrV1->size = JUMBO_FRAME_SIZE_LIMIT; + hdrJUMBO->size = v1PayloadSize; + } + else { + hdrV1->size = v1PayloadSize; + } + + hdrV2->flags = packet->flags; + hdrV2->cmd = packet->cmd; + hdrV2->size = dataLen; + + crcBuf[crcLen] = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t)); + crcBuf[crcLen] = crc8_dvb_s2_update(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen); + crcLen++; + + crcBuf[crcLen] = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS); + crcBuf[crcLen] = mspSerialChecksumBuf(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen); + crcBuf[crcLen] = mspSerialChecksumBuf(crcBuf[crcLen], crcBuf, crcLen); + crcLen++; + } + else if (mspVersion == MSP_V2_NATIVE) { + mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen]; + hdrLen += sizeof(mspHeaderV2_t); + + hdrV2->flags = packet->flags; + hdrV2->cmd = packet->cmd; + hdrV2->size = dataLen; + + crcBuf[crcLen] = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t)); + crcBuf[crcLen] = crc8_dvb_s2_update(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen); + crcLen++; + } + else { + return 0; + } + + const int totalFrameLength = hdrLen + dataLen + crcLen; + if (frameBufSize < totalFrameLength) { + return 0; + } + + memcpy(frameBuf, hdrBuf, hdrLen); + memcpy(frameBuf + hdrLen, sbufPtr(&packet->buf), dataLen); + memcpy(frameBuf + hdrLen + dataLen, crcBuf, crcLen); + + return totalFrameLength; +} + static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn) { uint8_t outBuf[MSP_PORT_OUTBUF_SIZE]; @@ -422,6 +519,20 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr return mspPostProcessFn; } +mspResult_e mspSerialProcessCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) +{ + mspPacket_t command = { + .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, }, + .cmd = msp->cmdMSP, + .flags = msp->cmdFlags, + .result = 0, + }; + + const mspResult_e status = mspProcessCommandFn(&command, reply, mspPostProcessFn); + msp->c_state = MSP_IDLE; + return status; +} + static void mspEvaluateNonMspData(mspPort_t * mspPort, uint8_t receivedChar) { if (receivedChar == '#') { diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 67487606da0..86fa6461217 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -101,6 +101,9 @@ typedef struct mspPort_s { void mspSerialInit(void); void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort); +bool mspSerialProcessReceivedByte(mspPort_t *mspPort, uint8_t c); +int mspSerialEncodePacket(mspPacket_t *packet, mspVersion_e mspVersion, uint8_t *frameBuf, int frameBufSize); +mspResult_e mspSerialProcessCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialAllocatePorts(void); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index f285d50e15d..31ae7b49ca6 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2174,10 +2174,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState); if (landEvent == NAV_FSM_EVENT_SUCCESS) { + const bool commandedLanding = posControl.flags.forcedLandingActivated; + posControl.flags.forcedLandingActivated = false; // Landing controller returned success - invoke RTH finish states and finish the waypoint. // Success maps straight to NAV_STATE_WAYPOINT_FINISHED, bypassing NAV_STATE_WAYPOINT_NEXT, - // so the LAND item must be marked reached here or it never reports completion. - navMarkWaypointReached(posControl.activeWaypointIndex); + // so the LAND item must be marked reached here or it never reports completion. A + // command-triggered landing borrows this state with a transient waypoint and must + // not credit an unrelated mission item. + if (!commandedLanding) { + navMarkWaypointReached(posControl.activeWaypointIndex); + } navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState); navOnEnteringState_NAV_STATE_RTH_FINISHED(previousState); } @@ -2579,11 +2585,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(nav { UNUSED(previousState); + const bool commandedLanding = posControl.flags.forcedLandingActivated; + posControl.flags.forcedLandingActivated = false; + // A mission LAND item handed off to the autoland FSM finishes here, not // through NAV_STATE_WAYPOINT_RTH_LAND's success branch - credit the item // or the mission never reports completion. landState guards re-entry - // (this state self-loops on timeout). - if (posControl.fwLandState.landWp && posControl.fwLandState.landState != FW_AUTOLAND_STATE_IDLE) { + // (this state self-loops on timeout). A command-triggered landing must + // not credit an unrelated mission item. + if (!commandedLanding && posControl.fwLandState.landWp && posControl.fwLandState.landState != FW_AUTOLAND_STATE_IDLE) { navMarkWaypointReached(posControl.activeWaypointIndex); } @@ -2598,6 +2608,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga posControl.fwLandState.landAborted = true; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE; + if (posControl.flags.forcedLandingActivated) { + posControl.flags.forcedLandingActivated = false; + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + return posControl.fwLandState.landWp ? NAV_FSM_EVENT_SWITCH_TO_WAYPOINT : NAV_FSM_EVENT_SWITCH_TO_RTH; } #endif @@ -2680,7 +2695,9 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) /* Process new injected event if event defined, * otherwise process timeout event if defined */ - if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) { + if (injectedEvent == NAV_FSM_EVENT_SWITCH_TO_LANDING) { + previousState = navSetNewFSMState(NAV_STATE_WAYPOINT_RTH_LAND); + } else if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) { /* Update state */ previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[injectedEvent]); } else if ((navFSM[posControl.navState].timeoutMs > 0) && (navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT] != NAV_STATE_UNDEFINED) && @@ -2735,7 +2752,9 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) NAV_Status.activeWpNumber = NAV_Status.activeWpIndex + 1; NAV_Status.activeWpAction = 0; - if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) { + if (posControl.flags.forcedLandingActivated) { + NAV_Status.activeWpAction = NAV_WP_ACTION_LAND; + } else if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) { NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action; } } @@ -2787,15 +2806,22 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) } break; - case RTH_HOME_FINAL_LAND: + case RTH_HOME_FINAL_LAND: { // if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation) - if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) { - posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100; // 100 -> m to cm - if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) { + const navWaypoint_t *landingWaypoint = NULL; + if (posControl.flags.forcedLandingActivated) { + landingWaypoint = &posControl.commandLandingWaypoint; + } else if (FLIGHT_MODE(NAV_WP_MODE)) { + landingWaypoint = &posControl.waypointList[posControl.activeWaypointIndex]; + } + if (landingWaypoint && landingWaypoint->action == NAV_WP_ACTION_LAND && landingWaypoint->p2 != 0) { + posControl.rthState.homeTmpWaypoint.z = landingWaypoint->p2 * 100; // 100 -> m to cm + if (waypointMissionAltConvMode(landingWaypoint->p3) == GEO_ALT_ABSOLUTE) { posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt; // correct to relative if absolute SL altitude datum used } } break; + } } return &posControl.rthState.homeTmpWaypoint; @@ -3852,6 +3878,26 @@ void resetGCSFlags(void) posControl.flags.isGCSAssistedNavigationEnabled = false; } +void navigationSetLoiterRadiusOverride(uint32_t loiterRadiusCm) +{ + posControl.gcsLoiterRadiusOverride = loiterRadiusCm; +} + +uint32_t navigationGetLoiterRadiusOverride(void) +{ + return posControl.gcsLoiterRadiusOverride; +} + +uint32_t navigationGetLoiterRadius(void) +{ + if (posControl.gcsLoiterRadiusOverride != 0 && + (posControl.navState == NAV_STATE_POSHOLD_3D_INITIALIZE || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { + return posControl.gcsLoiterRadiusOverride; + } + + return navConfig()->fw.loiter_radius; +} + void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData) { /* Default waypoint to send */ @@ -3916,6 +3962,14 @@ int isGCSValid(void) (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)); } +bool navCanSetHome(void) +{ + return ARMING_FLAG(ARMED) && + posControl.flags.estPosStatus >= EST_USABLE && + posControl.gpsOrigin.valid && + posControl.flags.isGCSAssistedNavigationEnabled; +} + /* * navSetActiveWaypointIndex - MSP2_INAV_SET_WP_INDEX handler * @@ -3990,7 +4044,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) wpLLH.alt = wpData->alt; // WP #0 - special waypoint - HOME - if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) { + if ((wpNumber == 0) && navCanSetHome()) { // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL); @@ -3999,7 +4053,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) // Only valid when armed and in poshold mode else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) && isGCSValid()) { // Convert to local coordinates - geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); + geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, waypointMissionAltConvMode(wpData->p3)); navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY; @@ -4117,6 +4171,7 @@ void loadSelectedMultiMission(uint8_t missionIndex) posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0; posControl.activeWaypointIndex = posControl.startWpIndex; + posControl.wpReachedNotificationPending = false; } bool updateWpMissionChange(void) @@ -4214,7 +4269,16 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded) bool saveNonVolatileWaypointList(void) { - if (ARMING_FLAG(ARMED) || !posControl.waypointListValid) + if (ARMING_FLAG(ARMED)) + return false; + + if (posControl.waypointCount == 0) { + memset(nonVolatileWaypointListMutable(0), 0, sizeof(navWaypoint_t) * NAV_MAX_WAYPOINTS); + saveConfigAndNotify(); + return true; + } + + if (!posControl.waypointListValid) return false; for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) { @@ -4416,8 +4480,10 @@ void applyWaypointNavigationAndAltitudeHold(void) if (!ARMING_FLAG(ARMED)) { // If we are disarmed, abort forced RTH or Emergency Landing posControl.flags.forcedRTHActivated = false; + posControl.flags.forcedLandingActivated = false; posControl.flags.forcedEmergLandingActivated = false; posControl.flags.manualEmergLandActive = false; + posControl.gcsLoiterRadiusOverride = 0; // ensure WP missions always restart from first waypoint after disarm posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback @@ -4628,6 +4694,17 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } + if (posControl.flags.forcedLandingActivated) { + if (posControl.navState == NAV_STATE_WAYPOINT_RTH_LAND +#ifdef USE_FW_AUTOLAND + || FLIGHT_MODE(NAV_FW_AUTOLAND) +#endif + ) { + return NAV_FSM_EVENT_NONE; + } + return NAV_FSM_EVENT_SWITCH_TO_LANDING; + } + #ifdef USE_GEOZONE if (posControl.flags.sendToActive) { return NAV_FSM_EVENT_SWITCH_TO_SEND_TO; @@ -5121,9 +5198,12 @@ void navigationInit(void) posControl.flags.forcedRTHActivated = false; posControl.flags.forcedEmergLandingActivated = false; + posControl.gcsLoiterRadiusOverride = 0; posControl.waypointCount = 0; posControl.activeWaypointIndex = 0; posControl.waypointListValid = false; + posControl.wpReachedSeq = 0; + posControl.wpReachedNotificationPending = false; posControl.wpPlannerActiveWPIndex = 0; posControl.flags.wpMissionPlannerActive = false; posControl.startWpIndex = 0; @@ -5184,9 +5264,57 @@ float getEstimatedActualPosition(int axis) /*----------------------------------------------------------- * Ability to execute RTH on external event *-----------------------------------------------------------*/ +bool activateRTHMode(void) +{ + if (!ARMING_FLAG(ARMED)) { + return false; + } + + abortFixedWingLaunch(); + posControl.flags.forcedLandingActivated = false; + rcModeSetActivationOverride(BOXNAVRTH); +#ifdef USE_SAFE_HOME + checkSafeHomeState(true); +#endif + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + + if (navGetStateFlags(posControl.navState) & NAV_AUTO_RTH) { + return true; + } + + rcModeClearActivationOverride(BOXNAVRTH); + return false; +} + +bool activatePositionHoldMode(void) +{ + if (!ARMING_FLAG(ARMED) || + posControl.flags.estPosStatus < EST_USABLE || + posControl.flags.estVelStatus < EST_TRUSTED || + posControl.flags.estAltStatus < EST_USABLE || + posControl.flags.estHeadingStatus < EST_USABLE) { + return false; + } + + abortFixedWingLaunch(); + posControl.flags.forcedRTHActivated = false; + posControl.flags.forcedLandingActivated = false; + rcModeSetActivationOverride(BOXNAVPOSHOLD); + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + + if (navGetStateFlags(posControl.navState) & NAV_CTL_HOLD) { + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return true; + } + + rcModeClearActivationOverride(BOXNAVPOSHOLD); + return false; +} + void activateForcedRTH(void) { abortFixedWingLaunch(); + posControl.flags.forcedLandingActivated = false; posControl.flags.forcedRTHActivated = true; #ifdef USE_SAFE_HOME checkSafeHomeState(true); @@ -5194,6 +5322,41 @@ void activateForcedRTH(void) navProcessFSMEvents(selectNavEventFromBoxModeInput()); } +bool activateForcedLanding(void) +{ + if (!ARMING_FLAG(ARMED) || + posControl.flags.estPosStatus < EST_USABLE || + posControl.flags.estAltStatus < EST_USABLE || + posControl.flags.estHeadingStatus < EST_USABLE) { + return false; + } + + if (posControl.flags.forcedLandingActivated) { + return true; + } + + abortFixedWingLaunch(); + posControl.flags.forcedRTHActivated = false; +#ifdef USE_SAFE_HOME + checkSafeHomeState(false); +#endif + posControl.flags.forcedLandingActivated = true; + posControl.commandLandingWaypoint = (navWaypoint_t) { + .action = NAV_WP_ACTION_LAND, + .alt = (int32_t)lrintf(posControl.actualState.abs.pos.z), + .flag = NAV_WP_FLAG_LAST, + }; + + resetPositionController(); + resetAltitudeController(false); + setupAltitudeController(); + calculateAndSetActiveWaypointToLocalPosition(&posControl.actualState.abs.pos); + resetLandingDetector(); + navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_LANDING); + + return (navGetStateFlags(posControl.navState) & NAV_CTL_LAND) || FLIGHT_MODE(NAV_FW_AUTOLAND); +} + void abortForcedRTH(void) { // Disable failsafe RTH and make sure we back out of navigation mode to IDLE diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b6cf4692b66..ba8aa9dd7a8 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -239,18 +239,18 @@ void abortForcedPosHold(void); #define NAV_ACCEL_CUTOFF_FREQUENCY_HZ 2 // low-pass filter on XY-acceleration target -enum { +typedef enum { NAV_GPS_ATTI = 0, // Pitch/roll stick controls attitude (pitch/roll lean angles) NAV_GPS_CRUISE = 1 // Pitch/roll stick controls velocity (forward/right speed) -}; +} nav_control_type_e; -enum { +typedef enum { NAV_LOITER_RIGHT = 0, // Loitering direction right NAV_LOITER_LEFT = 1, // Loitering direction left NAV_LOITER_YAW = 2 -}; +} nav_loiter_type_e; -enum { +typedef enum { NAV_RTH_NO_ALT = 0, // Maintain current altitude NAV_RTH_EXTRA_ALT = 1, // Maintain current altitude + predefined safety margin NAV_RTH_CONST_ALT = 2, // Climb/descend to predefined altitude @@ -258,18 +258,18 @@ enum { NAV_RTH_AT_LEAST_ALT = 4, // Climb to predefined altitude if below it NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT = 5, // Climb to predefined altitude if below it, // descend linearly to reach home at predefined altitude if above it -}; +} nav_rth_alt_profile_e; -enum { +typedef enum { NAV_RTH_CLIMB_STAGE_AT_LEAST = 0, // Will climb to the lesser of rth_climb_first_stage_altitude or rth_altitude, before turning NAV_RTH_CLIMB_STAGE_EXTRA = 1, // Will climb the lesser of rth_climb_first_stage_altitude above the current altitude or to nav_rth_altitude, before turning -}; +} nav_rth_climb_profile_e; -enum { +typedef enum { NAV_HEADING_CONTROL_NONE = 0, NAV_HEADING_CONTROL_AUTO, NAV_HEADING_CONTROL_MANUAL -}; +} nav_heading_control_e; typedef enum { NAV_RESET_NEVER = 0, @@ -712,6 +712,9 @@ bool isWaypointListValid(void); int isGCSValid(void); void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData); void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData); +void navigationSetLoiterRadiusOverride(uint32_t loiterRadiusCm); +uint32_t navigationGetLoiterRadiusOverride(void); +uint32_t navigationGetLoiterRadius(void); void resetWaypointList(void); bool navSetActiveWaypointIndex(uint8_t index); // MSP2_INAV_SET_WP_INDEX: jump to WP during active mission bool navSetCruiseHeading(int32_t headingCd); // MSP2_INAV_SET_CRUISE_HEADING: set cruise/course-hold heading (centidegrees) @@ -737,7 +740,8 @@ typedef enum { typedef enum { NAV_WP_TAKEOFF_DATUM, NAV_WP_MSL_DATUM, - NAV_WP_TERRAIN_DATUM + NAV_WP_TERRAIN_DATUM, + NAV_WP_RELATIVE_DATUM } geoAltitudeDatumFlag_e; // geoSetOrigin stores the location provided in llh as a GPS origin in the @@ -770,11 +774,20 @@ bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypoint bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); // NOT USED uint32_t distanceToFirstWP(void); +/* Commanded normal RTH mode */ +bool activateRTHMode(void); + +/* Commanded normal position hold mode */ +bool activatePositionHoldMode(void); + /* Failsafe-forced RTH mode */ void activateForcedRTH(void); void abortForcedRTH(void); rthState_e getStateOfForcedRTH(void); +/* Commanded normal landing mode */ +bool activateForcedLanding(void); + /* Failsafe-forced Emergency Landing mode */ void activateForcedEmergLanding(void); void abortForcedEmergLanding(void); @@ -788,6 +801,8 @@ bool navigationIsFlyingAutonomousMode(void); bool navigationIsExecutingAnEmergencyLanding(void); bool navigationIsControllingAltitude(void); bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm); +bool navigationConsumeWaypointReached(uint16_t *seq); +bool navCanSetHome(void); /* Returns true if navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active. */ @@ -807,7 +822,6 @@ bool isProbablyStillFlying(void); void resetLandingDetectorActiveState(void); const navigationPIDControllers_t* getNavigationPIDControllers(void); -bool navigationConsumeWaypointReached(uint16_t *seq); int32_t navigationGetHeadingError(void); float navigationGetCrossTrackError(void); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 1db29a37330..aec266d77d7 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -332,7 +332,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // Limit minimum forward velocity to 1 m/s float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f); - uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius); + uint32_t navLoiterRadius = getLoiterRadius(navigationGetLoiterRadius()); fpVector3_t loiterCenterPos = posControl.desiredState.pos; int8_t loiterTurnDirection = loiterDirection(); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 2e305289e83..303e5b91f53 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -104,6 +104,7 @@ typedef struct navigationFlags_s { // Failsafe actions bool forcedRTHActivated; + bool forcedLandingActivated; bool forcedEmergLandingActivated; /* Landing detector */ @@ -167,6 +168,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_MIXERAT, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING, NAV_FSM_EVENT_SWITCH_TO_SEND_TO, + NAV_FSM_EVENT_SWITCH_TO_LANDING, NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event @@ -463,6 +465,7 @@ typedef struct { /* Local system state, both actual (estimated) and desired (target setpoint)*/ navigationEstimatedState_t actualState; navigationDesiredState_t desiredState; // waypoint coordinates + velocity + uint32_t gcsLoiterRadiusOverride; // Temporary GCS PosHold loiter radius override, cm; 0 = configured default uint32_t lastValidPositionTimeMs; uint32_t lastValidAltitudeTimeMs; @@ -485,6 +488,7 @@ typedef struct { /* Waypoint list */ navWaypoint_t waypointList[NAV_MAX_WAYPOINTS]; + navWaypoint_t commandLandingWaypoint; bool waypointListValid; int8_t waypointCount; // number of WPs in loaded mission int8_t startWpIndex; // index of first waypoint in mission @@ -506,9 +510,9 @@ typedef struct { float wpInitialDistance; // Distance when starting flight to WP float wpDistance; // Distance to active WP timeMs_t wpReachedTime; // Time the waypoint was reached + bool wpAltitudeReached; // WP altitude achieved uint16_t wpReachedSeq; // Last reached mission item sequence relative to startWpIndex bool wpReachedNotificationPending; - bool wpAltitudeReached; // WP altitude achieved #ifdef USE_FW_AUTOLAND /* Fixedwing autoland */ diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 009b2d486f8..dd9a798544f 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -490,6 +490,14 @@ static int logicConditionCompute( return navigationSetAltitudeTargetWithDatum((geoAltitudeDatumFlag_e)operandA, operandB); break; + case LOGIC_CONDITION_ACTIVATE_RTH: + return activateRTHMode(); + break; + + case LOGIC_CONDITION_ACTIVATE_LANDING: + return activateForcedLanding(); + break; + case LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE: if (operandA >= 0 && operandA <= 2) { diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 517074fe18b..c13f9b7a8e7 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -91,6 +91,8 @@ typedef enum { LOGIC_CONDITION_ASIN = 59, LOGIC_CONDITION_ATAN2 = 60, LOGIC_CONDITION_DISABLE_AUTOSPEED_AIRSPEED = 61, + LOGIC_CONDITION_ACTIVATE_RTH = 62, + LOGIC_CONDITION_ACTIVATE_LANDING = 63, LOGIC_CONDITION_LAST } logicOperation_e; diff --git a/src/test/mavlink/tunnel/mavlink_msp_tunnel_smoketest.py b/src/test/mavlink/tunnel/mavlink_msp_tunnel_smoketest.py new file mode 100644 index 00000000000..e98b669b695 --- /dev/null +++ b/src/test/mavlink/tunnel/mavlink_msp_tunnel_smoketest.py @@ -0,0 +1,442 @@ +#!/usr/bin/env python3 +""" +Usage: + python src/test/mavlink/tunnel/mavlink_msp_tunnel_smoketest.py --mavlink-endpoint tcp:127.0.0.1:5761 + python src/test/mavlink/tunnel/mavlink_msp_tunnel_smoketest.py --mavlink-endpoint tcp:127.0.0.1:5761 --target-system 1 --post-reboot-timeout 20 +""" + +from __future__ import annotations + +import argparse +import os +import struct +import time +from dataclasses import dataclass +from typing import Iterable, List, Optional, Sequence + +os.environ["MAVLINK20"] = "1" + +from pymavlink import mavutil + + +MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP = 0x8001 +MAVLINK_CONFIGURATOR_SYSTEM_ID = 253 +MAVLINK_CONFIGURATOR_COMPONENT_ID = 25 +MAVLINK_TUNNEL_CHUNK_SIZE = 128 + +MSP_API_VERSION = 1 +MSP_FC_VARIANT = 2 +MSP_FC_VERSION = 3 +MSP_BUILD_INFO = 5 +MSP_EEPROM_WRITE = 250 +MSP_REBOOT = 68 + +MSP_DIRECTION_TO_FC = ord("<") +MSP_DIRECTION_FROM_FC = ord(">") +MSP_DIRECTION_ERROR = ord("!") +MSP_V1_JUMBO_PAYLOAD_MARKER = 255 + + +def msp_v1_checksum(data: bytes) -> int: + checksum = 0 + for byte in data: + checksum ^= byte + return checksum + + +def build_msp_v1_request(cmd: int, payload: bytes = b"") -> bytes: + payload_length = len(payload) + header = bytearray(b"$M<") + if payload_length >= MSP_V1_JUMBO_PAYLOAD_MARKER: + header.extend((MSP_V1_JUMBO_PAYLOAD_MARKER, cmd)) + header.extend(struct.pack(" None: + self.buffer = bytearray() + + def clear(self) -> None: + self.buffer.clear() + + def feed(self, data: bytes) -> List[MspV1Reply]: + self.buffer.extend(data) + replies: List[MspV1Reply] = [] + + while True: + frame_start = self.buffer.find(b"$M") + if frame_start < 0: + self.buffer.clear() + return replies + if frame_start > 0: + del self.buffer[:frame_start] + + if len(self.buffer) < 6: + return replies + + direction = self.buffer[2] + if direction not in (MSP_DIRECTION_FROM_FC, MSP_DIRECTION_ERROR): + del self.buffer[:2] + continue + + payload_length = self.buffer[3] + cmd = self.buffer[4] + frame_header_length = 5 + + if payload_length == MSP_V1_JUMBO_PAYLOAD_MARKER: + if len(self.buffer) < 8: + return replies + payload_length = struct.unpack_from(" str: + msp_protocol_version, api_major, api_minor = struct.unpack(" str: + variant = payload.rstrip(b"\x00").decode("ascii", errors="ignore") + return f"fc_variant={variant}" + + +def decode_fc_version(payload: bytes) -> str: + major, minor, patch = struct.unpack(" str: + build_date, build_time, git_revision = struct.unpack("<11s8s8s", payload) + build_date_text = build_date.rstrip(b" ").decode("ascii", errors="ignore") + build_time_text = build_time.rstrip(b" ").decode("ascii", errors="ignore") + git_revision_text = git_revision.rstrip(b"\x00").decode("ascii", errors="ignore") + return ( + f"build_date={build_date_text} " + f"build_time={build_time_text} " + f"git_revision={git_revision_text}" + ) + + +def split_frame(frame: bytes, chunk_sizes: Sequence[int]) -> List[bytes]: + chunks: List[bytes] = [] + offset = 0 + for chunk_size in chunk_sizes: + if offset >= len(frame): + break + next_offset = min(offset + chunk_size, len(frame)) + chunks.append(frame[offset:next_offset]) + offset = next_offset + if offset < len(frame): + chunks.extend(split_frame_default(frame[offset:])) + return chunks + + +def split_frame_default(frame: bytes) -> List[bytes]: + return [ + frame[offset:offset + MAVLINK_TUNNEL_CHUNK_SIZE] + for offset in range(0, len(frame), MAVLINK_TUNNEL_CHUNK_SIZE) + ] + + +def normalize_mavlink_endpoint(endpoint: str) -> str: + if endpoint.startswith("tcp://"): + return "tcp:" + endpoint[len("tcp://"):] + if endpoint.startswith("udp://"): + return "udp:" + endpoint[len("udp://"):] + if endpoint.startswith("udpin://"): + return "udpin:" + endpoint[len("udpin://"):] + if endpoint.startswith("udpout://"): + return "udpout:" + endpoint[len("udpout://"):] + return endpoint + + +class MavlinkMspTunnelSmokeTest: + def __init__(self, args: argparse.Namespace) -> None: + self.args = args + self.endpoint = normalize_mavlink_endpoint(args.mavlink_endpoint) + self.connection = mavutil.mavlink_connection( + self.endpoint, + source_system=args.source_system, + source_component=args.source_component, + dialect="common", + autoreconnect=True, + ) + self.connection.mav.srcSystem = args.source_system + self.connection.mav.srcComponent = args.source_component + self.target_system = 0 + self.target_component = args.target_component + self.parser = MspV1Parser() + self.next_heartbeat_at = time.monotonic() + self.last_target_heartbeat_at = 0.0 + + def close(self) -> None: + self.connection.close() + + def send_heartbeat(self) -> None: + self.connection.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + 0, + mavutil.mavlink.MAV_STATE_ACTIVE, + ) + + def maybe_send_heartbeat(self) -> None: + now = time.monotonic() + if now >= self.next_heartbeat_at: + self.send_heartbeat() + self.next_heartbeat_at = now + 1.0 / self.args.heartbeat_hz + + def learn_target_from_heartbeat(self, message: object) -> None: + if int(message.autopilot) == int(mavutil.mavlink.MAV_AUTOPILOT_INVALID): + return + + src_system = int(message.get_srcSystem()) + src_component = int(message.get_srcComponent()) + if self.args.target_system and src_system != self.args.target_system: + return + + self.last_target_heartbeat_at = time.monotonic() + + if self.target_system == 0: + self.target_system = src_system + if self.target_component == 0: + self.target_component = 0 + print( + f"target_discovered system_id={self.target_system} component_id={src_component} " + f"target_component_for_tunnel={self.target_component}", + flush=True, + ) + + def handle_incoming_message(self, message: object) -> List[MspV1Reply]: + message_type = message.get_type() + if message_type == "HEARTBEAT": + self.learn_target_from_heartbeat(message) + return [] + + if message_type != "TUNNEL": + return [] + + src_system = int(message.get_srcSystem()) + src_component = int(message.get_srcComponent()) + if self.target_system and src_system != self.target_system: + return [] + + if int(message.payload_type) != MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP: + return [] + + if int(message.target_system) != self.args.source_system: + return [] + + if int(message.target_component) != self.args.source_component: + return [] + + tunnel_bytes = bytes(message.payload[:int(message.payload_length)]) + print( + f"rx_tunnel src_system={src_system} src_component={src_component} payload_length={len(tunnel_bytes)}", + flush=True, + ) + return self.parser.feed(tunnel_bytes) + + def poll(self, deadline: float) -> List[MspV1Reply]: + replies: List[MspV1Reply] = [] + while time.monotonic() < deadline: + self.maybe_send_heartbeat() + message = self.connection.recv_match(blocking=False) + if message is None: + time.sleep(0.01) + continue + replies.extend(self.handle_incoming_message(message)) + if replies: + return replies + return replies + + def wait_for_target(self, timeout_s: float) -> None: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + self.maybe_send_heartbeat() + message = self.connection.recv_match(type="HEARTBEAT", blocking=False) + if message is None: + time.sleep(0.01) + continue + self.learn_target_from_heartbeat(message) + if self.target_system != 0: + return + raise TimeoutError( + f"mavlink_endpoint={self.endpoint} requested_target_system={self.args.target_system} timeout_s={timeout_s}" + ) + + def drain(self, duration_s: float) -> None: + deadline = time.monotonic() + duration_s + while time.monotonic() < deadline: + self.maybe_send_heartbeat() + message = self.connection.recv_match(blocking=False) + if message is None: + time.sleep(0.01) + continue + self.handle_incoming_message(message) + + def send_tunnel_chunks(self, chunks: Iterable[bytes]) -> None: + for chunk in chunks: + if len(chunk) > MAVLINK_TUNNEL_CHUNK_SIZE: + raise ValueError(f"chunk_length={len(chunk)} exceeds tunnel_limit={MAVLINK_TUNNEL_CHUNK_SIZE}") + print( + f"tx_tunnel target_system={self.target_system} target_component={self.target_component} payload_length={len(chunk)}", + flush=True, + ) + padded_chunk = bytes(chunk) + bytes(MAVLINK_TUNNEL_CHUNK_SIZE - len(chunk)) + self.connection.mav.tunnel_send( + self.target_system, + self.target_component, + MAVLINK_TUNNEL_PAYLOAD_TYPE_INAV_MSP, + len(chunk), + padded_chunk, + ) + + def request(self, cmd: int, payload: bytes, timeout_s: float, split_chunks: Optional[Sequence[int]] = None) -> MspV1Reply: + self.parser.clear() + self.drain(0.1) + frame = build_msp_v1_request(cmd, payload) + chunks = split_frame(frame, split_chunks) if split_chunks else split_frame_default(frame) + print( + f"tx_msp cmd={cmd} msp_length={len(frame)} tunnel_chunks={len(chunks)} timeout_s={timeout_s}", + flush=True, + ) + self.send_tunnel_chunks(chunks) + + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + replies = self.poll(deadline) + for reply in replies: + print( + f"rx_msp cmd={reply.cmd} payload_length={len(reply.payload)} is_error={int(reply.is_error)}", + flush=True, + ) + if reply.cmd == cmd: + return reply + raise TimeoutError(f"cmd={cmd} timeout_s={timeout_s}") + + def wait_for_reboot_reconnect(self, timeout_s: float) -> None: + self.parser.clear() + self.drain(0.2) + deadline = time.monotonic() + timeout_s + last_seen = self.last_target_heartbeat_at + while time.monotonic() < deadline: + self.maybe_send_heartbeat() + message = self.connection.recv_match(type="HEARTBEAT", blocking=False) + if message is None: + time.sleep(0.05) + continue + self.learn_target_from_heartbeat(message) + if self.last_target_heartbeat_at > last_seen: + print( + f"post_reboot_heartbeat system_id={self.target_system} last_target_heartbeat_at={self.last_target_heartbeat_at:.3f}", + flush=True, + ) + return + raise TimeoutError(f"post_reboot_timeout_s={timeout_s} target_system={self.target_system}") + + +def verify_reply_ok(reply: MspV1Reply, cmd: int) -> None: + if reply.cmd != cmd: + raise ValueError(f"expected_cmd={cmd} actual_cmd={reply.cmd}") + if reply.is_error: + raise RuntimeError(f"cmd={cmd} returned_error=1") + + +def main() -> None: + parser = argparse.ArgumentParser( + description="Headless MSP-over-MAVLink tunnel smoke test with configurator-like heartbeat, save, and reboot flow." + ) + parser.add_argument("--mavlink-endpoint", required=True, help='Existing SITL or FC MAVLink listener, e.g. "tcp:127.0.0.1:5761"') + parser.add_argument("--source-system", type=int, default=MAVLINK_CONFIGURATOR_SYSTEM_ID, help="MAVLink source system ID") + parser.add_argument("--source-component", type=int, default=MAVLINK_CONFIGURATOR_COMPONENT_ID, help="MAVLink source component ID") + parser.add_argument("--target-system", type=int, default=0, help="Expected FC system ID, 0 accepts the first autopilot heartbeat") + parser.add_argument("--target-component", type=int, default=0, help="Tunnel target component, 0 matches configurator behavior") + parser.add_argument("--connect-timeout", type=float, default=10.0, help="Seconds to wait for FC heartbeat discovery") + parser.add_argument("--request-timeout", type=float, default=2.0, help="Seconds to wait for normal MSP replies") + parser.add_argument("--save-timeout", type=float, default=5.0, help="Seconds to wait for MSP_EEPROM_WRITE reply") + parser.add_argument("--reboot-timeout", type=float, default=5.0, help="Seconds to wait for MSP_REBOOT reply") + parser.add_argument("--post-reboot-timeout", type=float, default=15.0, help="Seconds to wait for heartbeat and tunnel recovery after reboot") + parser.add_argument("--heartbeat-hz", type=float, default=1.0, help="Outbound GCS heartbeat rate") + args = parser.parse_args() + + tester = MavlinkMspTunnelSmokeTest(args) + try: + print( + f"connect mavlink_endpoint={tester.endpoint} source_system={args.source_system} source_component={args.source_component} " + f"target_system={args.target_system} target_component={args.target_component}", + flush=True, + ) + tester.wait_for_target(args.connect_timeout) + + api_reply = tester.request(MSP_API_VERSION, b"", args.request_timeout, split_chunks=(2,)) + verify_reply_ok(api_reply, MSP_API_VERSION) + print(decode_api_version(api_reply.payload), flush=True) + + fc_variant_reply = tester.request(MSP_FC_VARIANT, b"", args.request_timeout) + verify_reply_ok(fc_variant_reply, MSP_FC_VARIANT) + print(decode_fc_variant(fc_variant_reply.payload), flush=True) + + fc_version_reply = tester.request(MSP_FC_VERSION, b"", args.request_timeout) + verify_reply_ok(fc_version_reply, MSP_FC_VERSION) + print(decode_fc_version(fc_version_reply.payload), flush=True) + + build_info_reply = tester.request(MSP_BUILD_INFO, b"", args.request_timeout) + verify_reply_ok(build_info_reply, MSP_BUILD_INFO) + print(decode_build_info(build_info_reply.payload), flush=True) + + save_reply = tester.request(MSP_EEPROM_WRITE, b"", args.save_timeout) + verify_reply_ok(save_reply, MSP_EEPROM_WRITE) + print("save_ok cmd=250", flush=True) + + reboot_reply = tester.request(MSP_REBOOT, b"", args.reboot_timeout) + verify_reply_ok(reboot_reply, MSP_REBOOT) + print("reboot_ack cmd=68", flush=True) + + tester.wait_for_reboot_reconnect(args.post_reboot_timeout) + post_reboot_api_reply = tester.request(MSP_API_VERSION, b"", args.request_timeout) + verify_reply_ok(post_reboot_api_reply, MSP_API_VERSION) + print("post_reboot_tunnel_ok cmd=1", flush=True) + print(decode_api_version(post_reboot_api_reply.payload), flush=True) + finally: + tester.close() + + +if __name__ == "__main__": + main() diff --git a/src/test/mavlink/tunnel/results/.gitignore b/src/test/mavlink/tunnel/results/.gitignore new file mode 100644 index 00000000000..d6b7ef32c84 --- /dev/null +++ b/src/test/mavlink/tunnel/results/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc index df3fd1b9923..fe7aeae77bf 100644 --- a/src/test/unit/mavlink_unittest.cc +++ b/src/test/unit/mavlink_unittest.cc @@ -180,6 +180,26 @@ static std::vector makeMspV1Request(uint8_t cmd, const std::vector encodeMspV1Reply(uint8_t cmd, int16_t result, const std::vector &payload = {}) +{ + uint8_t payloadBuf[MSP_PORT_OUTBUF_SIZE]; + mspPacket_t reply = { + .buf = { .ptr = payloadBuf, .end = ARRAYEND(payloadBuf), }, + .cmd = cmd, + .flags = 0, + .result = result, + }; + uint8_t *payloadHead = reply.buf.ptr; + if (!payload.empty()) { + sbufWriteData(&reply.buf, payload.data(), (int)payload.size()); + } + sbufSwitchToReader(&reply.buf, payloadHead); + + uint8_t frameBuf[testMspFrameBufSize]; + const int frameLength = mspSerialEncodePacket(&reply, MSP_V1, frameBuf, sizeof(frameBuf)); + return std::vector(frameBuf, frameBuf + frameLength); +} + static void pushRxMessage(const mavlink_message_t *msg) { uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; @@ -358,10 +378,98 @@ static void initMavlinkTestState(void) checkMAVLinkTelemetryState(); } +TEST(MavlinkTelemetryTest, TunnelMalformedPayloadLengthIsDroppedAndDoesNotPoisonState) +{ + initMavlinkTestState(); + + std::vector request = makeMspV1Request(testSimpleMspCommand); + request.resize(MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN, 0); + + pushTunnelPayload(MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN + 1, request); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 0); + EXPECT_TRUE(parseTxMessages().empty()); + + resetSerialBuffers(); + pushTunnelPayload((uint8_t)makeMspV1Request(testSimpleMspCommand).size(), makeMspV1Request(testSimpleMspCommand)); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 1); + EXPECT_EQ(collectTunnelPayload(parseTxMessages()), encodeMspV1Reply(testSimpleMspCommand, MSP_RESULT_ACK)); +} + +TEST(MavlinkTelemetryTest, TunnelRejectsPassthroughBeforeDispatch) +{ + initMavlinkTestState(); + const std::vector request = makeMspV1Request((uint8_t)MSP_SET_PASSTHROUGH); + pushTunnelPayload((uint8_t)request.size(), request); + handleMAVLinkTelemetry(1000); + EXPECT_EQ(mspCommandCallCount, 0); + EXPECT_EQ(mspPassthroughDispatchCount, 0); + EXPECT_EQ(waitForSerialPortToFinishTransmittingCalls, 0); + EXPECT_EQ(mspRebootPostProcessCount, 0); + EXPECT_EQ(collectTunnelPayload(parseTxMessages()), encodeMspV1Reply((uint8_t)MSP_SET_PASSTHROUGH, MSP_RESULT_ERROR)); +} +TEST(MavlinkTelemetryTest, TunnelRebootUsesIngressPortForPostProcess) +{ + initMavlinkTestState(); + + const std::vector request = makeMspV1Request((uint8_t)MSP_REBOOT); + + pushTunnelPayload((uint8_t)request.size(), request); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 1); + EXPECT_EQ(waitForSerialPortToFinishTransmittingCalls, 1); + EXPECT_EQ(mspRebootPostProcessCount, 1); + EXPECT_EQ(lastPostProcessPort, &testSerialPort); + EXPECT_EQ(collectTunnelPayload(parseTxMessages()), encodeMspV1Reply((uint8_t)MSP_REBOOT, MSP_RESULT_ACK)); +} + +TEST(MavlinkTelemetryTest, TunnelStalePartialFrameResetsBeforeNextRequest) +{ + initMavlinkTestState(); + + pushTunnelPayload(3, {'$', 'M', '<'}); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 0); + EXPECT_TRUE(parseTxMessages().empty()); + + resetSerialBuffers(); + fakeMillis = 2000; + + const std::vector request = makeMspV1Request(testSimpleMspCommand); + pushTunnelPayload((uint8_t)request.size(), request); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mspCommandCallCount, 1); + EXPECT_EQ(collectTunnelPayload(parseTxMessages()), encodeMspV1Reply(testSimpleMspCommand, MSP_RESULT_ACK)); +} + +TEST(MavlinkTelemetryTest, TunnelLargeReplyFragmentsAcrossMultipleMessages) +{ + initMavlinkTestState(); + + const std::vector request = makeMspV1Request(testLargeReplyMspCommand); + pushTunnelPayload((uint8_t)request.size(), request); + handleMAVLinkTelemetry(1000); + + const std::vector messages = parseTxMessages(); + ASSERT_EQ(messages.size(), 3U); + + std::vector expectedPayload(300); + for (size_t i = 0; i < expectedPayload.size(); i++) { + expectedPayload[i] = (uint8_t)i; + } + + EXPECT_EQ(collectTunnelPayload(messages), encodeMspV1Reply(testLargeReplyMspCommand, MSP_RESULT_ACK, expectedPayload)); +} TEST(MavlinkTelemetryTest, MlrsRadioLinkStatsUpdateRxStatisticsOnMavlinkSerialRxPort) { @@ -643,17 +751,323 @@ TEST(MavlinkTelemetryTest, CommandLongRepositionUsesGlobalFrameAndParams) EXPECT_EQ(lastWaypoint.p1, 123); } +TEST(MavlinkTelemetryTest, ComponentArmDisarmUsesFcArmingPath) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_COMPONENT_ARM_DISARM, + 0, + 1.0f, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_COMPONENT_ARM_DISARM); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(setArmStateCalls, 1); + EXPECT_TRUE(requestedArmState); +} + +TEST(MavlinkTelemetryTest, ReturnToLaunchRequiresArmedState) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_NAV_RETURN_TO_LAUNCH, + 0, + 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.result, MAV_RESULT_DENIED); + EXPECT_EQ(activateRthCalls, 0); +} + +TEST(MavlinkTelemetryTest, ReturnToLaunchUsesNormalRthModePath) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + forcedRthState = RTH_IN_PROGRESS; + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_NAV_RETURN_TO_LAUNCH, + 0, + 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(activateRthCalls, 1); +} + +TEST(MavlinkTelemetryTest, DoSetModeRtlRequiresArmedState) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_MODE, + 0, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PLANE_MODE_RTL, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_SET_MODE); + EXPECT_EQ(ack.result, MAV_RESULT_DENIED); + EXPECT_EQ(activateRthCalls, 0); +} + +TEST(MavlinkTelemetryTest, DoSetModeRtlUsesNormalRthModePath) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + forcedRthState = RTH_IN_PROGRESS; + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_MODE, + 0, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PLANE_MODE_RTL, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_SET_MODE); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(activateRthCalls, 1); +} + +TEST(MavlinkTelemetryTest, DoSetModeCopterRtlUsesNormalRthModePath) +{ + initMavlinkTestState(); + mixerProfilesMutable(0)->mixer_config.platformType = PLATFORM_MULTIROTOR; + ENABLE_ARMING_FLAG(ARMED); + forcedRthState = RTH_IN_PROGRESS; + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_MODE, + 0, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, COPTER_MODE_RTL, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_SET_MODE); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(activateRthCalls, 1); +} + +TEST(MavlinkTelemetryTest, DoSetModeLoiterUsesNormalPositionHoldModePath) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_MODE, + 0, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PLANE_MODE_LOITER, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_SET_MODE); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(activateRthCalls, 0); + EXPECT_EQ(activatePositionHoldCalls, 1); +} + +TEST(MavlinkTelemetryTest, DoSetModeUnsupportedModeStaysUnsupported) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_MODE, + 0, + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, PLANE_MODE_CRUISE, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_SET_MODE); + EXPECT_EQ(ack.result, MAV_RESULT_UNSUPPORTED); + EXPECT_EQ(activateRthCalls, 0); + EXPECT_EQ(activatePositionHoldCalls, 0); +} + +TEST(MavlinkTelemetryTest, LandUsesNormalForcedLandingPath) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_NAV_LAND, + 0, + 0, 0, 0, 0, 37.5f, -122.25f, 12.3f); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(activateLandingCalls, 1); +} + +TEST(MavlinkTelemetryTest, TakeoffFoundationReturnsUnsupported) +{ + initMavlinkTestState(); + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_NAV_TAKEOFF, + 0, + 0, 0, 0, 0, 0, 0, 0); + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + EXPECT_EQ(ack.result, MAV_RESULT_UNSUPPORTED); +} +TEST(MavlinkTelemetryTest, SetHomeUsesWaypointZeroAndConvertsAbsoluteAltitude) +{ + initMavlinkTestState(); + posControl.gpsOrigin.alt = 1000; + mavlink_message_t cmd; + mavlink_msg_command_int_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_FRAME_GLOBAL_INT, + MAV_CMD_DO_SET_HOME, + 0, 0, + 0, 0, 0, 0, + 375000000, -1222500000, 34.56f); + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(lastWaypointNumber, 0); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_NEAR((double)lastWaypoint.lon, -1222500000.0, 100.0); + EXPECT_EQ(lastWaypoint.alt, 2456); +} +TEST(MavlinkTelemetryTest, SetHomeCurrentPositionUsesGpsLocation) +{ + initMavlinkTestState(); + posControl.gpsOrigin.alt = 1000; + gpsSol.llh.lat = 375000000; + gpsSol.llh.lon = -1222500000; + gpsSol.llh.alt = 3456; + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_SET_HOME, + 0, + 1.0f, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(lastWaypointNumber, 0); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_EQ(lastWaypoint.lon, -1222500000); + EXPECT_EQ(lastWaypoint.alt, 2456); +} TEST(MavlinkTelemetryTest, BroadcastCommandLongRepositionExecutesLocally) { @@ -772,6 +1186,34 @@ TEST(MavlinkTelemetryTest, CommandIntRepositionScalesCoordinates) EXPECT_EQ(lastWaypoint.p1, 45); } +TEST(MavlinkTelemetryTest, CommandIntRepositionSetsLoiterRadiusOverride) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_int_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_DO_REPOSITION, + 0, 0, + 0, 0, 42.5f, 45.6f, + 375000000, -1222500000, 12.3f); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_COMMAND_ACK); + + mavlink_command_ack_t ack; + mavlink_msg_command_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.command, MAV_CMD_DO_REPOSITION); + EXPECT_EQ(ack.result, MAV_RESULT_ACCEPTED); + EXPECT_EQ(lastLoiterRadiusOverride, 4250U); +} TEST(MavlinkTelemetryTest, MissionClearAllAcksAndResets) { @@ -1080,7 +1522,61 @@ TEST(MavlinkTelemetryTest, MissionItemIntNonFinalAutocontinueZeroIsRejected) EXPECT_EQ(ack.type, MAV_MISSION_UNSUPPORTED); } +TEST(MavlinkTelemetryTest, MissionItemIntGuidedWhileArmedUpdatesWaypoint) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t msg; + mavlink_msg_mission_item_int_pack( + 42, 200, &msg, + 1, testTargetComponent, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_NAV_WAYPOINT, 2, 1, + 0, 0, 0, 0, + 375000000, -1222500000, 12.3f, + MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_EQ(lastWaypoint.lon, -1222500000); + EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f)); + EXPECT_EQ(lastWaypoint.p3, 0); +} + +TEST(MavlinkTelemetryTest, MissionItemIntGuidedWhileArmedCurrentThreeChangesAltitude) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t msg; + mavlink_msg_mission_item_int_pack( + 42, 200, &msg, + 1, testTargetComponent, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_NAV_WAYPOINT, 3, 1, + 0, 0, 0, 0, + 375000000, -1222500000, 12.3f, + MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + mavlink_message_t ackMsg; + ASSERT_TRUE(popTxMessage(&ackMsg)); + ASSERT_EQ(ackMsg.msgid, MAVLINK_MSG_ID_MISSION_ACK); + + mavlink_mission_ack_t ack; + mavlink_msg_mission_ack_decode(&ackMsg, &ack); + + EXPECT_EQ(ack.type, MAV_MISSION_ACCEPTED); + EXPECT_EQ(altitudeTargetSetCalls, 1); + EXPECT_EQ(lastAltitudeTargetDatum, NAV_WP_TAKEOFF_DATUM); + EXPECT_EQ(lastAltitudeTargetCm, 1230); +} TEST(MavlinkTelemetryTest, MissionRequestListSendsCount) { @@ -1170,6 +1666,27 @@ TEST(MavlinkTelemetryTest, MissionItemReachedIsBroadcastOnceWhenPending) EXPECT_FALSE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &reachedMsg)); } +TEST(MavlinkTelemetryTest, LegacyGuidedMissionItemUsesAbsoluteAltitude) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t msg; + mavlink_msg_mission_item_pack( + 42, 200, &msg, + 1, testTargetComponent, 0, + MAV_FRAME_GLOBAL, + MAV_CMD_NAV_WAYPOINT, 2, 1, + 0, 0, 0, 0, + 37.5f, -122.25f, 12.3f, + MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE); +} TEST(MavlinkTelemetryTest, ParamRequestListRespondsWithEmptyParam) { @@ -1216,12 +1733,183 @@ TEST(MavlinkTelemetryTest, BroadcastParamRequestListRespondsWithEmptyParam) EXPECT_EQ(param.param_index, 0); } +TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntSetsWaypoint) +{ + initMavlinkTestState(); + mavlink_message_t msg; + mavlink_msg_set_position_target_global_int_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0, + 375000000, -1222500000, 12.3f, + 0, 0, 0, 0, 0, 0, 0, 0); + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_EQ(lastWaypoint.lon, -1222500000); + EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f)); + EXPECT_EQ(lastWaypoint.p3, 0); +} +TEST(MavlinkTelemetryTest, BroadcastSetPositionTargetGlobalIntSetsWaypoint) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_set_position_target_global_int_pack( + 42, 200, &msg, + 0, 0, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, 0, + 375000000, -1222500000, 12.3f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_EQ(lastWaypoint.lon, -1222500000); +} +TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntUsesAbsoluteAltitude) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_set_position_target_global_int_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_GLOBAL_INT, 0, + 375000000, -1222500000, 12.3f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE); +} + +TEST(MavlinkTelemetryTest, SetPositionTargetGlobalIntAltitudeOnlyRequiresValidGcs) +{ + initMavlinkTestState(); + gcsValid = false; + + mavlink_message_t msg; + mavlink_msg_set_position_target_global_int_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + POSITION_TARGET_TYPEMASK_X_IGNORE | + POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 0, 0, 12.3f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 0); +} + +TEST(MavlinkTelemetryTest, SetPositionTargetLocalNedAltitudeOnlySetsAltitudeTarget) +{ + initMavlinkTestState(); + estimatedPosition[Z] = 1000.0f; + + mavlink_message_t msg; + mavlink_msg_set_position_target_local_ned_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_LOCAL_OFFSET_NED, + POSITION_TARGET_TYPEMASK_X_IGNORE | + POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 0.0f, 0.0f, -2.5f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 1); + EXPECT_EQ(lastAltitudeTargetDatum, NAV_WP_TAKEOFF_DATUM); + EXPECT_EQ(lastAltitudeTargetCm, 1250); +} + +TEST(MavlinkTelemetryTest, BroadcastSetPositionTargetLocalNedAltitudeOnlySetsAltitudeTarget) +{ + initMavlinkTestState(); + estimatedPosition[Z] = 1000.0f; + + mavlink_message_t msg; + mavlink_msg_set_position_target_local_ned_pack( + 42, 200, &msg, + 0, 0, 0, + MAV_FRAME_LOCAL_OFFSET_NED, + POSITION_TARGET_TYPEMASK_X_IGNORE | + POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 0.0f, 0.0f, -2.5f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 1); + EXPECT_EQ(lastAltitudeTargetCm, 1250); +} + +TEST(MavlinkTelemetryTest, SetPositionTargetLocalNedIgnoresXyMotionRequests) +{ + initMavlinkTestState(); + estimatedPosition[Z] = 1000.0f; + + mavlink_message_t msg; + mavlink_msg_set_position_target_local_ned_pack( + 42, 200, &msg, + 0, 1, testTargetComponent, + MAV_FRAME_LOCAL_OFFSET_NED, + POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, + 1.0f, 0.0f, -2.5f, + 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMavlinkUntilRxEmpty(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 0); +} TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream) { diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc index 5b1151a5f7c..76e86bf25be 100644 --- a/src/test/unit/rcdevice_unittest.cc +++ b/src/test/unit/rcdevice_unittest.cc @@ -34,6 +34,7 @@ extern "C" { #include "fc/rc_controls.h" #include "fc/rc_modes.h" + #include "fc/runtime_config.h" @@ -124,6 +125,15 @@ static void resetRCDeviceStatus() clearResponseBuff(); } +static void resetRcModeActivationForTest() +{ + DISABLE_ARMING_FLAG(ARMED); + + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + rcModeUpdate(&mask); +} + static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlushSerial) { UNUSED(withDataForFlushSerial); @@ -132,6 +142,75 @@ static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlus testData.responseBufCount++; } +TEST(RCModeTest, ActivationOverrideSurvivesUnrelatedModeChange) +{ + resetRcModeActivationForTest(); + + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + bitArraySet(mask.bits, BOXANGLE); + rcModeUpdate(&mask); + + ENABLE_ARMING_FLAG(ARMED); + rcModeSetActivationOverride(BOXNAVRTH); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXANGLE)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + + bitArraySet(mask.bits, BOXCAMERA1); + rcModeUpdate(&mask); + + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXANGLE)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXCAMERA1)); + + resetRcModeActivationForTest(); +} + +TEST(RCModeTest, ActivationOverrideClearsOnFlightModeChange) +{ + resetRcModeActivationForTest(); + + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + bitArraySet(mask.bits, BOXANGLE); + rcModeUpdate(&mask); + + ENABLE_ARMING_FLAG(ARMED); + rcModeSetActivationOverride(BOXNAVRTH); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + + bitArrayClr(mask.bits, BOXANGLE); + bitArraySet(mask.bits, BOXHORIZON); + rcModeUpdate(&mask); + + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXHORIZON)); + + resetRcModeActivationForTest(); +} + +TEST(RCModeTest, ActivationOverrideClearsWhenDisarmed) +{ + resetRcModeActivationForTest(); + + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + bitArraySet(mask.bits, BOXANGLE); + rcModeUpdate(&mask); + + ENABLE_ARMING_FLAG(ARMED); + rcModeSetActivationOverride(BOXNAVRTH); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + + DISABLE_ARMING_FLAG(ARMED); + rcModeUpdate(&mask); + + EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXNAVRTH)); + EXPECT_TRUE(IS_RC_MODE_ACTIVE(BOXANGLE)); + + resetRcModeActivationForTest(); +} + TEST(RCDeviceTest, TestRCSplitInitWithoutPortConfigurated) { runcamDevice_t device; @@ -949,7 +1028,7 @@ extern "C" { uint32_t millis(void) { return testData.millis++; } timeUs_t micros(void) { return millis() * 1000; } void beeper(beeperMode_e mode) { UNUSED(mode); } - uint8_t armingFlags = 0; + uint32_t armingFlags = 0; bool cmsInMenu; uint32_t resumeRefreshAt = 0; int getArmingDisableFlags(void) {return 0;}