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..bcd5c9fa8bd 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]`. | +| 61 | 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. | +| 62 | 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 cbc526adb33..27a14478b0a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -959,6 +959,16 @@ Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, --- +### dual_rx_enabled + +Enable dual receiver (RX) handling. When OFF, only the primary receiver is used even if secondary settings are configured. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### dynamic_gyro_notch_3d_q Q factor for 3D dynamic notches @@ -2855,9 +2865,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 | | --- | --- | --- | @@ -2865,9 +2875,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 | | --- | --- | --- | @@ -2875,9 +2885,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 | | --- | --- | --- | @@ -2885,9 +2895,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 | | --- | --- | --- | @@ -2895,9 +2905,19 @@ Rate of the extra3 message for MAVLink telemetry --- -### mavlink_min_txbuffer +### mavlink_port1_high_latency + +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, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits. +Minimum percent of TX buffer space free for MAVLink port 1. Requires RADIO_STATUS messages. | Default | Min | Max | | --- | --- | --- | @@ -2905,9 +2925,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 | | --- | --- | --- | @@ -2915,21 +2935,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 | | --- | --- | --- | @@ -2937,6 +2958,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 @@ -6180,6 +6300,19 @@ Selection of receiver (RX) type. Additional configuration of a `serialrx_provide --- +### receiver_type_secondary + +Selection of the secondary receiver (RX) type for dual RX setups. Set to `NONE` to disable the secondary link. Additional configuration of `serialrx_provider_secondary` and a UART is needed for `SERIAL`. + +| Allowed Values | | +| --- | --- | +| NONE | Default | +| SERIAL | | +| MSP | | +| SIM (SITL) | | + +--- + ### report_cell_voltage S.Port and IBUS telemetry: Send the average cell voltage if set to ON @@ -6357,6 +6490,16 @@ SBUS sync interval in us. Default value is 3000us. Lower values may cause issues --- +### sbus_sync_interval_secondary + +SBUS sync interval in us for the secondary receiver. Default value is 3000us. Lower values may cause issues with some receivers. + +| Default | Min | Max | +| --- | --- | --- | +| 3000 | 500 | 10000 | + +--- + ### sdcard_detect_inverted This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. @@ -6379,6 +6522,18 @@ Allow serial receiver to operate on UART TX pin. With some receivers will allow --- +### serialrx_halfduplex_secondary + +Allow the secondary serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. + +| Allowed Values | | +| --- | --- | +| AUTO | Default | +| ON | | +| OFF | | + +--- + ### serialrx_inverted Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). @@ -6389,6 +6544,16 @@ Reverse the serial inversion of the serial RX protocol. When this value is OFF, --- +### serialrx_inverted_secondary + +Secondary serial RX inversion flag. Reverse the default inversion of the protocol for the secondary link. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### serialrx_provider When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. @@ -6414,6 +6579,31 @@ When feature SERIALRX is enabled, this allows connection to several receivers wh --- +### serialrx_provider_secondary + +Secondary UART-based receiver provider. Only used when `receiver_type_secondary` is `SERIAL`. + +| Allowed Values | | +| --- | --- | +| SPEK1024 | | +| SPEK2048 | | +| SBUS | | +| SUMD | | +| IBUS | | +| JETIEXBUS | | +| CRSF | | +| FPORT | | +| SBUS_FAST | | +| FPORT2 | | +| SRXL2 | | +| GHST | | +| MAVLINK | | +| FBUS | | +| SBUS2 | | +| _target default_ | Default | + +--- + ### servo_autotrim_rotation_limit Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index fafcf5d4762..006c4fd480f 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) @@ -3215,6 +3219,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: | `capacityWarning` | `uint32_t` | 4 | mAh/mWh | Capacity warning threshold (`currentBatteryProfile->capacity.warning`) | | `capacityCritical` | `uint32_t` | 4 | mAh/mWh | Capacity critical threshold (`currentBatteryProfile->capacity.critical`) | | `capacityUnit` | `uint8_t` | 1 | [batCapacityUnit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-batcapacityunit_e) | Enum `batCapacityUnit_e` Capacity unit (`batteryMetersConfig()->capacity_unit`) | +| `activeRX` | `uint8_t` | 1 | [rxLink_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-rxlink_e) | Enum `rxLink_e` Current active receiver link if dual RX activated; otherwise always 0 | ## `MSP2_INAV_SET_MISC (8196 / 0x2004)` **Description:** Sets miscellaneous configuration settings, superseding `MSP_SET_MISC`. @@ -4603,7 +4608,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 +4688,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 +4707,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 +4755,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..77df3089d02 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", @@ -1944,7 +1945,8 @@ "(1 << 11)", "USE_GPS_FIX_ESTIMATION" ], - "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED": "(1 << 12)" + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED": "(1 << 12)", + "LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RX_LINK": "(1 << 13)" }, "logicFlightModeOperands_e": { "_source": "inav/src/main/programming/logic_condition.h", @@ -2094,7 +2096,10 @@ "LOGIC_CONDITION_ACOS": "58", "LOGIC_CONDITION_ASIN": "59", "LOGIC_CONDITION_ATAN2": "60", - "LOGIC_CONDITION_LAST": "61" + "LOGIC_CONDITION_ACTIVATE_RTH": "61", + "LOGIC_CONDITION_ACTIVATE_LANDING": "62", + "LOGIC_CONDITION_RX_SELECT_OVERRIDE": "63", + "LOGIC_CONDITION_LAST": "64" }, "logicWaypointOperands_e": { "_source": "inav/src/main/programming/logic_condition.h", @@ -2155,6 +2160,7 @@ "LSM6DXX_VAL_CTRL1_XL_LPF2": "1", "LSM6DXX_VAL_CTRL2_G_ODR6664": "10", "LSM6DXX_VAL_CTRL2_G_2000DPS": "3", + "LSM6DXX_VAL_CTRL3_C_BDU": "BIT(6)", "LSM6DXX_VAL_CTRL3_C_H_LACTIVE": "0", "LSM6DXX_VAL_CTRL3_C_PP_OD": "0", "LSM6DXX_VAL_CTRL3_C_SIM": "0", @@ -2172,7 +2178,21 @@ "LSM6DXX_VAL_CTRL7_G_HPM_G_65": "1", "LSM6DXX_VAL_CTRL7_G_HPM_G_260": "2", "LSM6DXX_VAL_CTRL7_G_HPM_G_1040": "3", - "LSM6DXX_VAL_CTRL9_XL_I3C_DISABLE": "BIT(1)" + "LSM6DXX_VAL_CTRL9_XL_I3C_DISABLE": "BIT(1)", + "LSM6DXX_VAL_CTRL1_XL_V_OPMODE_HIGH_ACCURACY": "1", + "LSM6DXX_VAL_CTRL1_XL_V_ODR_1000HZ_HAODR1": "9", + "LSM6DXX_VAL_CTRL2_G_V_OPMODE_HIGH_ACCURACY": "1", + "LSM6DXX_VAL_CTRL2_G_V_ODR_8000HZ_HAODR1": "12", + "LSM6DXX_VAL_HAODR_CFG_MODE1": "1", + "LSM6DXX_VAL_CTRL6_C_V_FS_G_2000DPS": "4", + "LSM6DXX_VAL_CTRL6_C_V_DSK320X_RESERVED_BIT3": "BIT(3)", + "LSM6DXX_VAL_CTRL6_C_V_LPF1_BW_288HZ": "0", + "LSM6DXX_VAL_CTRL6_C_V_LPF1_BW_215HZ": "1", + "LSM6DXX_VAL_CTRL6_C_V_LPF1_BW_157HZ": "2", + "LSM6DXX_VAL_CTRL6_C_V_LPF1_BW_455HZ": "3", + "LSM6DXX_VAL_CTRL7_G_V_LPF1_EN": "BIT(0)", + "LSM6DXX_VAL_CTRL8_XL_V_FS_16G": "3", + "LSM6DXX_VAL_CTRL4_C_V_DRDY_PULSED": "BIT(1)" }, "lsm6dxxRegister_e": { "_source": "inav/src/main/drivers/accgyro/accgyro_lsm6dxx.h", @@ -2204,7 +2224,8 @@ "LSM6DXX_REG_OUTY_L_A": "42", "LSM6DXX_REG_OUTY_H_A": "43", "LSM6DXX_REG_OUTZ_L_A": "44", - "LSM6DXX_REG_OUTZ_H_A": "45" + "LSM6DXX_REG_OUTZ_H_A": "45", + "LSM6DXX_REG_HAODR_CFG": "98" }, "ltm_frame_e": { "_source": "inav/src/main/telemetry/ltm.h", @@ -2385,12 +2406,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 +2500,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", @@ -2943,7 +2996,8 @@ "OSD_NAV_MIN_GROUND_SPEED": "167", "OSD_THROTTLE_GAUGE": "168", "OSD_GPS_EXTRA_STATS": "169", - "OSD_ITEM_COUNT": "170" + "OSD_ACTIVE_RX_LINK": "170", + "OSD_ITEM_COUNT": "171" }, "osd_sidebar_arrow_e": { "_source": "inav/src/main/io/osd_grid.c", @@ -3456,6 +3510,12 @@ "RX_FRAME_PROCESSING_REQUIRED": "(1 << 2)", "RX_FRAME_DROPPED": "(1 << 3)" }, + "rxLink_e": { + "_source": "inav/src/main/rx/rx.h", + "RX_LINK_PRIMARY": "0", + "RX_LINK_SECONDARY": "1", + "RX_LINK_COUNT": "2" + }, "rxReceiverType_e": { "_source": "inav/src/main/rx/rx.h", "RX_TYPE_NONE": "0", @@ -3600,7 +3660,7 @@ "FUNCTION_RCDEVICE": "(1 << 10)", "FUNCTION_VTX_SMARTAUDIO": "(1 << 11)", "FUNCTION_VTX_TRAMP": "(1 << 12)", - "FUNCTION_UNUSED_1": "(1 << 13)", + "FUNCTION_RX_SERIAL_SECONDARY": "(1 << 13)", "FUNCTION_OPTICAL_FLOW": "(1 << 14)", "FUNCTION_LOG": "(1 << 15)", "FUNCTION_RANGEFINDER": "(1 << 16)", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index 275145df9f6..7e7037942cb 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) @@ -273,6 +278,7 @@ - [rthTargetMode_e](#enum-rthtargetmode_e) - [rthTrackbackMode_e](#enum-rthtrackbackmode_e) - [rxFrameState_e](#enum-rxframestate_e) +- [rxLink_e](#enum-rxlink_e) - [rxReceiverType_e](#enum-rxreceivertype_e) - [rxSerialReceiverType_e](#enum-rxserialreceivertype_e) - [safehomeUsageMode_e](#enum-safehomeusagemode_e) @@ -2246,6 +2252,7 @@ | `NAV_WP_TAKEOFF_DATUM` | 0 | | | `NAV_WP_MSL_DATUM` | 1 | | | `NAV_WP_TERRAIN_DATUM` | 2 | | +| `NAV_WP_RELATIVE_DATUM` | 3 | | --- ## `geoOriginResetMode_e` @@ -3031,6 +3038,7 @@ | `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS` | (1 << 10) | | | `LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX` | (1 << 11) | USE_GPS_FIX_ESTIMATION | | `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED` | (1 << 12) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RX_LINK` | (1 << 13) | | --- ## `logicFlightModeOperands_e` @@ -3200,7 +3208,10 @@ | `LOGIC_CONDITION_ACOS` | 58 | | | `LOGIC_CONDITION_ASIN` | 59 | | | `LOGIC_CONDITION_ATAN2` | 60 | | -| `LOGIC_CONDITION_LAST` | 61 | | +| `LOGIC_CONDITION_ACTIVATE_RTH` | 61 | | +| `LOGIC_CONDITION_ACTIVATE_LANDING` | 62 | | +| `LOGIC_CONDITION_RX_SELECT_OVERRIDE` | 63 | | +| `LOGIC_CONDITION_LAST` | 64 | | --- ## `logicWaypointOperands_e` @@ -3281,6 +3292,7 @@ | `LSM6DXX_VAL_CTRL1_XL_LPF2` | 1 | | | `LSM6DXX_VAL_CTRL2_G_ODR6664` | 10 | | | `LSM6DXX_VAL_CTRL2_G_2000DPS` | 3 | | +| `LSM6DXX_VAL_CTRL3_C_BDU` | BIT(6) | | | `LSM6DXX_VAL_CTRL3_C_H_LACTIVE` | 0 | | | `LSM6DXX_VAL_CTRL3_C_PP_OD` | 0 | | | `LSM6DXX_VAL_CTRL3_C_SIM` | 0 | | @@ -3299,6 +3311,20 @@ | `LSM6DXX_VAL_CTRL7_G_HPM_G_260` | 2 | | | `LSM6DXX_VAL_CTRL7_G_HPM_G_1040` | 3 | | | `LSM6DXX_VAL_CTRL9_XL_I3C_DISABLE` | BIT(1) | | +| `LSM6DXX_VAL_CTRL1_XL_V_OPMODE_HIGH_ACCURACY` | 1 | | +| `LSM6DXX_VAL_CTRL1_XL_V_ODR_1000HZ_HAODR1` | 9 | | +| `LSM6DXX_VAL_CTRL2_G_V_OPMODE_HIGH_ACCURACY` | 1 | | +| `LSM6DXX_VAL_CTRL2_G_V_ODR_8000HZ_HAODR1` | 12 | | +| `LSM6DXX_VAL_HAODR_CFG_MODE1` | 1 | | +| `LSM6DXX_VAL_CTRL6_C_V_FS_G_2000DPS` | 4 | | +| `LSM6DXX_VAL_CTRL6_C_V_DSK320X_RESERVED_BIT3` | BIT(3) | | +| `LSM6DXX_VAL_CTRL6_C_V_LPF1_BW_288HZ` | 0 | | +| `LSM6DXX_VAL_CTRL6_C_V_LPF1_BW_215HZ` | 1 | | +| `LSM6DXX_VAL_CTRL6_C_V_LPF1_BW_157HZ` | 2 | | +| `LSM6DXX_VAL_CTRL6_C_V_LPF1_BW_455HZ` | 3 | | +| `LSM6DXX_VAL_CTRL7_G_V_LPF1_EN` | BIT(0) | | +| `LSM6DXX_VAL_CTRL8_XL_V_FS_16G` | 3 | | +| `LSM6DXX_VAL_CTRL4_C_V_DRDY_PULSED` | BIT(1) | | --- ## `lsm6dxxRegister_e` @@ -3336,6 +3362,7 @@ | `LSM6DXX_REG_OUTY_H_A` | 43 | | | `LSM6DXX_REG_OUTZ_L_A` | 44 | | | `LSM6DXX_REG_OUTZ_H_A` | 45 | | +| `LSM6DXX_REG_HAODR_CFG` | 98 | | --- ## `ltm_frame_e` @@ -3607,6 +3634,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 +3677,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 +3793,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 | | @@ -4340,7 +4424,8 @@ | `OSD_NAV_MIN_GROUND_SPEED` | 167 | | | `OSD_THROTTLE_GAUGE` | 168 | | | `OSD_GPS_EXTRA_STATS` | 169 | | -| `OSD_ITEM_COUNT` | 170 | | +| `OSD_ACTIVE_RX_LINK` | 170 | | +| `OSD_ITEM_COUNT` | 171 | | --- ## `osd_sidebar_arrow_e` @@ -5040,6 +5125,17 @@ | `RX_FRAME_PROCESSING_REQUIRED` | (1 << 2) | | | `RX_FRAME_DROPPED` | (1 << 3) | | +--- +## `rxLink_e` + +> Source: ../../../src/main/rx/rx.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RX_LINK_PRIMARY` | 0 | | +| `RX_LINK_SECONDARY` | 1 | | +| `RX_LINK_COUNT` | 2 | | + --- ## `rxReceiverType_e` @@ -5270,7 +5366,7 @@ | `FUNCTION_RCDEVICE` | (1 << 10) | | | `FUNCTION_VTX_SMARTAUDIO` | (1 << 11) | | | `FUNCTION_VTX_TRAMP` | (1 << 12) | | -| `FUNCTION_UNUSED_1` | (1 << 13) | | +| `FUNCTION_RX_SERIAL_SECONDARY` | (1 << 13) | | | `FUNCTION_OPTICAL_FLOW` | (1 << 14) | | | `FUNCTION_LOG` | (1 << 15) | | | `FUNCTION_RANGEFINDER` | (1 << 16) | | diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index e8f64d0cb93..ecd7e30e853 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": { @@ -7094,6 +7094,13 @@ "desc": "Enum `batCapacityUnit_e` Capacity unit (`batteryMetersConfig()->capacity_unit`)", "units": "Enum", "enum": "batCapacityUnit_e" + }, + { + "name": "activeRX", + "ctype": "uint8_t", + "desc": "Enum `rxLink_e` Current active receiver link if dual RX activated; otherwise always 0", + "units": "Enum", + "enum": "rxLink_e" } ] }, @@ -11015,7 +11022,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 +11231,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 +11279,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 +11390,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 +11517,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/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 00918a7112e..492187925cb 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -290,6 +290,7 @@ static const OSD_Entry menuOsdElemsEntries[] = #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) OSD_ELEMENT_ENTRY("RC SOURCE", OSD_RC_SOURCE), #endif + OSD_ELEMENT_ENTRY("RX LINK", OSD_ACTIVE_RX_LINK), OSD_ELEMENT_ENTRY("IMU TEMP", OSD_IMU_TEMPERATURE), #ifdef USE_BARO 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..41634198462 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); @@ -51,27 +184,32 @@ static uint16_t mavlinkDbmToMilliwatts(int8_t powerDbm) return powerDbm <= 0 ? 0 : lrintf(powf(10.0f, powerDbm / 10.0f)); } -static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { +static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) +{ + rxLinkStatistics_t *statistics = rxGetLinkStatisticsMutable(RX_LINK_PRIMARY); + switch(mavActiveConfig->radio_type) { case MAVLINK_RADIO_SIK: - rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; - rxLinkStatistics.uplinkSNR = msg->noise / 1.9; - rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + statistics->uplinkRSSI = (msg->rssi / 1.9) - 127; + statistics->uplinkSNR = msg->noise / 1.9; + statistics->uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; break; case MAVLINK_RADIO_ELRS: - rxLinkStatistics.uplinkRSSI = -msg->remrssi; - rxLinkStatistics.uplinkSNR = msg->noise; - rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100); + statistics->uplinkRSSI = -msg->remrssi; + statistics->uplinkSNR = msg->noise; + statistics->uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100); break; case MAVLINK_RADIO_MLRS: break; case MAVLINK_RADIO_GENERIC: default: - rxLinkStatistics.uplinkRSSI = msg->rssi; - rxLinkStatistics.uplinkSNR = msg->noise; - rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + statistics->uplinkRSSI = msg->rssi; + statistics->uplinkSNR = msg->noise; + statistics->uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; break; } + + rxLinkStatisticsUpdated(RX_LINK_PRIMARY); } static bool handleIncoming_RADIO_STATUS(void) { @@ -136,11 +274,13 @@ static bool handleIncoming_MLRS_RADIO_LINK_STATS(uint8_t ingressPortIndex) return true; } - rxLinkStatistics.uplinkLQ = stats->rxLinkQualityRc; - rxLinkStatistics.downlinkLQ = stats->rxLinkQualitySerial; - rxLinkStatistics.uplinkRSSI = stats->rxRssi; - rxLinkStatistics.uplinkSNR = stats->rxSnr; - rxLinkStatistics.activeAntenna = stats->activeAntenna; + rxLinkStatistics_t *statistics = rxGetLinkStatisticsMutable(RX_LINK_PRIMARY); + statistics->uplinkLQ = stats->rxLinkQualityRc; + statistics->downlinkLQ = stats->rxLinkQualitySerial; + statistics->uplinkRSSI = stats->rxRssi; + statistics->uplinkSNR = stats->rxSnr; + statistics->activeAntenna = stats->activeAntenna; + rxLinkStatisticsUpdated(RX_LINK_PRIMARY); return true; } @@ -177,14 +317,16 @@ static bool handleIncoming_MLRS_RADIO_LINK_INFORMATION(uint8_t ingressPortIndex) return true; } - rxLinkStatistics.uplinkTXPower = info->txPowerMw; - rxLinkStatistics.downlinkTXPower = info->rxPowerMw; - memset(rxLinkStatistics.band, 0, sizeof(rxLinkStatistics.band)); - memset(rxLinkStatistics.mode, 0, sizeof(rxLinkStatistics.mode)); - memcpy(rxLinkStatistics.band, info->bandStr, sizeof(rxLinkStatistics.band) - 1); - memcpy(rxLinkStatistics.mode, info->modeStr, sizeof(rxLinkStatistics.mode) - 1); - sl_toupperptr(rxLinkStatistics.band); - sl_toupperptr(rxLinkStatistics.mode); + rxLinkStatistics_t *statistics = rxGetLinkStatisticsMutable(RX_LINK_PRIMARY); + statistics->uplinkTXPower = info->txPowerMw; + statistics->downlinkTXPower = info->rxPowerMw; + memset(statistics->band, 0, sizeof(statistics->band)); + memset(statistics->mode, 0, sizeof(statistics->mode)); + memcpy(statistics->band, info->bandStr, sizeof(statistics->band) - 1); + memcpy(statistics->mode, info->modeStr, sizeof(statistics->mode) - 1); + sl_toupperptr(statistics->band); + sl_toupperptr(statistics->mode); + rxLinkStatisticsUpdated(RX_LINK_PRIMARY); return true; } @@ -260,21 +402,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 +434,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 50ddc2d9f09..790194ce773 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -742,6 +742,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()); @@ -971,6 +979,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU32(dst, currentBatteryProfile->capacity.warning); sbufWriteU32(dst, currentBatteryProfile->capacity.critical); sbufWriteU8(dst, batteryMetersConfig()->capacity_unit); + sbufWriteU8(dst, rxGetActiveLink()); break; case MSP2_INAV_MISC2: @@ -1772,7 +1781,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timer2id(timerHardware[i].tim)); #endif sbufWriteU32(dst, timerHardware[i].usageFlags); - + #if defined(SITL_BUILD) || defined(WASM_BUILD) sbufWriteU8(dst, 0); #else @@ -3353,11 +3362,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) uint8_t sublinkID = sbufReadU8(src); // Sublink ID sbufReadU8(src); // Valid link (Failsafe backup) if (sublinkID == 0) { + rxLinkStatistics_t *statistics = rxGetLinkStatisticsMutable(RX_LINK_PRIMARY); setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI % - rxLinkStatistics.uplinkRSSI = -sbufReadU8(src); - rxLinkStatistics.downlinkLQ = sbufReadU8(src); - rxLinkStatistics.uplinkLQ = sbufReadU8(src); - rxLinkStatistics.uplinkSNR = sbufReadI8(src); + statistics->uplinkRSSI = -sbufReadU8(src); + statistics->downlinkLQ = sbufReadU8(src); + statistics->uplinkLQ = sbufReadU8(src); + statistics->uplinkSNR = sbufReadI8(src); + rxLinkStatisticsUpdated(RX_LINK_PRIMARY); } return MSP_RESULT_NO_REPLY; @@ -3371,20 +3382,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) uint8_t sublinkID = sbufReadU8(src); if (sublinkID == 0) { - rxLinkStatistics.uplinkTXPower = sbufReadU16(src); - rxLinkStatistics.downlinkTXPower = sbufReadU16(src); + rxLinkStatistics_t *statistics = rxGetLinkStatisticsMutable(RX_LINK_PRIMARY); + statistics->uplinkTXPower = sbufReadU16(src); + statistics->downlinkTXPower = sbufReadU16(src); for (int i = 0; i < 4; i++) { - rxLinkStatistics.band[i] = sbufReadU8(src); + statistics->band[i] = sbufReadU8(src); } - sl_toupperptr(rxLinkStatistics.band); + sl_toupperptr(statistics->band); for (int i = 0; i < 6; i++) { - rxLinkStatistics.mode[i] = sbufReadU8(src); + statistics->mode[i] = sbufReadU8(src); } - sl_toupperptr(rxLinkStatistics.mode); + sl_toupperptr(statistics->mode); + rxLinkStatisticsUpdated(RX_LINK_PRIMARY); } return MSP_RESULT_NO_REPLY; @@ -3966,6 +3979,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; } @@ -4418,7 +4452,7 @@ static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_ } // Feed data to navigation gpsProcessNewDriverData(); - gpsProcessNewSolutionData(false); + gpsProcessNewSolutionData(false); } else { sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); } @@ -4471,26 +4505,26 @@ static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_ sbufReadU16(src); } - if (simMspVersion == SIMULATOR_MSP_VERSION_3) { - + if (simMspVersion == SIMULATOR_MSP_VERSION_3) { + if (SIMULATOR_HAS_OPTION(HITL_RANGEFINDER)) { simulatorData.rangefinder = sbufReadU16(src); if (simulatorData.rangefinder == 0xFFFF) { fakeRangefindersSetData(-1); } else { - fakeRangefindersSetData(simulatorData.rangefinder); + fakeRangefindersSetData(simulatorData.rangefinder); } - + } else { sbufReadU16(src); } - + if (SIMULATOR_HAS_OPTION(HITL_CURRENT_SENSOR)) { simulatorData.current = sbufReadU16(src); } else { sbufReadU16(src); } - + if (SIMULATOR_HAS_OPTION(HITL_SIM_RC_INPUT)) { for (int i = 0; i < HITL_SIM_MAX_RC_INPUTS; i++) { simulatorData.rcInput[i] = sbufReadU16(src); @@ -4792,7 +4826,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; } @@ -4804,8 +4838,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; } @@ -4823,6 +4859,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; @@ -4844,6 +4883,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/multifunction.c b/src/main/fc/multifunction.c index 50cf3af1aea..6a936b3731e 100644 --- a/src/main/fc/multifunction.c +++ b/src/main/fc/multifunction.c @@ -38,6 +38,8 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "rx/rx.h" + #include "sensors/acceleration.h" #include "sensors/battery.h" #include "sensors/compass.h" @@ -130,10 +132,10 @@ multi_function_e multiFunctionSelection(void) } #endif // multifunction -static bool osdCheckWarning(bool condition, uint8_t warningFlag) +static bool osdCheckWarning(bool condition, uint16_t warningFlag) { static timeMs_t newWarningEndTime = 0; - static uint8_t newWarningFlags = 0; // bitfield + static uint16_t newWarningFlags = 0; // bitfield const timeMs_t currentTimeMs = millis(); /* New warnings dislayed individually for 10s with blinking after which @@ -260,10 +262,10 @@ textAttributes_t osdGetMultiFunctionMessage(char *buff) #endif // MULTIFUNCTION - functions only, warnings always defined /* --- WARNINGS --- */ - const char *messages[8]; + const char *messages[9]; uint8_t messageCount = 0; bool warningCondition = false; - uint8_t warningFlagID = 1; + uint16_t warningFlagID = 1; // Low Battery Voltage const batteryState_e batteryVoltageState = checkBatteryVoltageState(); @@ -321,6 +323,12 @@ textAttributes_t osdGetMultiFunctionMessage(char *buff) } #endif + // Primary RX link in failsafe while secondary link still has signal + warningCondition = rxIsPrimaryFailsafe() && rxIsLinkReceivingSignal(RX_LINK_SECONDARY); + if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) { + messages[messageCount++] = "PRI RX FS"; + } + // Vibration levels TODO - needs better vibration measurement to be useful // const float vibrationLevel = accGetVibrationLevel(); // warningCondition = vibrationLevel > 1.5f; diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h index cdbdd17ea0f..cdabcd049bb 100644 --- a/src/main/fc/multifunction.h +++ b/src/main/fc/multifunction.h @@ -28,7 +28,7 @@ #include "drivers/display.h" typedef struct multiFunctionWarning_s { - uint8_t osdWarningsFlags; // bitfield + uint16_t osdWarningsFlags; // bitfield bool newWarningActive; } multiFunctionWarning_t; 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 2e972d1b304..63714b6c3b9 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -135,6 +135,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/fc/settings.yaml b/src/main/fc/settings.yaml index 0f1c239f1f0..927aabeccb2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -747,6 +747,11 @@ groups: default_value: 30 min: 1 max: 100 + - name: dual_rx_enabled + description: "Enable dual receiver (RX) handling. When OFF, only the primary receiver is used even if secondary settings are configured." + type: bool + default_value: OFF + field: dualRxEnabled - name: serialrx_provider description: "When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section." default_value: :target @@ -794,6 +799,36 @@ groups: condition: USE_MSP_RC_OVERRIDE min: 0 max: UINT32_MAX + - name: receiver_type_secondary + description: "Selection of the secondary receiver (RX) type for dual RX setups. Set to `NONE` to disable the secondary link. Additional configuration of `serialrx_provider_secondary` and a UART is needed for `SERIAL`." + default_value: "NONE" + field: receiverTypeSecondary + table: receiver_type + - name: serialrx_provider_secondary + description: "Secondary UART-based receiver provider. Only used when `receiver_type_secondary` is `SERIAL`." + default_value: :target + condition: USE_SERIAL_RX + field: serialrx_provider_secondary + table: serial_rx + - name: serialrx_inverted_secondary + description: "Secondary serial RX inversion flag. Reverse the default inversion of the protocol for the secondary link." + default_value: OFF + condition: USE_SERIAL_RX + type: bool + field: serialrx_inverted_secondary + - name: serialrx_halfduplex_secondary + description: "Allow the secondary serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire." + default_value: "AUTO" + condition: USE_SERIAL_RX + field: halfDuplexSecondary + table: tristate + - name: sbus_sync_interval_secondary + description: "SBUS sync interval in us for the secondary receiver. Default value is 3000us. Lower values may cause issues with some receivers." + min: 500 + max: 10000 + default_value: 3000 + condition: USE_SERIAL_RX + field: sbusSyncIntervalSecondary - name: PG_BLACKBOX_CONFIG type: blackboxConfig_t diff --git a/src/main/io/osd.c b/src/main/io/osd.c index dc592dca874..9dae8e94e4e 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2606,6 +2606,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)) @@ -3851,6 +3853,37 @@ static bool osdDrawSingleElement(uint8_t item) } #endif + case OSD_ACTIVE_RX_LINK: + { + const rxLink_e link = rxGetActiveLink(); + const bool primaryValid = rxIsLinkReceivingSignal(RX_LINK_PRIMARY); + const bool secondaryValid = rxIsLinkReceivingSignal(RX_LINK_SECONDARY); + + buff[0] = 'R'; + buff[1] = 'X'; + buff[2] = ':'; + + if (link == RX_LINK_PRIMARY && primaryValid) { + buff[3] = 'P'; + } else if (link == RX_LINK_SECONDARY && secondaryValid) { + buff[3] = 'S'; + } else if (primaryValid) { + buff[3] = 'P'; + } else if (secondaryValid) { + buff[3] = 'S'; + } else { + buff[3] = '-'; + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + + buff[4] = '\0'; + + if (!primaryValid && secondaryValid) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + #if defined(USE_ESC_SENSOR) case OSD_ESC_RPM: { @@ -4510,6 +4543,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) osdLayoutsConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4); #endif + osdLayoutsConfig->item_pos[0][OSD_ACTIVE_RX_LINK] = OSD_POS(3, 5); #ifdef USE_POWER_LIMITS osdLayoutsConfig->item_pos[0][OSD_PLIMIT_REMAINING_BURST_TIME] = OSD_POS(3, 4); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 4767ae035d6..4e9e868efe8 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -342,7 +342,8 @@ typedef enum { OSD_NAV_FW_ALT_CONTROL_RESPONSE, OSD_NAV_MIN_GROUND_SPEED, OSD_THROTTLE_GAUGE, - OSD_GPS_EXTRA_STATS, + OSD_GPS_EXTRA_STATS, + OSD_ACTIVE_RX_LINK, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 36f2e02328a..5682cc30577 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -44,7 +44,7 @@ typedef enum { FUNCTION_RCDEVICE = (1 << 10), // 1024 FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048 FUNCTION_VTX_TRAMP = (1 << 12), // 4096 - FUNCTION_UNUSED_1 = (1 << 13), // 8192: former\ UAV_INTERCONNECT + FUNCTION_RX_SERIAL_SECONDARY = (1 << 13), // 8192 FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 FUNCTION_LOG = (1 << 15), // 32768 FUNCTION_RANGEFINDER = (1 << 16), // 65536 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 ca0f74face7..bb93fcb4934 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2170,10 +2170,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); } @@ -2575,11 +2581,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); } @@ -2594,6 +2604,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 @@ -2676,7 +2691,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) && @@ -2731,7 +2748,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; } } @@ -2783,15 +2802,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; @@ -3848,6 +3874,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 */ @@ -3912,6 +3958,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 * @@ -3986,7 +4040,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); @@ -3995,7 +4049,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; @@ -4113,6 +4167,7 @@ void loadSelectedMultiMission(uint8_t missionIndex) posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0; posControl.activeWaypointIndex = posControl.startWpIndex; + posControl.wpReachedNotificationPending = false; } bool updateWpMissionChange(void) @@ -4210,7 +4265,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++) { @@ -4412,8 +4476,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 @@ -4623,6 +4689,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; @@ -5107,9 +5184,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; @@ -5170,9 +5250,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); @@ -5180,6 +5308,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 65b66d778ce..374259f6a1d 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, @@ -707,6 +707,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) @@ -732,7 +735,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 @@ -765,11 +769,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); @@ -783,6 +796,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. */ @@ -802,7 +817,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 f7b43bf2bd5..0b954f276b9 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 9f4d546f98c..c59c30a5ae1 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 @@ -455,6 +457,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; @@ -477,6 +480,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 @@ -498,9 +502,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 8c988d78384..9f98c8cc5b5 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -490,6 +490,20 @@ 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_RX_SELECT_OVERRIDE: + logicConditionValuesByType[LOGIC_CONDITION_RX_SELECT_OVERRIDE] = constrain(operandA, RX_LINK_PRIMARY, RX_LINK_SECONDARY); + LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RX_LINK); + return true; + 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 183c0cfa054..c449bd6eecd 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -90,6 +90,9 @@ typedef enum { LOGIC_CONDITION_ACOS = 58, LOGIC_CONDITION_ASIN = 59, LOGIC_CONDITION_ATAN2 = 60, + LOGIC_CONDITION_ACTIVATE_RTH = 61, + LOGIC_CONDITION_ACTIVATE_LANDING = 62, + LOGIC_CONDITION_RX_SELECT_OVERRIDE = 63, LOGIC_CONDITION_LAST } logicOperation_e; @@ -211,6 +214,7 @@ typedef enum { LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11), #endif LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED = (1 << 12), + LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RX_LINK = (1 << 13), } logicConditionsGlobalFlags_t; typedef enum { diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index e5855205642..d5e730eb925 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -49,15 +49,22 @@ #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type) #define CRSF_POWER_COUNT 9 -STATIC_UNIT_TESTED bool crsfFrameDone = false; -STATIC_UNIT_TESTED crsfFrame_t crsfFrame; - -STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; - -static serialPort_t *serialPort; -static timeUs_t crsfFrameStartAtUs = 0; -static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; -static uint8_t telemetryBufLen = 0; +typedef struct crsfLinkState_s { + bool frameDone; + crsfFrame_t frame; + uint32_t channelData[CRSF_MAX_CHANNEL]; + timeUs_t frameStartAtUs; + uint8_t framePosition; + serialPort_t *port; + uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; + uint8_t telemetryBufLen; + rxLink_e link; +} crsfLinkState_t; + +// Per-link parser and channel state so a primary and secondary CRSF receiver +// don't share frame/channel buffers. Primary is index 0, so the single-RX path +// is unchanged. +static crsfLinkState_t crsfLinkStates[RX_LINK_COUNT]; const uint16_t crsfTxPowerStatesmW[CRSF_POWER_COUNT] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50}; @@ -125,12 +132,12 @@ typedef struct crsfPayloadLinkStatistics_s { typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t; -STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void) +STATIC_UNIT_TESTED uint8_t crsfFrameCRC(const crsfFrame_t *crsfFrame) { // CRC includes type and payload - uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type); - for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) { - crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]); + uint8_t crc = crc8_dvb_s2(0, crsfFrame->frame.type); + for (int ii = 0; ii < crsfFrame->frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) { + crc = crc8_dvb_s2(crc, crsfFrame->frame.payload[ii]); } return crc; } @@ -138,53 +145,52 @@ STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void) // Receive ISR callback, called back from serial port STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData) { - UNUSED(rxCallbackData); + crsfLinkState_t *link = rxCallbackData; - static uint8_t crsfFramePosition = 0; const timeUs_t currentTimeUs = microsISR(); #ifdef DEBUG_CRSF_PACKETS debug[2] = now - crsfFrameStartAt; #endif - if (cmpTimeUs(currentTimeUs, crsfFrameStartAtUs) > CRSF_TIME_NEEDED_PER_FRAME_US) { + if (cmpTimeUs(currentTimeUs, link->frameStartAtUs) > CRSF_TIME_NEEDED_PER_FRAME_US) { // We've received a character after max time needed to complete a frame, // so this must be the start of a new frame. - crsfFramePosition = 0; + link->framePosition = 0; } - if (crsfFramePosition == 0) { - crsfFrameStartAtUs = currentTimeUs; + if (link->framePosition == 0) { + link->frameStartAtUs = currentTimeUs; } // assume frame is 5 bytes long until we have received the frame length // full frame length includes the length of the address and framelength fields - const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH; + const int fullFrameLength = link->framePosition < 3 ? 5 : link->frame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH; if (fullFrameLength > CRSF_FRAME_SIZE_MAX) { - crsfFramePosition = 0; + link->framePosition = 0; return; } - if (crsfFramePosition < fullFrameLength) { - crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c; - crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true; - if (crsfFrameDone) { - crsfFramePosition = 0; - if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { - const uint8_t crc = crsfFrameCRC(); - if (crc == crsfFrame.bytes[fullFrameLength - 1]) { - switch (crsfFrame.frame.type) + if (link->framePosition < fullFrameLength) { + link->frame.bytes[link->framePosition++] = (uint8_t)c; + link->frameDone = link->framePosition < fullFrameLength ? false : true; + if (link->frameDone) { + link->framePosition = 0; + if (link->frame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { + const uint8_t crc = crsfFrameCRC(&link->frame); + if (crc == link->frame.bytes[fullFrameLength - 1]) { + switch (link->frame.frame.type) { #if defined(USE_MSP_OVER_TELEMETRY) case CRSF_FRAMETYPE_MSP_REQ: case CRSF_FRAMETYPE_MSP_WRITE: { - if (crsfFrame.frame.frameLength >= 4) { - uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; - if (bufferCrsfMspFrame(frameStart, crsfFrame.frame.frameLength - 4)) { - crsfScheduleMspResponse(crsfFrame.frame.payload[1]); + if (link->frame.frame.frameLength >= 4) { + uint8_t *frameStart = (uint8_t *)&link->frame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; + if (bufferCrsfMspFrame(link->link, frameStart, link->frame.frame.frameLength - 4)) { + crsfScheduleMspResponse(link->link, link->frame.frame.payload[1]); } } else { - crsfFrameDone = false; + link->frameDone = false; } break; } @@ -200,63 +206,66 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData) STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { - UNUSED(rxRuntimeConfig); + crsfLinkState_t *link = rxRuntimeConfig->frameData; - if (crsfFrameDone) { - crsfFrameDone = false; - if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { + if (link->frameDone) { + link->frameDone = false; + if (link->frame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { // CRC includes type and payload of each frame - const uint8_t crc = crsfFrameCRC(); - if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) { + const uint8_t crc = crsfFrameCRC(&link->frame); + if (crc != link->frame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) { return RX_FRAME_PENDING; } - crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; + link->frame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; // unpack the RC channels - const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; - crsfChannelData[0] = rcChannels->chan0; - crsfChannelData[1] = rcChannels->chan1; - crsfChannelData[2] = rcChannels->chan2; - crsfChannelData[3] = rcChannels->chan3; - crsfChannelData[4] = rcChannels->chan4; - crsfChannelData[5] = rcChannels->chan5; - crsfChannelData[6] = rcChannels->chan6; - crsfChannelData[7] = rcChannels->chan7; - crsfChannelData[8] = rcChannels->chan8; - crsfChannelData[9] = rcChannels->chan9; - crsfChannelData[10] = rcChannels->chan10; - crsfChannelData[11] = rcChannels->chan11; - crsfChannelData[12] = rcChannels->chan12; - crsfChannelData[13] = rcChannels->chan13; - crsfChannelData[14] = rcChannels->chan14; - crsfChannelData[15] = rcChannels->chan15; + const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&link->frame.frame.payload; + link->channelData[0] = rcChannels->chan0; + link->channelData[1] = rcChannels->chan1; + link->channelData[2] = rcChannels->chan2; + link->channelData[3] = rcChannels->chan3; + link->channelData[4] = rcChannels->chan4; + link->channelData[5] = rcChannels->chan5; + link->channelData[6] = rcChannels->chan6; + link->channelData[7] = rcChannels->chan7; + link->channelData[8] = rcChannels->chan8; + link->channelData[9] = rcChannels->chan9; + link->channelData[10] = rcChannels->chan10; + link->channelData[11] = rcChannels->chan11; + link->channelData[12] = rcChannels->chan12; + link->channelData[13] = rcChannels->chan13; + link->channelData[14] = rcChannels->chan14; + link->channelData[15] = rcChannels->chan15; return RX_FRAME_COMPLETE; } - else if (crsfFrame.frame.type == CRSF_FRAMETYPE_LINK_STATISTICS) { + else if (link->frame.frame.type == CRSF_FRAMETYPE_LINK_STATISTICS) { // CRC includes type and payload of each frame - const uint8_t crc = crsfFrameCRC(); - if (crc != crsfFrame.frame.payload[CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE]) { + const uint8_t crc = crsfFrameCRC(&link->frame); + if (crc != link->frame.frame.payload[CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE]) { return RX_FRAME_PENDING; } - crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; + link->frame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; - const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload; + const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&link->frame.frame.payload; const uint8_t crsftxpowerindex = (linkStats->uplinkTXPower < CRSF_POWER_COUNT) ? linkStats->uplinkTXPower : 0; - rxLinkStatistics.uplinkRSSI = -1* (linkStats->activeAntenna ? linkStats->uplinkRSSIAnt2 : linkStats->uplinkRSSIAnt1); - rxLinkStatistics.uplinkLQ = linkStats->uplinkLQ; - rxLinkStatistics.uplinkSNR = linkStats->uplinkSNR; - rxLinkStatistics.rfMode = linkStats->rfMode; - rxLinkStatistics.uplinkTXPower = crsfTxPowerStatesmW[crsftxpowerindex]; - rxLinkStatistics.activeAntenna = linkStats->activeAntenna; + rxLinkStatistics_t *statistics = rxRuntimeConfig->linkStatistics; + if (statistics) { + statistics->uplinkRSSI = -1* (linkStats->activeAntenna ? linkStats->uplinkRSSIAnt2 : linkStats->uplinkRSSIAnt1); + statistics->uplinkLQ = linkStats->uplinkLQ; + statistics->uplinkSNR = linkStats->uplinkSNR; + statistics->rfMode = linkStats->rfMode; + statistics->uplinkTXPower = crsfTxPowerStatesmW[crsftxpowerindex]; + statistics->activeAntenna = linkStats->activeAntenna; + } #ifdef USE_OSD - if (rxLinkStatistics.uplinkLQ > 0) { + if (statistics && statistics->uplinkLQ > 0) { int16_t uplinkStrength; // RSSI dBm converted to % - uplinkStrength = constrain((100 * sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)) - (100 * sq((osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI)))) / sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)),0,100); - if (rxLinkStatistics.uplinkRSSI >= osdConfig()->rssi_dbm_max ) + uplinkStrength = constrain((100 * sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)) - (100 * sq((osdConfig()->rssi_dbm_max - statistics->uplinkRSSI)))) / sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)),0,100); + if (statistics->uplinkRSSI >= osdConfig()->rssi_dbm_max ) uplinkStrength = 99; - else if (rxLinkStatistics.uplinkRSSI < osdConfig()->rssi_dbm_min) + else if (statistics->uplinkRSSI < osdConfig()->rssi_dbm_min) uplinkStrength = 0; lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(uplinkStrength, 0, 99, 0, RSSI_MAX_VALUE)); } else { @@ -272,7 +281,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { - UNUSED(rxRuntimeConfig); + const crsfLinkState_t *link = rxRuntimeConfig->frameData; /* conversion from RC value to PWM * RC PWM * min 172 -> 988us @@ -281,75 +290,111 @@ STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConf * scale factor = (2012-988) / (1811-172) = 0.62477120195241 * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548 */ - return (crsfChannelData[chan] * 1024 / 1639) + 881; + return (link->channelData[chan] * 1024 / 1639) + 881; } void crsfRxWriteTelemetryData(const void *data, int len) { - len = MIN(len, (int)sizeof(telemetryBuf)); - memcpy(telemetryBuf, data, len); - telemetryBufLen = len; + for (rxLink_e link = RX_LINK_PRIMARY; link < RX_LINK_COUNT; link++) { + if (crsfLinkStates[link].port) { + crsfRxWriteTelemetryDataForLink(link, data, len); + } + } } -void crsfRxSendTelemetryData(void) +void crsfRxWriteTelemetryDataForLink(rxLink_e linkIndex, const void *data, int len) { - // if there is telemetry data to write - if (telemetryBufLen > 0) { + crsfLinkState_t *link = &crsfLinkStates[linkIndex]; + len = MIN(len, (int)sizeof(link->telemetryBuf)); + memcpy(link->telemetryBuf, data, len); + link->telemetryBufLen = len; +} + +static void crsfRxSendTelemetryDataForLink(crsfLinkState_t *link) +{ + if (link->telemetryBufLen > 0) { // check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame) // and that there is time to send the telemetry frame before the next RX frame arrives if (CRSF_PORT_OPTIONS & SERIAL_BIDIR) { - const timeDelta_t timeSinceStartOfFrame = cmpTimeUs(micros(), crsfFrameStartAtUs); + const timeDelta_t timeSinceStartOfFrame = cmpTimeUs(micros(), link->frameStartAtUs); if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) || (timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) { return; } } - serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen); - telemetryBufLen = 0; // reset telemetry buffer + serialWriteBuf(link->port, link->telemetryBuf, link->telemetryBufLen); + link->telemetryBufLen = 0; } } -bool crsfRxIsTelemetryBufEmpty(void) +void crsfRxSendTelemetryData(void) { - return telemetryBufLen == 0; + for (rxLink_e link = RX_LINK_PRIMARY; link < RX_LINK_COUNT; link++) { + if (crsfLinkStates[link].port) { + crsfRxSendTelemetryDataForLink(&crsfLinkStates[link]); + } + } +} + +bool crsfRxIsTelemetryBufEmpty(rxLink_e link) +{ + return crsfLinkStates[link].telemetryBufLen == 0; +} + +bool crsfRxAreTelemetryBufsEmpty(void) +{ + for (rxLink_e link = RX_LINK_PRIMARY; link < RX_LINK_COUNT; link++) { + if (crsfLinkStates[link].port && !crsfRxIsTelemetryBufEmpty(link)) { + return false; + } + } + return true; } -bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { + const unsigned linkIndex = (portFunction == FUNCTION_RX_SERIAL_SECONDARY) ? RX_LINK_SECONDARY : RX_LINK_PRIMARY; + crsfLinkState_t *link = &crsfLinkStates[linkIndex]; + + link->frameDone = false; + link->framePosition = 0; + link->telemetryBufLen = 0; + link->link = linkIndex; for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { - crsfChannelData[ii] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408; + link->channelData[ii] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408; } rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL; rxRuntimeConfig->rcReadRawFn = crsfReadRawRC; rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus; + rxRuntimeConfig->frameData = link; - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(portFunction); if (!portConfig) { return false; } - serialPort = openSerialPort(portConfig->identifier, - FUNCTION_RX_SERIAL, + link->port = openSerialPort(portConfig->identifier, + portFunction, crsfDataReceive, - NULL, + link, CRSF_BAUDRATE, CRSF_PORT_MODE, CRSF_PORT_OPTIONS | (tristateWithDefaultOffIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0) ); - return serialPort != NULL; + return link->port != NULL; } bool crsfRxIsActive(void) { - return serialPort != NULL; + return crsfLinkStates[RX_LINK_PRIMARY].port || crsfLinkStates[RX_LINK_SECONDARY].port; } void crsfBind(void) { - if (serialPort != NULL) { + if (crsfLinkStates[RX_LINK_PRIMARY].port) { uint8_t bindFrame[] = { CRSF_SYNC_BYTE, 0x07, // frame length @@ -361,7 +406,7 @@ void crsfBind(void) 0x9E, // Command CRC8 0xE8, // Packet CRC8 }; - serialWriteBuf(serialPort, bindFrame, 9); + serialWriteBuf(crsfLinkStates[RX_LINK_PRIMARY].port, bindFrame, 9); } } diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 33f7c51bc9a..f28a28c7d49 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -17,6 +17,8 @@ #pragma once +#include "rx/rx.h" + #define CRSF_BAUDRATE 420000 #define CRSF_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO) #define CRSF_PORT_MODE MODE_RXTX @@ -133,12 +135,14 @@ typedef union crsfFrame_u { void crsfRxWriteTelemetryData(const void *data, int len); +void crsfRxWriteTelemetryDataForLink(rxLink_e link, const void *data, int len); void crsfRxSendTelemetryData(void); -bool crsfRxIsTelemetryBufEmpty(void); +bool crsfRxIsTelemetryBufEmpty(rxLink_e link); +bool crsfRxAreTelemetryBufsEmpty(void); struct rxConfig_s; struct rxRuntimeConfig_s; -bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); +bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig, serialPortFunction_e portFunction); bool crsfRxIsActive(void); void crsfBind(void); diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 32f8c638fe2..f701032b798 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -357,7 +357,7 @@ static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) return true; } -bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { static uint16_t sbusChannelData[SBUS_MAX_CHANNEL]; rxRuntimeConfig->channelData = sbusChannelData; @@ -367,13 +367,13 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus; rxRuntimeConfig->rcProcessFrameFn = fportProcessFrame; - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(portFunction); if (!portConfig) { return false; } fportPort = openSerialPort(portConfig->identifier, - FUNCTION_RX_SERIAL, + portFunction, fportDataReceive, NULL, FPORT_BAUDRATE, diff --git a/src/main/rx/fport.h b/src/main/rx/fport.h index 2c51979e336..443286afa2a 100644 --- a/src/main/rx/fport.h +++ b/src/main/rx/fport.h @@ -17,4 +17,6 @@ #pragma once -bool fportRxInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +#include "rx/rx.h" + +bool fportRxInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction); diff --git a/src/main/rx/fport2.c b/src/main/rx/fport2.c index 51735e0b255..9a1df87cf4d 100644 --- a/src/main/rx/fport2.c +++ b/src/main/rx/fport2.c @@ -630,7 +630,7 @@ static bool processFrame(const rxRuntimeConfig_t *rxRuntimeConfig) } -bool fport2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, bool isFBUS) +bool fport2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, bool isFBUS, serialPortFunction_e portFunction) { static uint16_t sbusChannelData[SBUS_MAX_CHANNEL]; rxRuntimeConfig->channelData = sbusChannelData; @@ -640,7 +640,7 @@ bool fport2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig rxRuntimeConfig->rcFrameStatusFn = frameStatus; rxRuntimeConfig->rcProcessFrameFn = processFrame; - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(portFunction); if (!portConfig) { return false; } @@ -648,7 +648,7 @@ bool fport2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig uint32_t baudRate = (isFBUS) ? FBUS_BAUDRATE : FPORT2_BAUDRATE; fportPort = openSerialPort(portConfig->identifier, - FUNCTION_RX_SERIAL, + portFunction, fportDataReceive, NULL, baudRate, diff --git a/src/main/rx/fport2.h b/src/main/rx/fport2.h index 2ebd1bd0268..ec726502da4 100644 --- a/src/main/rx/fport2.h +++ b/src/main/rx/fport2.h @@ -20,8 +20,10 @@ #pragma once +#include "rx/rx.h" + #ifdef USE_SERIALRX_FPORT2 -bool fport2RxInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, bool isFBUS); +bool fport2RxInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, bool isFBUS, serialPortFunction_e portFunction); #endif diff --git a/src/main/rx/ghst.c b/src/main/rx/ghst.c index cdab2668e8c..0da11278205 100644 --- a/src/main/rx/ghst.c +++ b/src/main/rx/ghst.c @@ -340,7 +340,7 @@ STATIC_UNIT_TESTED uint16_t ghstReadRawRC(const rxRuntimeConfig_t *rxRuntimeStat return (5 * (ghstChannelData[chan]+1) / 8) + 880; } -bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeState) +bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeState, serialPortFunction_e portFunction) { for (int iChan = 0; iChan < GHST_MAX_NUM_CHANNELS; ++iChan) { ghstChannelData[iChan] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408; @@ -351,13 +351,13 @@ bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeState) rxRuntimeState->rcFrameStatusFn = ghstFrameStatus; rxRuntimeState->rcProcessFrameFn = ghstProcessFrame; - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(portFunction); if (!portConfig) { return false; } serialPort = openSerialPort(portConfig->identifier, - FUNCTION_RX_SERIAL, + portFunction, ghstDataReceive, NULL, GHST_RX_BAUDRATE, diff --git a/src/main/rx/ghst.h b/src/main/rx/ghst.h index 43eed4db624..f1909d62e49 100644 --- a/src/main/rx/ghst.h +++ b/src/main/rx/ghst.h @@ -20,6 +20,7 @@ #pragma once +#include "rx/rx.h" #include "rx/ghst_protocol.h" #define GHST_MAX_NUM_CHANNELS 16 @@ -29,5 +30,5 @@ void ghstRxSendTelemetryData(void); struct rxConfig_s; struct rxRuntimeState_s; -bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeState); +bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeState, serialPortFunction_e portFunction); bool ghstRxIsActive(void); diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index 4dc9b09a996..dabb4dc091a 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -212,7 +212,7 @@ static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t } -bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { UNUSED(rxConfig); ibusSyncByte = 0; @@ -221,20 +221,20 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->rcReadRawFn = ibusReadRawRC; rxRuntimeConfig->rcFrameStatusFn = ibusFrameStatus; - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(portFunction); if (!portConfig) { return false; } #ifdef USE_TELEMETRY - bool portShared = isSerialPortShared(portConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS); + bool portShared = isSerialPortShared(portConfig, portFunction, FUNCTION_TELEMETRY_IBUS); #else bool portShared = false; #endif rxBytesToIgnore = 0; serialPort_t *ibusPort = openSerialPort(portConfig->identifier, - FUNCTION_RX_SERIAL, + portFunction, ibusDataReceive, NULL, IBUS_BAUDRATE, diff --git a/src/main/rx/ibus.h b/src/main/rx/ibus.h index 73221c44fd1..26e96d8167a 100755 --- a/src/main/rx/ibus.h +++ b/src/main/rx/ibus.h @@ -17,4 +17,6 @@ #pragma once -bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +#include "rx/rx.h" + +bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction); diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index a793bdae2c0..2af385cd586 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -276,7 +276,7 @@ static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uin return (jetiExBusChannelData[chan]); } -bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { UNUSED(rxConfig); @@ -288,14 +288,14 @@ bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfi jetiExBusFrameReset(); - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(portFunction); if (!portConfig) { return false; } jetiExBusPort = openSerialPort(portConfig->identifier, - FUNCTION_RX_SERIAL, + portFunction, jetiExBusDataReceive, NULL, JETIEXBUS_BAUDRATE, diff --git a/src/main/rx/jetiexbus.h b/src/main/rx/jetiexbus.h index 5029cad7c35..65250a1ecbe 100644 --- a/src/main/rx/jetiexbus.h +++ b/src/main/rx/jetiexbus.h @@ -17,6 +17,8 @@ #pragma once +#include "rx/rx.h" + #define EXBUS_HEADER_LEN 6 #define EXBUS_CRC_LEN 2 #define EXBUS_OVERHEAD (EXBUS_HEADER_LEN + EXBUS_CRC_LEN) @@ -51,4 +53,4 @@ extern struct serialPort_s *jetiExBusPort; extern volatile bool jetiExBusCanTx; uint16_t jetiExBusCalcCRC16(uint8_t *pt, uint8_t msgLen); -bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction); diff --git a/src/main/rx/mavlink.c b/src/main/rx/mavlink.c index c2e3941574d..a71353aa487 100644 --- a/src/main/rx/mavlink.c +++ b/src/main/rx/mavlink.c @@ -73,9 +73,10 @@ static uint16_t mavlinkReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8 return mavlinkChannelData[channel]; } -bool mavlinkRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool mavlinkRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { UNUSED(rxConfig); + UNUSED(portFunction); frameReceived = false; diff --git a/src/main/rx/mavlink.h b/src/main/rx/mavlink.h index 0290b010003..318b8d6a18c 100644 --- a/src/main/rx/mavlink.h +++ b/src/main/rx/mavlink.h @@ -24,6 +24,7 @@ #define MAVLINK_COMM_NUM_BUFFERS MAX_MAVLINK_PORTS #include "storm32/mavlink.h" #pragma GCC diagnostic pop +#include "rx/rx.h" void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg); -bool mavlinkRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); +bool mavlinkRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig, serialPortFunction_e portFunction); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index b897238ed1d..ac6d3985dd3 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -74,23 +74,35 @@ static timeUs_t lastMspRssiUpdateUs = 0; #define RX_LQ_INTERVAL_MS 200 #define RX_LQ_TIMEOUT_MS 1000 -static rxLinkQualityTracker_e rxLQTracker; static rssiSource_e activeRssiSource; -static bool rxDataProcessingRequired = false; -static bool auxiliaryProcessingRequired = false; - -#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) -static bool mspOverrideDataProcessingRequired = false; -#endif - static bool rxSignalReceived = false; static bool rxFlightChannelsValid = false; -static uint8_t rxChannelCount; -static timeUs_t rxNextUpdateAtUs = 0; -static timeUs_t needRxSignalBefore = 0; static bool isRxSuspended = false; +static bool dualRxEnabled = false; + +typedef struct rxLinkState_s { + rxRuntimeConfig_t runtimeConfig; + rxLinkStatistics_t statistics; + rxLinkQualityTracker_e linkQuality; + rcChannel_t channels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + uint16_t channelData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + bool dataProcessingRequired; + bool auxiliaryProcessingRequired; + bool signalReceived; + bool flightChannelsValid; + uint8_t frameStatus; + timeUs_t nextUpdateAtUs; + timeUs_t needRxSignalBefore; +} rxLinkState_t; + +static rxLinkState_t rxLinks[RX_LINK_COUNT]; +static rxLink_e activeLink = RX_LINK_PRIMARY; + +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) +static bool mspOverrideDataProcessingRequired = false; +#endif static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; @@ -101,7 +113,7 @@ rxLinkStatistics_t rxLinkStatistics; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; -PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 13); +PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 14); #ifndef SERIALRX_PROVIDER #define SERIALRX_PROVIDER 0 @@ -114,13 +126,28 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 13); #define RX_MIN_USEX 885 PG_RESET_TEMPLATE(rxConfig_t, rxConfig, .receiverType = DEFAULT_RX_TYPE, + .dualRxEnabled = 0, .rcmap = {0, 1, 3, 2}, // Default to AETR map .halfDuplex = SETTING_SERIALRX_HALFDUPLEX_DEFAULT, + .receiverTypeSecondary = RX_TYPE_NONE, +#ifdef USE_SERIAL_RX .serialrx_provider = SERIALRX_PROVIDER, + .serialrx_inverted = SETTING_SERIALRX_INVERTED_DEFAULT, + .serialrx_provider_secondary = SERIALRX_PROVIDER, + .serialrx_inverted_secondary = SETTING_SERIALRX_INVERTED_DEFAULT, + .halfDuplexSecondary = SETTING_SERIALRX_HALFDUPLEX_DEFAULT, + .sbusSyncIntervalSecondary = SETTING_SBUS_SYNC_INTERVAL_DEFAULT, +#else + .serialrx_provider = SERIALRX_PROVIDER, + .serialrx_inverted = 0, + .serialrx_provider_secondary = SERIALRX_PROVIDER, + .serialrx_inverted_secondary = 0, + .halfDuplexSecondary = 0, + .sbusSyncIntervalSecondary = SETTING_SBUS_SYNC_INTERVAL_DEFAULT, +#endif #ifdef USE_SPEKTRUM_BIND .spektrum_sat_bind = SETTING_SPEKTRUM_SAT_BIND_DEFAULT, #endif - .serialrx_inverted = SETTING_SERIALRX_INVERTED_DEFAULT, .mincheck = SETTING_MIN_CHECK_DEFAULT, .maxcheck = SETTING_MAX_CHECK_DEFAULT, .rx_min_usec = SETTING_RX_MIN_USEC_DEFAULT, // any of first 4 channels below this value will trigger rx loss detection @@ -176,78 +203,158 @@ static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) return RX_FRAME_PENDING; } +static bool nullProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) +{ + UNUSED(rxRuntimeConfig); + return true; +} + bool isRxPulseValid(uint16_t pulseDuration) { return pulseDuration >= rxConfig()->rx_min_usec && pulseDuration <= rxConfig()->rx_max_usec; } +static void rxInitChannelState(rcChannel_t *channels, timeMs_t nowMs) +{ + for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { + channels[i].raw = PWM_RANGE_MIDDLE; + channels[i].data = PWM_RANGE_MIDDLE; + channels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME; + } + + channels[THROTTLE].raw = feature(FEATURE_REVERSIBLE_MOTORS) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec; + channels[THROTTLE].data = channels[THROTTLE].raw; + + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + if (modeActivationConditions(i)->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationConditions(i)->range)) { + uint16_t value; + if (modeActivationConditions(i)->range.startStep > 0) { + value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.startStep - 1)); + } else { + value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.endStep + 1)); + } + rcChannel_t *armChannel = &channels[modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT]; + armChannel->raw = value; + armChannel->data = value; + } + } +} + +static void rxResetLinkState(rxLinkState_t *link, timeMs_t nowMs) +{ + lqTrackerReset(&link->linkQuality); + memset(&link->statistics, 0, sizeof(link->statistics)); + memset(link->channelData, 0, sizeof(link->channelData)); + + link->runtimeConfig.lqTracker = &link->linkQuality; + link->runtimeConfig.linkStatistics = &link->statistics; + link->runtimeConfig.rcReadRawFn = nullReadRawRC; + link->runtimeConfig.rcFrameStatusFn = nullFrameStatus; + link->runtimeConfig.rcProcessFrameFn = nullProcessFrame; + link->runtimeConfig.rxSignalTimeout = DELAY_10_HZ; + link->runtimeConfig.receiverType = RX_TYPE_NONE; + link->runtimeConfig.channelData = link->channelData; + link->runtimeConfig.channelCount = 0; + link->runtimeConfig.frameData = NULL; + + link->dataProcessingRequired = false; + link->auxiliaryProcessingRequired = false; + link->signalReceived = false; + link->flightChannelsValid = false; + link->frameStatus = RX_FRAME_PENDING; + link->nextUpdateAtUs = 0; + link->needRxSignalBefore = 0; + + rxInitChannelState(link->channels, nowMs); +} + +static void populateLinkConfig(rxConfig_t *dst, const rxConfig_t *src, rxLink_e link) +{ + *dst = *src; + +#ifdef USE_SERIAL_RX + if (link == RX_LINK_SECONDARY) { + dst->receiverType = src->receiverTypeSecondary; + dst->serialrx_provider = src->serialrx_provider_secondary; + dst->serialrx_inverted = src->serialrx_inverted_secondary; + dst->halfDuplex = src->halfDuplexSecondary; + dst->sbusSyncInterval = src->sbusSyncIntervalSecondary; + } +#else + if (link == RX_LINK_SECONDARY) { + dst->receiverType = RX_TYPE_NONE; + } + UNUSED(src); +#endif +} + #ifdef USE_SERIAL_RX -bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { bool enabled = false; switch (rxConfig->serialrx_provider) { #ifdef USE_SERIALRX_SRXL2 case SERIALRX_SRXL2: - enabled = srxl2RxInit(rxConfig, rxRuntimeConfig); + enabled = srxl2RxInit(rxConfig, rxRuntimeConfig, portFunction); break; #endif #ifdef USE_SERIALRX_SPEKTRUM case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: - enabled = spektrumInit(rxConfig, rxRuntimeConfig); + enabled = spektrumInit(rxConfig, rxRuntimeConfig, portFunction); break; #endif #ifdef USE_SERIALRX_SBUS case SERIALRX_SBUS2: case SERIALRX_SBUS: - enabled = sbusInit(rxConfig, rxRuntimeConfig); + enabled = sbusInit(rxConfig, rxRuntimeConfig, portFunction); break; case SERIALRX_SBUS_FAST: - enabled = sbusInitFast(rxConfig, rxRuntimeConfig); + enabled = sbusInitFast(rxConfig, rxRuntimeConfig, portFunction); break; #endif #ifdef USE_SERIALRX_SUMD case SERIALRX_SUMD: - enabled = sumdInit(rxConfig, rxRuntimeConfig); + enabled = sumdInit(rxConfig, rxRuntimeConfig, portFunction); break; #endif #ifdef USE_SERIALRX_IBUS case SERIALRX_IBUS: - enabled = ibusInit(rxConfig, rxRuntimeConfig); + enabled = ibusInit(rxConfig, rxRuntimeConfig, portFunction); break; #endif #ifdef USE_SERIALRX_JETIEXBUS case SERIALRX_JETIEXBUS: - enabled = jetiExBusInit(rxConfig, rxRuntimeConfig); + enabled = jetiExBusInit(rxConfig, rxRuntimeConfig, portFunction); break; #endif #ifdef USE_SERIALRX_CRSF case SERIALRX_CRSF: - enabled = crsfRxInit(rxConfig, rxRuntimeConfig); + enabled = crsfRxInit(rxConfig, rxRuntimeConfig, portFunction); break; #endif #ifdef USE_SERIALRX_FPORT case SERIALRX_FPORT: - enabled = fportRxInit(rxConfig, rxRuntimeConfig); + enabled = fportRxInit(rxConfig, rxRuntimeConfig, portFunction); break; #endif #ifdef USE_SERIALRX_FPORT2 case SERIALRX_FPORT2: - enabled = fport2RxInit(rxConfig, rxRuntimeConfig, false); + enabled = fport2RxInit(rxConfig, rxRuntimeConfig, false, portFunction); break; case SERIALRX_FBUS: - enabled = fport2RxInit(rxConfig, rxRuntimeConfig, true); + enabled = fport2RxInit(rxConfig, rxRuntimeConfig, true, portFunction); break; #endif #ifdef USE_SERIALRX_GHST case SERIALRX_GHST: - enabled = ghstRxInit(rxConfig, rxRuntimeConfig); + enabled = ghstRxInit(rxConfig, rxRuntimeConfig, portFunction); break; #endif #ifdef USE_SERIALRX_MAVLINK case SERIALRX_MAVLINK: - enabled = mavlinkRxInit(rxConfig, rxRuntimeConfig); + enabled = mavlinkRxInit(rxConfig, rxRuntimeConfig, portFunction); break; #endif default: @@ -258,74 +365,114 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig } #endif -void rxInit(void) +// Identifies the driver backing a serial RX provider. Providers that map to the +// same group share that driver's module-static parser state, so two links in the +// same group alias each other unless the driver has been made instance-safe. +static uint8_t serialRxDriverGroup(uint8_t provider) { - lqTrackerReset(&rxLQTracker); + switch (provider) { + case SERIALRX_SBUS: + case SERIALRX_SBUS_FAST: + case SERIALRX_SBUS2: + return SERIALRX_SBUS; + case SERIALRX_FPORT2: + case SERIALRX_FBUS: + return SERIALRX_FPORT2; + default: + return provider; + } +} + +// Driver groups that carry per-link state and so can run on both links at once. +static bool serialRxDriverGroupIsDualLinkSafe(uint8_t group) +{ + return group == SERIALRX_SBUS || group == SERIALRX_CRSF; +} - rxRuntimeConfig.lqTracker = &rxLQTracker; - rxRuntimeConfig.rcReadRawFn = nullReadRawRC; - rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; - rxRuntimeConfig.rxSignalTimeout = DELAY_10_HZ; +void rxInit(void) +{ rcSampleIndex = 0; + dualRxEnabled = rxConfig()->dualRxEnabled; + if (rxConfig()->receiverType == RX_TYPE_MSP || rxConfig()->receiverTypeSecondary == RX_TYPE_MSP) { + dualRxEnabled = false; + } + if ((rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK) || + (rxConfig()->receiverTypeSecondary == RX_TYPE_SERIAL && rxConfig()->serialrx_provider_secondary == SERIALRX_MAVLINK)) { + dualRxEnabled = false; + } + // Two links sharing a not-yet-instance-safe serial driver would alias its module + // parser buffers and corrupt both. Refuse dual RX for that combination; links on + // different drivers are unaffected. + if (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->receiverTypeSecondary == RX_TYPE_SERIAL) { + const uint8_t primaryGroup = serialRxDriverGroup(rxConfig()->serialrx_provider); + const uint8_t secondaryGroup = serialRxDriverGroup(rxConfig()->serialrx_provider_secondary); + if (primaryGroup == secondaryGroup && !serialRxDriverGroupIsDualLinkSafe(primaryGroup)) { + dualRxEnabled = false; + } + } - timeMs_t nowMs = millis(); + const timeMs_t nowMs = millis(); - for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rcChannels[i].raw = PWM_RANGE_MIDDLE; - rcChannels[i].data = PWM_RANGE_MIDDLE; - rcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME; + for (int linkIndex = 0; linkIndex < RX_LINK_COUNT; linkIndex++) { + rxResetLinkState(&rxLinks[linkIndex], nowMs); } - rcChannels[THROTTLE].raw = (feature(FEATURE_REVERSIBLE_MOTORS)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec; - rcChannels[THROTTLE].data = rcChannels[THROTTLE].raw; + rxConfig_t linkConfigs[RX_LINK_COUNT]; + populateLinkConfig(&linkConfigs[RX_LINK_PRIMARY], rxConfig(), RX_LINK_PRIMARY); + populateLinkConfig(&linkConfigs[RX_LINK_SECONDARY], rxConfig(), RX_LINK_SECONDARY); - // Initialize ARM switch to OFF position when arming via switch is defined - for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - if (modeActivationConditions(i)->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationConditions(i)->range)) { - // ARM switch is defined, determine an OFF value - uint16_t value; - if (modeActivationConditions(i)->range.startStep > 0) { - value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.startStep - 1)); - } else { - value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.endStep + 1)); - } - // Initialize ARM AUX channel to OFF value - rcChannel_t *armChannel = &rcChannels[modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT]; - armChannel->raw = value; - armChannel->data = value; - } + if (!dualRxEnabled) { + linkConfigs[RX_LINK_SECONDARY].receiverType = RX_TYPE_NONE; } - switch (rxConfig()->receiverType) { + for (int linkIndex = 0; linkIndex < RX_LINK_COUNT; linkIndex++) { + rxLinkState_t *link = &rxLinks[linkIndex]; + rxConfig_t *linkConfig = &linkConfigs[linkIndex]; + link->runtimeConfig.receiverType = linkConfig->receiverType; + + switch (linkConfig->receiverType) { #ifdef USE_SERIAL_RX case RX_TYPE_SERIAL: - if (!serialRxInit(rxConfig(), &rxRuntimeConfig)) { - rxConfigMutable()->receiverType = RX_TYPE_NONE; - rxRuntimeConfig.rcReadRawFn = nullReadRawRC; - rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; + { + const serialPortFunction_e linkPortFunction = linkIndex == RX_LINK_PRIMARY ? FUNCTION_RX_SERIAL : FUNCTION_RX_SERIAL_SECONDARY; + if (!serialRxInit(linkConfig, &link->runtimeConfig, linkPortFunction)) { + if (linkIndex == RX_LINK_PRIMARY) { + rxConfigMutable()->receiverType = RX_TYPE_NONE; + } else { + rxConfigMutable()->receiverTypeSecondary = RX_TYPE_NONE; + } + link->runtimeConfig.receiverType = RX_TYPE_NONE; + link->runtimeConfig.rcReadRawFn = nullReadRawRC; + link->runtimeConfig.rcFrameStatusFn = nullFrameStatus; + link->runtimeConfig.rcProcessFrameFn = nullProcessFrame; } break; + } #endif #ifdef USE_RX_MSP case RX_TYPE_MSP: - rxMspInit(rxConfig(), &rxRuntimeConfig); + rxMspInit(linkConfig, &link->runtimeConfig); break; #endif #ifdef USE_RX_SIM - case RX_TYPE_SIM: - rxSimInit(rxConfig(), &rxRuntimeConfig); - break; + case RX_TYPE_SIM: + rxSimInit(linkConfig, &link->runtimeConfig); + break; #endif default: case RX_TYPE_NONE: - rxConfigMutable()->receiverType = RX_TYPE_NONE; - rxRuntimeConfig.rcReadRawFn = nullReadRawRC; - rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; + link->runtimeConfig.receiverType = RX_TYPE_NONE; + link->runtimeConfig.rcReadRawFn = nullReadRawRC; + link->runtimeConfig.rcFrameStatusFn = nullFrameStatus; + link->runtimeConfig.rcProcessFrameFn = nullProcessFrame; break; + } + + link->runtimeConfig.channelCount = MIN(MAX_SUPPORTED_RC_CHANNEL_COUNT, link->runtimeConfig.channelCount); } rxUpdateRSSISource(); @@ -336,7 +483,7 @@ void rxInit(void) } #endif - rxChannelCount = MIN(MAX_SUPPORTED_RC_CHANNEL_COUNT, rxRuntimeConfig.channelCount); + rxSetActiveLink(RX_LINK_PRIMARY); } void rxUpdateRSSISource(void) @@ -379,7 +526,7 @@ uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapE bool rxIsReceivingSignal(void) { - return rxSignalReceived; + return rxSignalReceived && rxFlightChannelsValid; } bool rxAreFlightChannelsValid(void) @@ -387,6 +534,101 @@ bool rxAreFlightChannelsValid(void) return rxFlightChannelsValid; } +rxLink_e rxGetActiveLink(void) +{ + return activeLink; +} + +static bool rxLinkHasValidSignal(const rxLinkState_t *link) +{ + return link->signalReceived && link->flightChannelsValid; +} + +bool rxIsLinkReceivingSignal(rxLink_e link) +{ + if ((unsigned)link >= RX_LINK_COUNT) { + return false; + } + + return rxLinkHasValidSignal(&rxLinks[link]); +} + +bool rxIsPrimaryFailsafe(void) +{ + return !rxIsLinkReceivingSignal(RX_LINK_PRIMARY); +} + +rxLinkStatistics_t *rxGetLinkStatisticsMutable(rxLink_e link) +{ + if ((unsigned)link >= RX_LINK_COUNT) { + return NULL; + } + + return &rxLinks[link].statistics; +} + +void rxLinkStatisticsUpdated(rxLink_e link) +{ + if (link == activeLink) { + rxLinkStatistics = rxLinks[link].statistics; + } +} + +static void rxApplyActiveLink(rxLink_e link) +{ + activeLink = link; + + rxRuntimeConfig = rxLinks[link].runtimeConfig; + rxLinkStatistics = rxLinks[link].statistics; + + rxSignalReceived = rxLinks[link].signalReceived; + rxFlightChannelsValid = rxLinks[link].flightChannelsValid; + memcpy(rcChannels, rxLinks[link].channels, sizeof(rcChannels)); +} + +void rxSetActiveLink(rxLink_e link) +{ + if ((unsigned)link >= RX_LINK_COUNT) { + return; + } + + if (!dualRxEnabled && link != RX_LINK_PRIMARY) { + return; + } + + rxApplyActiveLink(link); +} + +static void rxSelectActiveLink(void) +{ + if (!dualRxEnabled) { + rxApplyActiveLink(RX_LINK_PRIMARY); + return; + } + + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RX_LINK)) { + const rxLink_e forcedLink = logicConditionValuesByType[LOGIC_CONDITION_RX_SELECT_OVERRIDE] == RX_LINK_SECONDARY ? RX_LINK_SECONDARY : RX_LINK_PRIMARY; + // Only honour the forced link while it actually has signal; otherwise fall + // through to automatic selection so a dead forced link can't strand control + // on stale channel data with failsafe suppressed. + if (rxLinkHasValidSignal(&rxLinks[forcedLink])) { + rxApplyActiveLink(forcedLink); + return; + } + } + + const bool primaryValid = rxLinkHasValidSignal(&rxLinks[RX_LINK_PRIMARY]); + const bool secondaryValid = rxLinkHasValidSignal(&rxLinks[RX_LINK_SECONDARY]); + + if (primaryValid) { + rxApplyActiveLink(RX_LINK_PRIMARY); + } else if (secondaryValid) { + rxApplyActiveLink(RX_LINK_SECONDARY); + } else { + rxApplyActiveLink(RX_LINK_PRIMARY); + } +} + void suspendRxSignal(void) { failsafeOnRxSuspend(); @@ -403,42 +645,51 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) { UNUSED(currentDeltaTime); - if (rxSignalReceived) { - if (currentTimeUs >= needRxSignalBefore) { - rxSignalReceived = false; + bool result = false; + + const int linkCount = dualRxEnabled ? RX_LINK_COUNT : 1; + for (int linkIndex = 0; linkIndex < linkCount; linkIndex++) { + rxLinkState_t *link = &rxLinks[linkIndex]; + + if (link->signalReceived && currentTimeUs >= link->needRxSignalBefore) { + link->signalReceived = false; } - } - const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig); + const uint8_t frameStatus = link->runtimeConfig.rcFrameStatusFn(&link->runtimeConfig); + link->frameStatus = frameStatus; - if (frameStatus & RX_FRAME_COMPLETE) { - // RX_FRAME_COMPLETE updated the failsafe status regardless - rxSignalReceived = (frameStatus & RX_FRAME_FAILSAFE) == 0; - needRxSignalBefore = currentTimeUs + rxRuntimeConfig.rxSignalTimeout; - rxDataProcessingRequired = true; - } - else if ((frameStatus & RX_FRAME_FAILSAFE) && rxSignalReceived) { - // All other receiver statuses are allowed to report failsafe, but not allowed to leave it - rxSignalReceived = false; - } + if (frameStatus & RX_FRAME_COMPLETE) { + link->signalReceived = (frameStatus & RX_FRAME_FAILSAFE) == 0; + link->needRxSignalBefore = currentTimeUs + link->runtimeConfig.rxSignalTimeout; + link->dataProcessingRequired = true; + } else if ((frameStatus & RX_FRAME_FAILSAFE) && link->signalReceived) { + link->signalReceived = false; + } - if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) { - auxiliaryProcessingRequired = true; - } + if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) { + link->auxiliaryProcessingRequired = true; + } - if (cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) { - rxDataProcessingRequired = true; - } + if (cmpTimeUs(currentTimeUs, link->nextUpdateAtUs) > 0) { + link->dataProcessingRequired = true; + } - bool result = rxDataProcessingRequired || auxiliaryProcessingRequired; + result = result || link->dataProcessingRequired || link->auxiliaryProcessingRequired; + } #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - if (rxConfig()->receiverType != RX_TYPE_MSP) { + if (rxRuntimeConfig.receiverType != RX_TYPE_MSP) { mspOverrideDataProcessingRequired = mspOverrideUpdateCheck(currentTimeUs, currentDeltaTime); result = result || mspOverrideDataProcessingRequired; } #endif + if (!result && !isRxSuspended) { + // Re-select even when no frame needs processing so a link that has just + // timed out hands control over without waiting for the next (absent) frame. + rxSelectActiveLink(); + } + return result; } @@ -448,67 +699,75 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) const timeMs_t currentTimeMs = millis(); #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - if ((rxConfig()->receiverType != RX_TYPE_MSP) && mspOverrideDataProcessingRequired) { + if ((rxRuntimeConfig.receiverType != RX_TYPE_MSP) && mspOverrideDataProcessingRequired) { mspOverrideCalculateChannels(currentTimeUs); } #endif - if (auxiliaryProcessingRequired) { - auxiliaryProcessingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig); - } + bool processed = false; - if (!rxDataProcessingRequired) { - return false; - } + const int linkCount = dualRxEnabled ? RX_LINK_COUNT : 1; + for (int linkIndex = 0; linkIndex < linkCount; linkIndex++) { + rxLinkState_t *link = &rxLinks[linkIndex]; - rxDataProcessingRequired = false; - rxNextUpdateAtUs = currentTimeUs + DELAY_10_HZ; + if (link->auxiliaryProcessingRequired) { + link->auxiliaryProcessingRequired = !link->runtimeConfig.rcProcessFrameFn(&link->runtimeConfig); + } - // If RX is suspended, do not process any data - if (isRxSuspended) { - return true; - } + if (!link->dataProcessingRequired) { + continue; + } - rxFlightChannelsValid = true; + link->dataProcessingRequired = false; + link->nextUpdateAtUs = currentTimeUs + DELAY_10_HZ; + processed = true; - // Read and process channel data - for (int channel = 0; channel < rxChannelCount; channel++) { - const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); + if (isRxSuspended) { + continue; + } - // sample the channel - uint16_t sample = (*rxRuntimeConfig.rcReadRawFn)(&rxRuntimeConfig, rawChannel); + link->flightChannelsValid = true; - // apply the rx calibration to flight channel - if (channel < NON_AUX_CHANNEL_COUNT && sample != 0) { - sample = scaleRange(sample, rxChannelRangeConfigs(channel)->min, rxChannelRangeConfigs(channel)->max, PWM_RANGE_MIN, PWM_RANGE_MAX); - sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX); - } + const uint8_t channelCount = MIN(MAX_SUPPORTED_RC_CHANNEL_COUNT, link->runtimeConfig.channelCount); + link->runtimeConfig.channelCount = channelCount; + + for (int channel = 0; channel < channelCount; channel++) { + const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); - // Store as rxRaw - rcChannels[channel].raw = sample; + uint16_t sample = (*link->runtimeConfig.rcReadRawFn)(&link->runtimeConfig, rawChannel); - // Apply invalid pulse value logic - if (!isRxPulseValid(sample)) { - sample = rcChannels[channel].data; // hold channel, replace with old value - if ((currentTimeMs > rcChannels[channel].expiresAt) && (channel < NON_AUX_CHANNEL_COUNT)) { - rxFlightChannelsValid = false; + if (channel < NON_AUX_CHANNEL_COUNT && sample != 0) { + sample = scaleRange(sample, rxChannelRangeConfigs(channel)->min, rxChannelRangeConfigs(channel)->max, PWM_RANGE_MIN, PWM_RANGE_MAX); + sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX); } - } else { - rcChannels[channel].expiresAt = currentTimeMs + MAX_INVALID_RX_PULSE_TIME; + + link->channels[channel].raw = sample; + + if (!isRxPulseValid(sample)) { + sample = link->channels[channel].data; + if ((currentTimeMs > link->channels[channel].expiresAt) && (channel < NON_AUX_CHANNEL_COUNT)) { + link->flightChannelsValid = false; + } + } else { + link->channels[channel].expiresAt = currentTimeMs + MAX_INVALID_RX_PULSE_TIME; + } + + rcStaging[channel] = sample; } - // Save channel value - rcStaging[channel] = sample; + if (link->flightChannelsValid && link->signalReceived) { + for (int channel = 0; channel < channelCount; channel++) { + link->channels[channel].data = rcStaging[channel]; + } + } } - // Update channel input value if receiver is not in failsafe mode - // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained - if (rxFlightChannelsValid && rxSignalReceived) { - for (int channel = 0; channel < rxChannelCount; channel++) { - rcChannels[channel].data = rcStaging[channel]; - } + if (isRxSuspended) { + return processed; } + rxSelectActiveLink(); + #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe()) { mspOverrideChannels(rcChannels); @@ -540,15 +799,18 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } } - // Update failsafe - if (rxFlightChannelsValid && rxSignalReceived) { + // Update failsafe — main failsafe only fires when both RX links are down + if (rxIsLinkReceivingSignal(RX_LINK_PRIMARY) || (dualRxEnabled && rxIsLinkReceivingSignal(RX_LINK_SECONDARY))) { failsafeOnValidDataReceived(); } else { failsafeOnValidDataFailed(); } - rcSampleIndex++; - return true; + if (processed) { + rcSampleIndex++; + } + + return processed; } void parseRcChannels(const char *input) @@ -645,7 +907,11 @@ static void updateRSSIFromADC(void) static void updateRSSIFromProtocol(void) { - setRSSIValue(lqTrackerGet(&rxLQTracker), RSSI_SOURCE_RX_PROTOCOL, false); + if (!rxRuntimeConfig.lqTracker) { + return; + } + + setRSSIValue(lqTrackerGet(rxRuntimeConfig.lqTracker), RSSI_SOURCE_RX_PROTOCOL, false); } void updateRSSI(timeUs_t currentTimeUs) @@ -734,3 +1000,14 @@ uint16_t lqTrackerGet(rxLinkQualityTracker_e * lqTracker) return lqTracker->lqValue; } + +#ifdef UNIT_TEST +rxRuntimeConfig_t *rxTestRuntimeConfig(rxLink_e link) +{ + if ((unsigned)link >= RX_LINK_COUNT) { + return NULL; + } + + return &rxLinks[link].runtimeConfig; +} +#endif diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 5b5bcea50c3..b8ffd05a6aa 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -24,6 +24,7 @@ #include "common/tristate.h" #include "config/parameter_group.h" +#include "io/serial.h" #define STICK_CHANNEL_COUNT 4 @@ -107,10 +108,16 @@ PG_DECLARE_ARRAY(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeCo typedef struct rxConfig_s { uint8_t receiverType; // RC receiver type (rxReceiverType_e enum) + uint8_t dualRxEnabled; // Enable dual RX link handling uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // Type of UART-based receiver (rxSerialReceiverType_e enum). Only used if receiverType is RX_TYPE_SERIAL uint8_t serialrx_inverted; // Flip the default inversion of the protocol - e.g. sbus (Futaba, FrSKY) is inverted if this is false, uninverted if it's true. Support for uninverted OpenLRS (and modified FrSKY) receivers. uint8_t halfDuplex; // allow rx to operate in half duplex mode. From tristate_e. + uint8_t receiverTypeSecondary; // Secondary RX type + uint8_t serialrx_provider_secondary; // Secondary UART-based RX provider + uint8_t serialrx_inverted_secondary; // Secondary inversion flag + uint8_t halfDuplexSecondary; // Secondary half duplex flag. From tristate_e. + uint16_t sbusSyncIntervalSecondary; #ifdef USE_SPEKTRUM_BIND uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot @@ -145,6 +152,25 @@ typedef struct rxLinkQualityTracker_s { uint32_t lqValue; } rxLinkQualityTracker_e; +typedef enum { + RX_LINK_PRIMARY = 0, + RX_LINK_SECONDARY, + RX_LINK_COUNT +} rxLink_e; + +typedef struct rxLinkStatistics_s { + int16_t uplinkRSSI; // RSSI value in dBm + uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100] + uint8_t downlinkLQ; // A protocol specific measure of the link quality in [0..100] + int8_t uplinkSNR; // The SNR of the uplink in dB + uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz] + uint16_t uplinkTXPower; // power in mW + uint16_t downlinkTXPower; // power in mW + uint8_t activeAntenna; + char band[4]; + char mode[6]; +} rxLinkStatistics_t; + struct rxRuntimeConfig_s; typedef struct rxRuntimeConfig_s rxRuntimeConfig_t; @@ -161,6 +187,8 @@ typedef struct rxRuntimeConfig_s { rcFrameStatusFnPtr rcFrameStatusFn; rcProcessFrameFnPtr rcProcessFrameFn; rxLinkQualityTracker_e * lqTracker; // Pointer to a + rxLinkStatistics_t *linkStatistics; + rxReceiverType_e receiverType; uint16_t *channelData; void *frameData; } rxRuntimeConfig_t; @@ -180,24 +208,6 @@ typedef enum { RSSI_SOURCE_MSP, } rssiSource_e; -typedef struct rxLinkStatistics_s { - int16_t uplinkRSSI; // RSSI value in dBm - uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100] - uint8_t downlinkLQ; // A protocol specific measure of the link quality in [0..100] - int8_t uplinkSNR; // The SNR of the uplink in dB - uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz] - uint16_t uplinkTXPower; // power in mW - uint16_t downlinkTXPower; // power in mW - uint8_t activeAntenna; - char band[4]; - char mode[6]; -} rxLinkStatistics_t; - -typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data -typedef uint8_t (*rcFrameStatusFnPtr)(rxRuntimeConfig_t *rxRuntimeConfig); -typedef bool (*rcProcessFrameFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig); -typedef uint16_t (*rcGetLinkQualityPtr)(const rxRuntimeConfig_t *rxRuntimeConfig); - extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount extern rxLinkStatistics_t rxLinkStatistics; void lqTrackerReset(rxLinkQualityTracker_e * lqTracker); @@ -236,3 +246,13 @@ int16_t rxGetChannelValue(unsigned channelNumber); // MSP aux channel overlay (CH13-CH32). Sets a channel value that persists // across RX update cycles. value=0 ignores that channel and skips it. void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value); + +rxLink_e rxGetActiveLink(void); +void rxSetActiveLink(rxLink_e link); +bool rxIsLinkReceivingSignal(rxLink_e link); +bool rxIsPrimaryFailsafe(void); +rxLinkStatistics_t *rxGetLinkStatisticsMutable(rxLink_e link); +void rxLinkStatisticsUpdated(rxLink_e link); +#ifdef UNIT_TEST +rxRuntimeConfig_t *rxTestRuntimeConfig(rxLink_e link); +#endif diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index f08267eeb1d..5279357dc52 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -67,12 +67,15 @@ typedef struct sbusFrameData_s { uint8_t buffer[SBUS_FRAME_SIZE]; uint8_t position; timeUs_t lastActivityTimeUs; + timeUs_t frameTime; + uint16_t syncInterval; + uint8_t activeTelemetryPage; + bool shortFrameInterval; + bool isSbus2; } sbusFrameData_t; -static uint8_t sbus2ActiveTelemetryPage = 0; -static uint8_t sbus2ActiveTelemetrySlot = 0; -static uint8_t sbus2ShortFrameInterval = 0; -timeUs_t frameTime = 0; +static sbusFrameData_t sbusFrameData[RX_LINK_COUNT]; +static serialPort_t *sbusPorts[RX_LINK_COUNT]; // Receive ISR callback static void sbusDataReceive(uint16_t c, void *data) @@ -82,14 +85,14 @@ static void sbusDataReceive(uint16_t c, void *data) const timeDelta_t timeSinceLastByteUs = cmpTimeUs(currentTimeUs, sbusFrameData->lastActivityTimeUs); sbusFrameData->lastActivityTimeUs = currentTimeUs; - const int32_t syncInterval = sbus2ShortFrameInterval + const int32_t syncInterval = sbusFrameData->shortFrameInterval ? ((6300 - SBUS_BYTE_TIME_US(25)) / 2) - : rxConfig()->sbusSyncInterval; + : sbusFrameData->syncInterval; // Handle inter-frame gap. We dwell in STATE_SBUS_WAIT_SYNC state ignoring all incoming bytes until we get long enough quite period on the wire if ((sbusFrameData->state == STATE_SBUS_WAIT_SYNC && timeSinceLastByteUs >= syncInterval) - || (rxConfig()->serialrx_provider == SERIALRX_SBUS2 && timeSinceLastByteUs >= SBUS_BYTE_TIME_US(3))) { + || (sbusFrameData->isSbus2 && timeSinceLastByteUs >= SBUS_BYTE_TIME_US(3))) { sbusFrameData->state = STATE_SBUS_SYNC; } else if ((sbusFrameData->state == STATE_SBUS_PAYLOAD || sbusFrameData->state == STATE_SBUS26_PAYLOAD) && timeSinceLastByteUs >= SBUS_BYTE_TIME_US(3)) { // payload is pausing too long, possible if some telemetry have been sent between frames, or false positves mid frame @@ -125,14 +128,13 @@ static void sbusDataReceive(uint16_t c, void *data) case 0x24: // S.BUS 2 telemetry page 3 case 0x34: // S.BUS 2 telemetry page 4 if(frame->endByte & 0x4) { - sbus2ActiveTelemetryPage = (frame->endByte >> 4) & 0xF; - frameTime = currentTimeUs; + sbusFrameData->activeTelemetryPage = (frame->endByte >> 4) & 0xF; + sbusFrameData->frameTime = currentTimeUs; } else if(frame->endByte == 0x08) { - sbus2ShortFrameInterval = 1; + sbusFrameData->shortFrameInterval = true; } else { - sbus2ActiveTelemetryPage = 0; - sbus2ActiveTelemetrySlot = 0; - frameTime = -1; + sbusFrameData->activeTelemetryPage = 0; + sbusFrameData->frameTime = -1; } frameValid = true; @@ -167,7 +169,7 @@ static void sbusDataReceive(uint16_t c, void *data) case 0x14: // S.BUS 2 telemetry page 2 case 0x24: // S.BUS 2 telemetry page 3 case 0x34: // S.BUS 2 telemetry page 4 - frameTime = -1; // ignore this one, as you can't fit telemetry between this and the next frame. + sbusFrameData->frameTime = -1; // ignore this one, as you can't fit telemetry between this and the next frame. frameValid = true; sbusFrameData->state = STATE_SBUS_SYNC; // Next piece of data should be a sync byte break; @@ -224,13 +226,20 @@ static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) return retValue; } -static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint32_t sbusBaudRate) +static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint32_t sbusBaudRate, serialPortFunction_e portFunction) { - static uint16_t sbusChannelData[SBUS_MAX_CHANNEL]; - static sbusFrameData_t sbusFrameData = { .is26channels = false}; + // Per-link storage so a primary and secondary SBUS receiver don't share + // parser/channel state. Primary stays index 0, so the single-RX path is + // unchanged. + static uint16_t sbusChannelData[RX_LINK_COUNT][SBUS_MAX_CHANNEL]; + const unsigned link = (portFunction == FUNCTION_RX_SERIAL_SECONDARY) ? RX_LINK_SECONDARY : RX_LINK_PRIMARY; - rxRuntimeConfig->channelData = sbusChannelData; - rxRuntimeConfig->frameData = &sbusFrameData; + memset(&sbusFrameData[link], 0, sizeof(sbusFrameData[link])); + sbusFrameData[link].syncInterval = rxConfig->sbusSyncInterval; + sbusFrameData[link].isSbus2 = rxConfig->serialrx_provider == SERIALRX_SBUS2; + + rxRuntimeConfig->channelData = sbusChannelData[link]; + rxRuntimeConfig->frameData = &sbusFrameData[link]; sbusChannelsInit(rxRuntimeConfig); @@ -238,7 +247,7 @@ static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeC rxRuntimeConfig->rcFrameStatusFn = sbusFrameStatus; - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(portFunction); if (!portConfig) { return false; } @@ -250,9 +259,9 @@ static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeC #endif serialPort_t *sBusPort = openSerialPort(portConfig->identifier, - FUNCTION_RX_SERIAL, + portFunction, sbusDataReceive, - &sbusFrameData, + &sbusFrameData[link], sbusBaudRate, (portShared || rxConfig->serialrx_provider == SERIALRX_SBUS2) ? MODE_RXTX : MODE_RX, SBUS_PORT_OPTIONS | @@ -260,9 +269,10 @@ static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeC ((rxConfig->serialrx_provider == SERIALRX_SBUS2) ? SERIAL_BIDIR : 0) | (tristateWithDefaultOffIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0) ); + sbusPorts[link] = sBusPort; #ifdef USE_TELEMETRY - if (portShared || (rxConfig->serialrx_provider == SERIALRX_SBUS2)) { + if (link == RX_LINK_PRIMARY && (portShared || rxConfig->serialrx_provider == SERIALRX_SBUS2)) { telemetrySharedPort = sBusPort; } #endif @@ -270,30 +280,30 @@ static bool sbusInitEx(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeC return sBusPort != NULL; } -bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { - return sbusInitEx(rxConfig, rxRuntimeConfig, SBUS_BAUDRATE); + return sbusInitEx(rxConfig, rxRuntimeConfig, SBUS_BAUDRATE, portFunction); } -bool sbusInitFast(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool sbusInitFast(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { - return sbusInitEx(rxConfig, rxRuntimeConfig, SBUS_BAUDRATE_FAST); + return sbusInitEx(rxConfig, rxRuntimeConfig, SBUS_BAUDRATE_FAST, portFunction); } #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2) -timeUs_t sbusGetLastFrameTime(void) { - return frameTime; +serialPort_t *sbusGetTelemetryPort(rxLink_e link) +{ + return sbusPorts[link]; } -uint8_t sbusGetCurrentTelemetryNextSlot(void) +timeUs_t sbusGetLastFrameTime(rxLink_e link) { - uint8_t current = sbus2ActiveTelemetrySlot; - sbus2ActiveTelemetrySlot++; - return current; + return sbusFrameData[link].frameTime; } -uint8_t sbusGetCurrentTelemetryPage(void) { - return sbus2ActiveTelemetryPage; +uint8_t sbusGetCurrentTelemetryPage(rxLink_e link) +{ + return sbusFrameData[link].activeTelemetryPage; } #endif // USE_TELEMETRY && USE_SBUS2_TELEMETRY diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h index 20ed82e2dae..1adabbea685 100644 --- a/src/main/rx/sbus.h +++ b/src/main/rx/sbus.h @@ -21,13 +21,13 @@ #include "rx/rx.h" -bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig); -bool sbusInitFast(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction); +bool sbusInitFast(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction); #define SBUS_BYTE_TIME_US(bytes) MS2US(10 * 12 * bytes) // 10us per bit * (1 start + 8 data + 1 parity + 2 stop) * number of bytes #ifdef USE_TELEMETRY_SBUS2 -uint8_t sbusGetCurrentTelemetryPage(void); -uint8_t sbusGetCurrentTelemetryNextSlot(void); -timeUs_t sbusGetLastFrameTime(void); +serialPort_t *sbusGetTelemetryPort(rxLink_e link); +uint8_t sbusGetCurrentTelemetryPage(rxLink_e link); +timeUs_t sbusGetLastFrameTime(rxLink_e link); #endif diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 31e4f2f04a3..d747a7455a5 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -292,7 +292,7 @@ void srxlRxWriteTelemetryData(const void *data, int len) } #endif -bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { rxRuntimeConfigPtr = rxRuntimeConfig; @@ -316,7 +316,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig rxRuntimeConfig->rcReadRawFn = spektrumReadRawRC; rxRuntimeConfig->rcFrameStatusFn = spektrumFrameStatus; - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(portFunction); if (!portConfig) { return false; } @@ -328,7 +328,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig #endif serialPort = openSerialPort(portConfig->identifier, - FUNCTION_RX_SERIAL, + portFunction, spektrumDataReceive, NULL, SPEKTRUM_BAUDRATE, diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h index c63396b4a9e..bba8a4d7a8b 100644 --- a/src/main/rx/spektrum.h +++ b/src/main/rx/spektrum.h @@ -17,6 +17,8 @@ #pragma once +#include "rx/rx.h" + #define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12 #define SPEKTRUM_2048_CHANNEL_COUNT 12 #define SPEKTRUM_1024_CHANNEL_COUNT 7 @@ -35,8 +37,8 @@ extern bool srxlEnabled; void spektrumBind(rxConfig_t *rxConfig); -bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction); bool srxlTelemetryBufferEmpty(void); void srxlRxWriteTelemetryData(const void *data, int len); -bool srxlRxIsActive(void); \ No newline at end of file +bool srxlRxIsActive(void); diff --git a/src/main/rx/srxl2.c b/src/main/rx/srxl2.c index 95ab941027d..eb5f6e112bd 100644 --- a/src/main/rx/srxl2.c +++ b/src/main/rx/srxl2.c @@ -478,7 +478,7 @@ void srxl2RxWriteData(const void *data, int len) writeBufferIdx = len; } -bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { static uint16_t channelData[SRXL2_MAX_CHANNELS]; for (size_t i = 0; i < SRXL2_MAX_CHANNELS; ++i) { @@ -494,7 +494,7 @@ bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->rcFrameStatusFn = srxl2FrameStatus; rxRuntimeConfig->rcProcessFrameFn = srxl2ProcessFrame; - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(portFunction); if (!portConfig) { return false; } @@ -507,7 +507,7 @@ bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) options |= SERIAL_BIDIR; } - serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, srxl2DataReceive, + serialPort = openSerialPort(portConfig->identifier, portFunction, srxl2DataReceive, NULL, SRXL2_PORT_BAUDRATE_DEFAULT, SRXL2_PORT_MODE, options); if (!serialPort) { diff --git a/src/main/rx/srxl2.h b/src/main/rx/srxl2.h index 5c3bb142760..a0ec7cc1537 100644 --- a/src/main/rx/srxl2.h +++ b/src/main/rx/srxl2.h @@ -6,7 +6,7 @@ struct sbuf_s; -bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction); bool srxl2RxIsActive(void); void srxl2RxWriteData(const void *data, int len); bool srxl2TelemetryRequested(void); diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 7097b6c9fd2..4cd6d214daf 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -162,7 +162,7 @@ static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t return sumdChannels[chan] / 8; } -bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction) { UNUSED(rxConfig); @@ -170,7 +170,7 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->rcReadRawFn = sumdReadRawRC; rxRuntimeConfig->rcFrameStatusFn = sumdFrameStatus; - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(portFunction); if (!portConfig) { return false; } @@ -182,7 +182,7 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) #endif serialPort_t *sumdPort = openSerialPort(portConfig->identifier, - FUNCTION_RX_SERIAL, + portFunction, sumdDataReceive, NULL, SUMD_BAUDRATE, diff --git a/src/main/rx/sumd.h b/src/main/rx/sumd.h index 97f1a2224d8..5db90db8569 100644 --- a/src/main/rx/sumd.h +++ b/src/main/rx/sumd.h @@ -17,4 +17,6 @@ #pragma once -bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +#include "rx/rx.h" + +bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, serialPortFunction_e portFunction); diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index e79c32b1832..5c33b57507e 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -149,21 +149,6 @@ void taskSystem(timeUs_t currentTimeUs) #else UNUSED(currentTimeUs); -#if defined(SITL_BUILD) - if (sitlLoadWindowStartUs == 0) { - sitlLoadWindowStartUs = currentTimeUs; - sitlLoadBusyTimeUs = 0; - return; - } - - const timeDelta_t elapsedUs = cmpTimeUs(currentTimeUs, sitlLoadWindowStartUs); - if (elapsedUs > 0) { - const timeUs_t loadPercent = (100U * sitlLoadBusyTimeUs) / (timeUs_t)elapsedUs; - averageSystemLoadPercent = loadPercent > 100U ? 100U : (uint16_t)loadPercent; - sitlLoadWindowStartUs = currentTimeUs; - sitlLoadBusyTimeUs = 0; - } -#else // Calculate system load if (totalWaitingTasksSamples > 0) { averageSystemLoadPercent = 100 * totalWaitingTasks / totalWaitingTasksSamples; diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 16392362830..fca1e995cb2 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -79,6 +79,10 @@ #define CRSF_MSP_BUFFER_SIZE 96 #define CRSF_MSP_LENGTH_OFFSET 1 +#define CRSF_MSP_START_MASK 0x10 +#define CRSF_MSP_VERSION_MASK 0x60 +#define CRSF_MSP_VERSION_SHIFT 5 +#define CRSF_MSP_TRANSACTION_TIMEOUT_US 1000000 static uint8_t crsfCrc; static bool crsfTelemetryEnabled; @@ -89,60 +93,127 @@ static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX]; typedef struct mspBuffer_s { uint8_t bytes[CRSF_MSP_BUFFER_SIZE]; int len; + uint16_t expectedPayloadLength; + uint16_t receivedPayloadLength; + timeUs_t lastActivityTimeUs; + bool started; + bool complete; } mspBuffer_t; -static mspBuffer_t mspRxBuffer; +static mspBuffer_t mspRxBuffers[RX_LINK_COUNT]; +static bool mspResponsePending[RX_LINK_COUNT]; +static rxLink_e mspIngressOwner = RX_LINK_COUNT; void initCrsfMspBuffer(void) { - mspRxBuffer.len = 0; + memset(mspRxBuffers, 0, sizeof(mspRxBuffers)); + memset(mspResponsePending, 0, sizeof(mspResponsePending)); + mspIngressOwner = RX_LINK_COUNT; } -bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength) +bool bufferCrsfMspFrame(rxLink_e link, uint8_t *frameStart, int frameLength) { - if (mspRxBuffer.len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) { - return false; + mspBuffer_t *buffer = &mspRxBuffers[link]; + const timeUs_t currentTimeUs = micros(); + const uint8_t status = frameStart[0]; + const bool start = status & CRSF_MSP_START_MASK; + + if (mspIngressOwner != RX_LINK_COUNT && !mspResponsePending[mspIngressOwner]) { + mspBuffer_t *ownerBuffer = &mspRxBuffers[mspIngressOwner]; + if (cmpTimeUs(currentTimeUs, ownerBuffer->lastActivityTimeUs) > CRSF_MSP_TRANSACTION_TIMEOUT_US) { + ownerBuffer->len = 0; + ownerBuffer->started = false; + ownerBuffer->complete = false; + mspIngressOwner = RX_LINK_COUNT; + } + } + + if (start) { + if (mspIngressOwner != RX_LINK_COUNT) { + return false; + } + mspIngressOwner = link; + buffer->len = 0; + buffer->receivedPayloadLength = 0; + buffer->started = true; + buffer->complete = false; + + const uint8_t version = (status & CRSF_MSP_VERSION_MASK) >> CRSF_MSP_VERSION_SHIFT; + if (version == 1) { + if (frameLength < 3) { + buffer->started = false; + mspIngressOwner = RX_LINK_COUNT; + return false; + } + buffer->expectedPayloadLength = frameStart[1]; + buffer->receivedPayloadLength = frameLength - 3; + } else { + if (frameLength < 6) { + buffer->started = false; + mspIngressOwner = RX_LINK_COUNT; + return false; + } + buffer->expectedPayloadLength = frameStart[4] | (frameStart[5] << 8); + buffer->receivedPayloadLength = frameLength - 6; + } } else { - uint8_t *p = mspRxBuffer.bytes + mspRxBuffer.len; - *p++ = frameLength; - memcpy(p, frameStart, frameLength); - mspRxBuffer.len += CRSF_MSP_LENGTH_OFFSET + frameLength; - return true; + if (mspIngressOwner != link || !buffer->started || frameLength < 1) { + return false; + } + buffer->receivedPayloadLength += frameLength - 1; + } + + if (buffer->len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) { + buffer->started = false; + buffer->complete = false; + buffer->len = 0; + mspIngressOwner = RX_LINK_COUNT; + return false; } + + buffer->lastActivityTimeUs = currentTimeUs; + uint8_t *p = buffer->bytes + buffer->len; + *p++ = frameLength; + memcpy(p, frameStart, frameLength); + buffer->len += CRSF_MSP_LENGTH_OFFSET + frameLength; + buffer->complete = buffer->receivedPayloadLength >= buffer->expectedPayloadLength; + return true; } -bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn) +static bool handleCrsfMspFrameBuffer(rxLink_e link, uint8_t payloadSize, mspResponseFnPtr responseFn) { - static bool replyPending = false; - if (replyPending) { - if (crsfRxIsTelemetryBufEmpty()) { - replyPending = sendMspReply(payloadSize, responseFn); + mspBuffer_t *buffer = &mspRxBuffers[link]; + + if (mspResponsePending[link]) { + if (crsfRxIsTelemetryBufEmpty(link)) { + mspResponsePending[link] = sendMspReply(payloadSize, responseFn); } - return replyPending; + return mspResponsePending[link]; } - if (!mspRxBuffer.len) { + if (!buffer->len) { return false; } int pos = 0; while (true) { - const int mspFrameLength = mspRxBuffer.bytes[pos]; - if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) { - if (crsfRxIsTelemetryBufEmpty()) { - replyPending = sendMspReply(payloadSize, responseFn); + const int mspFrameLength = buffer->bytes[pos]; + if (handleMspFrame(&buffer->bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) { + buffer->complete = true; + if (crsfRxIsTelemetryBufEmpty(link)) { + mspResponsePending[link] = sendMspReply(payloadSize, responseFn); } else { - replyPending = true; + mspResponsePending[link] = true; } } pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength; ATOMIC_BLOCK(NVIC_PRIO_SERIALUART) { - if (pos >= mspRxBuffer.len) { - mspRxBuffer.len = 0; - return replyPending ; + if (pos >= buffer->len) { + buffer->len = 0; + return mspResponsePending[link]; } } } - return replyPending; + return mspResponsePending[link]; } #endif @@ -599,15 +670,16 @@ static uint16_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; #if defined(USE_MSP_OVER_TELEMETRY) -static bool mspReplyPending; +static bool mspRequestPending[RX_LINK_COUNT]; +static rxLink_e mspActiveLink = RX_LINK_COUNT; //Id of the last receiver MSP frame over CRSF. Needed to send response with correct frame ID -static uint8_t mspRequestOriginID = 0; +static uint8_t mspRequestOriginID[RX_LINK_COUNT]; -void crsfScheduleMspResponse(uint8_t requestOriginID) +void crsfScheduleMspResponse(rxLink_e link, uint8_t requestOriginID) { - mspReplyPending = true; - mspRequestOriginID = requestOriginID; + mspRequestPending[link] = true; + mspRequestOriginID[link] = requestOriginID; } void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize) @@ -618,16 +690,18 @@ void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize) crsfInitializeFrame(dst); sbufWriteU8(dst, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP); - crsfSerialize8(dst, mspRequestOriginID); + crsfSerialize8(dst, mspRequestOriginID[mspActiveLink]); crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); crsfSerializeData(dst, (const uint8_t*)payload, payloadSize); - crsfFinalize(dst); + sbufWriteU8(dst, crsfCrc); + sbufSwitchToReader(dst, crsfFrame); + crsfRxWriteTelemetryDataForLink(mspActiveLink, sbufPtr(dst), sbufBytesRemaining(dst)); } #endif static void processCrsf(void) { - if (!crsfRxIsTelemetryBufEmpty()) { + if (!crsfRxAreTelemetryBufsEmpty()) { return; // do nothing if telemetry ouptut buffer is not empty yet. } @@ -705,7 +779,9 @@ void initCrsfTelemetry(void) deviceInfoReplyPending = false; #if defined(USE_MSP_OVER_TELEMETRY) - mspReplyPending = false; + initCrsfMspBuffer(); + memset(mspRequestPending, 0, sizeof(mspRequestPending)); + mspActiveLink = RX_LINK_COUNT; #endif int index = 0; @@ -781,8 +857,27 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) // Send ad-hoc response frames as soon as possible #if defined(USE_MSP_OVER_TELEMETRY) - if (mspReplyPending) { - mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse); + if (mspActiveLink == RX_LINK_COUNT) { + for (rxLink_e link = RX_LINK_PRIMARY; link < RX_LINK_COUNT; link++) { + if (mspRequestPending[link]) { + mspActiveLink = link; + break; + } + } + } + if (mspActiveLink != RX_LINK_COUNT) { + if (!crsfRxIsTelemetryBufEmpty(mspActiveLink)) { + return; + } + mspRequestPending[mspActiveLink] = handleCrsfMspFrameBuffer(mspActiveLink, CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse); + if (!mspRequestPending[mspActiveLink]) { + if (mspRxBuffers[mspActiveLink].complete) { + mspRxBuffers[mspActiveLink].started = false; + mspRxBuffers[mspActiveLink].complete = false; + mspIngressOwner = RX_LINK_COUNT; + } + mspActiveLink = RX_LINK_COUNT; + } crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request return; } diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 65565888486..77f0ace2299 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -27,9 +27,9 @@ void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); void crsfScheduleDeviceInfoResponse(void); -void crsfScheduleMspResponse(uint8_t requestOriginID); +void crsfScheduleMspResponse(rxLink_e link, uint8_t requestOriginID); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); #if defined(USE_MSP_OVER_TELEMETRY) void initCrsfMspBuffer(void); -bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength); -#endif \ No newline at end of file +bool bufferCrsfMspFrame(rxLink_e link, uint8_t *frameStart, int frameLength); +#endif diff --git a/src/main/telemetry/sbus2.c b/src/main/telemetry/sbus2.c index 1e800f2f098..7247e40aac0 100644 --- a/src/main/telemetry/sbus2.c +++ b/src/main/telemetry/sbus2.c @@ -60,8 +60,8 @@ const uint8_t sbus2SlotIds[SBUS2_SLOT_COUNT] = { sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_SLOT_COUNT] = {}; uint8_t sbusTelemetryDataUsed[SBUS2_SLOT_COUNT] = {}; -static uint8_t currentSlot = 0; -static timeUs_t nextSlotTime = 0; +static uint8_t currentSlot[RX_LINK_COUNT]; +static timeUs_t nextSlotTime[RX_LINK_COUNT]; void handleSbus2Telemetry(timeUs_t currentTimeUs) { @@ -144,62 +144,48 @@ void handleSbus2Telemetry(timeUs_t currentTimeUs) #endif } -uint8_t sbus2GetTelemetrySlot(timeUs_t elapsed) -{ - UNUSED(elapsed); - if (elapsed < SBUS2_DEADTIME) { - currentSlot = 0; - nextSlotTime = 0; - return 0xFF; // skip it - } - - if(currentSlot < SBUS2_TELEMETRY_SLOTS) { - return currentSlot; - } - - return 0xFF; -} - -void sbus2IncrementTelemetrySlot(timeUs_t currentTimeUs) -{ - nextSlotTime = currentTimeUs + (SBUS2_TRANSMIT_TIME + SBUS2_SLOT_DELAY); - currentSlot++; -} - FAST_CODE void taskSendSbus2Telemetry(timeUs_t currentTimeUs) { - if (!feature(FEATURE_TELEMETRY) || !telemetrySharedPort || rxConfig()->receiverType != RX_TYPE_SERIAL || - rxConfig()->serialrx_provider != SERIALRX_SBUS2) { + if (!feature(FEATURE_TELEMETRY)) { return; } - timeUs_t elapsedTime = currentTimeUs - sbusGetLastFrameTime(); - - if (elapsedTime > MS2US(8)) { - currentSlot = 0; - nextSlotTime = 0; - return; - } + for (rxLink_e link = RX_LINK_PRIMARY; link < RX_LINK_COUNT; link++) { + const bool configured = link == RX_LINK_PRIMARY + ? rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_SBUS2 + : rxConfig()->dualRxEnabled && rxConfig()->receiverTypeSecondary == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider_secondary == SERIALRX_SBUS2; + serialPort_t *port = sbusGetTelemetryPort(link); + if (!configured || !port) { + continue; + } - if (currentTimeUs < nextSlotTime) { - return; - } + const timeUs_t elapsedTime = currentTimeUs - sbusGetLastFrameTime(link); + if (elapsedTime > MS2US(8)) { + currentSlot[link] = 0; + nextSlotTime[link] = 0; + continue; + } - uint8_t telemetryPage = sbusGetCurrentTelemetryPage(); + if (elapsedTime < SBUS2_DEADTIME) { + currentSlot[link] = 0; + nextSlotTime[link] = 0; + continue; + } - uint8_t slot = sbus2GetTelemetrySlot(elapsedTime); + if (currentTimeUs < nextSlotTime[link] || currentSlot[link] >= SBUS2_TELEMETRY_SLOTS) { + continue; + } - if(slot < SBUS2_TELEMETRY_SLOTS) { - int slotIndex = (telemetryPage * SBUS2_TELEMETRY_SLOTS) + slot; - if (slotIndex < SBUS2_SLOT_COUNT) { - if (sbusTelemetryDataUsed[slotIndex] != 0) { - // send - serialWriteBuf(telemetrySharedPort, - (const uint8_t *)&sbusTelemetryData[slotIndex], - sizeof(sbus2_telemetry_frame_t)); - } + const uint8_t telemetryPage = sbusGetCurrentTelemetryPage(link); + const int slotIndex = (telemetryPage * SBUS2_TELEMETRY_SLOTS) + currentSlot[link]; + if (slotIndex < SBUS2_SLOT_COUNT && sbusTelemetryDataUsed[slotIndex] != 0) { + serialWriteBuf(port, + (const uint8_t *)&sbusTelemetryData[slotIndex], + sizeof(sbus2_telemetry_frame_t)); } - sbus2IncrementTelemetrySlot(currentTimeUs); + nextSlotTime[link] = currentTimeUs + (SBUS2_TRANSMIT_TIME + SBUS2_SLOT_DELAY); + currentSlot[link]++; } } diff --git a/src/main/telemetry/sbus2.h b/src/main/telemetry/sbus2.h index 0fc74f3298d..46a48184316 100644 --- a/src/main/telemetry/sbus2.h +++ b/src/main/telemetry/sbus2.h @@ -64,7 +64,4 @@ void handleSbus2Telemetry(timeUs_t currentTimeUs); // time critical, send sbus2 data void taskSendSbus2Telemetry(timeUs_t currentTimeUs); -uint8_t sbus2GetTelemetrySlot(timeUs_t elapsed); -void sbus2IncrementTelemetrySlot(timeUs_t now); - #endif 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/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 9cb858f0e81..1437e478061 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -27,6 +27,13 @@ set_property(SOURCE rcdevice_unittest.cc PROPERTY depends "common/bitarray.c" "common/crc.c" "io/rcdevice.c" "io/rcdevice_cam.c" "fc/rc_modes.c" "common/maths.c") +set_property(SOURCE rx_rx_unittest.cc PROPERTY depends + "rx/rx.c" + "fc/rc_modes.c" + "common/bitarray.c" + "common/maths.c") +set_property(SOURCE rx_rx_unittest.cc PROPERTY extra_includes "../../lib/main/MAVLink") + set_property(SOURCE sensor_gyro_unittest.cc PROPERTY depends "build/debug.c" "common/maths.c" "common/calibration.c" "common/filter.c" "drivers/accgyro/accgyro_fake.c" "sensors/gyro.c" "sensors/boardalignment.c") 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 7968233c9f5..623c93fafb9 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++; } uint32_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;} diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc new file mode 100644 index 00000000000..b70ff75865b --- /dev/null +++ b/src/test/unit/rx_rx_unittest.cc @@ -0,0 +1,280 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +extern "C" { +#include "platform.h" + +#include "fc/rc_modes.h" +#include "fc/rc_controls.h" +#include "fc/settings.h" +#include "programming/logic_condition.h" +#include "rx/rx.h" +} + +static uint32_t fakeTimeUs; +static uint32_t failsafeValidCount; +static uint32_t failsafeFailedCount; +static uint8_t frameStatusForLink[RX_LINK_COUNT]; +static uint16_t channelValueForLink[RX_LINK_COUNT] = { PWM_RANGE_MIDDLE, PWM_RANGE_MIDDLE }; + +extern "C" { + uint32_t micros(void) + { + return fakeTimeUs; + } + + uint32_t millis(void) + { + return fakeTimeUs / 1000; + } + + bool feature(uint32_t mask) + { + UNUSED(mask); + return false; + } + + int16_t getRcChannelOverride(uint8_t channelNumber, int16_t defaultValue) + { + UNUSED(channelNumber); + return defaultValue; + } + + uint64_t logicConditionsGlobalFlags; + + void failsafeOnValidDataReceived(void) + { + failsafeValidCount++; + } + + void failsafeOnValidDataFailed(void) + { + failsafeFailedCount++; + } + + void failsafeOnRxSuspend(void) + { + } + + void failsafeOnRxResume(void) + { + } + + void mspOverrideInit(void) + { + } + + bool mspOverrideUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) + { + UNUSED(currentTimeUs); + UNUSED(currentDeltaTime); + return false; + } + + void mspOverrideCalculateChannels(timeUs_t currentTimeUs) + { + UNUSED(currentTimeUs); + } + + bool mspOverrideIsInFailsafe(void) + { + return false; + } + + void mspOverrideChannels(rcChannel_t *channels) + { + UNUSED(channels); + } + + uint32_t stateFlags; + uint32_t armingFlags; + int16_t rcCommand[4]; + + rcControlsConfig_t rcControlsConfig_System; + rcControlsConfig_t rcControlsConfig_Copy; + const pgRegistry_t rcControlsConfig_Registry = {}; + + // Programming framework state referenced by rxSelectActiveLink(). The override + // flag is never set in these tests, so the values are only needed to link. + int logicConditionValuesByType[LOGIC_CONDITION_LAST]; + + // Physical RX drivers are not exercised by the dual-link tests (channels are + // driven through the fake runtime configs), so the protocol init entry points + // only need to satisfy the linker. + bool srxl2RxInit(const rxConfig_t *, rxRuntimeConfig_t *, serialPortFunction_e) { return false; } + bool sbusInit(const rxConfig_t *, rxRuntimeConfig_t *, serialPortFunction_e) { return false; } + bool sbusInitFast(const rxConfig_t *, rxRuntimeConfig_t *, serialPortFunction_e) { return false; } + bool ibusInit(const rxConfig_t *, rxRuntimeConfig_t *, serialPortFunction_e) { return false; } + bool jetiExBusInit(const rxConfig_t *, rxRuntimeConfig_t *, serialPortFunction_e) { return false; } + bool crsfRxInit(const rxConfig_t *, rxRuntimeConfig_t *, serialPortFunction_e) { return false; } + bool fportRxInit(const rxConfig_t *, rxRuntimeConfig_t *, serialPortFunction_e) { return false; } + bool fport2RxInit(const rxConfig_t *, rxRuntimeConfig_t *, bool, serialPortFunction_e) { return false; } + bool ghstRxInit(const rxConfig_t *, rxRuntimeConfig_t *, serialPortFunction_e) { return false; } + void rxMspInit(const rxConfig_t *, rxRuntimeConfig_t *) {} + uint8_t rxMspGetLastChannelCount(void) { return 0; } + void rxSimInit(const rxConfig_t *, rxRuntimeConfig_t *) {} +} + +static uint8_t primaryFrameStatusFn(rxRuntimeConfig_t *) +{ + return frameStatusForLink[RX_LINK_PRIMARY]; +} + +static uint8_t secondaryFrameStatusFn(rxRuntimeConfig_t *) +{ + return frameStatusForLink[RX_LINK_SECONDARY]; +} + +static uint16_t primaryReadFn(const rxRuntimeConfig_t *, uint8_t) +{ + return channelValueForLink[RX_LINK_PRIMARY]; +} + +static uint16_t secondaryReadFn(const rxRuntimeConfig_t *, uint8_t) +{ + return channelValueForLink[RX_LINK_SECONDARY]; +} + +class RxDualLinkTest : public ::testing::Test { +protected: + void SetUp() override + { + fakeTimeUs = 0; + failsafeValidCount = 0; + failsafeFailedCount = 0; + + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + modeActivationCondition_t *cond = modeActivationConditionsMutable(i); + memset(cond, 0, sizeof(*cond)); + } + + rxConfig_t *cfg = rxConfigMutable(); + memset(cfg, 0, sizeof(*cfg)); + cfg->receiverType = RX_TYPE_NONE; + cfg->rcmap[0] = 0; + cfg->rcmap[1] = 1; + cfg->rcmap[2] = 3; + cfg->rcmap[3] = 2; + cfg->serialrx_provider = 0; + cfg->serialrx_inverted = 0; + cfg->halfDuplex = SETTING_SERIALRX_HALFDUPLEX_DEFAULT; + cfg->dualRxEnabled = true; + cfg->mincheck = SETTING_MIN_CHECK_DEFAULT; + cfg->maxcheck = SETTING_MAX_CHECK_DEFAULT; + cfg->rx_min_usec = SETTING_RX_MIN_USEC_DEFAULT; + cfg->rx_max_usec = SETTING_RX_MAX_USEC_DEFAULT; + cfg->rssi_channel = SETTING_RSSI_CHANNEL_DEFAULT; + cfg->rssiMin = SETTING_RSSI_MIN_DEFAULT; + cfg->rssiMax = SETTING_RSSI_MAX_DEFAULT; + cfg->sbusSyncInterval = SETTING_SBUS_SYNC_INTERVAL_DEFAULT; + cfg->rcFilterFrequency = SETTING_RC_FILTER_LPF_HZ_DEFAULT; + cfg->autoSmooth = SETTING_RC_FILTER_AUTO_DEFAULT; + cfg->autoSmoothFactor = SETTING_RC_FILTER_SMOOTHING_FACTOR_DEFAULT; + cfg->mspOverrideChannels = 0; + cfg->rssi_source = SETTING_RSSI_SOURCE_DEFAULT; + + resetAllRxChannelRangeConfigurations(); + + rxConfigMutable()->receiverType = RX_TYPE_NONE; + rxInit(); + + configureLink(RX_LINK_PRIMARY, primaryFrameStatusFn, primaryReadFn); + configureLink(RX_LINK_SECONDARY, secondaryFrameStatusFn, secondaryReadFn); + + frameStatusForLink[RX_LINK_PRIMARY] = RX_FRAME_PENDING; + frameStatusForLink[RX_LINK_SECONDARY] = RX_FRAME_PENDING; + } + + void configureLink(rxLink_e link, rcFrameStatusFnPtr frameFn, rcReadRawDataFnPtr readFn) + { + rxRuntimeConfig_t *runtime = rxTestRuntimeConfig(link); + runtime->channelCount = STICK_CHANNEL_COUNT; + runtime->rcFrameStatusFn = frameFn; + runtime->rcReadRawFn = readFn; + runtime->receiverType = RX_TYPE_SERIAL; + } + + void stepRx(void) + { + fakeTimeUs += 100; + rxUpdateCheck(fakeTimeUs, 0); + calculateRxChannelsAndUpdateFailsafe(fakeTimeUs); + } +}; + +TEST_F(RxDualLinkTest, PrimarySwitchesToSecondaryOnFailsafe) +{ + frameStatusForLink[RX_LINK_PRIMARY] = RX_FRAME_COMPLETE; + frameStatusForLink[RX_LINK_SECONDARY] = RX_FRAME_COMPLETE; + + stepRx(); + + EXPECT_EQ(RX_LINK_PRIMARY, rxGetActiveLink()); + EXPECT_TRUE(rxIsLinkReceivingSignal(RX_LINK_PRIMARY)); + EXPECT_FALSE(rxIsPrimaryFailsafe()); + + frameStatusForLink[RX_LINK_PRIMARY] = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; + frameStatusForLink[RX_LINK_SECONDARY] = RX_FRAME_COMPLETE; + + stepRx(); + + EXPECT_EQ(RX_LINK_SECONDARY, rxGetActiveLink()); + EXPECT_TRUE(rxIsLinkReceivingSignal(RX_LINK_SECONDARY)); + EXPECT_TRUE(rxIsPrimaryFailsafe()); + EXPECT_EQ(0U, failsafeFailedCount); +} + +TEST_F(RxDualLinkTest, SecondaryTakesControlWhenPrimaryStartsInFailsafe) +{ + frameStatusForLink[RX_LINK_PRIMARY] = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; + frameStatusForLink[RX_LINK_SECONDARY] = RX_FRAME_COMPLETE; + + stepRx(); + + EXPECT_EQ(RX_LINK_SECONDARY, rxGetActiveLink()); + EXPECT_TRUE(rxIsLinkReceivingSignal(RX_LINK_SECONDARY)); + EXPECT_TRUE(rxIsPrimaryFailsafe()); + EXPECT_EQ(1U, failsafeValidCount); +} + +TEST_F(RxDualLinkTest, MainFailsafeRequiresBothLinksDown) +{ + frameStatusForLink[RX_LINK_PRIMARY] = RX_FRAME_COMPLETE; + frameStatusForLink[RX_LINK_SECONDARY] = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; + + stepRx(); + + EXPECT_EQ(1U, failsafeValidCount); + EXPECT_EQ(0U, failsafeFailedCount); + EXPECT_TRUE(rxIsLinkReceivingSignal(RX_LINK_PRIMARY)); + + frameStatusForLink[RX_LINK_PRIMARY] = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; + frameStatusForLink[RX_LINK_SECONDARY] = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; + + stepRx(); + + EXPECT_FALSE(rxIsLinkReceivingSignal(RX_LINK_PRIMARY)); + EXPECT_FALSE(rxIsLinkReceivingSignal(RX_LINK_SECONDARY)); + EXPECT_EQ(1U, failsafeFailedCount); +} diff --git a/src/test/unit/rx_rx_unittest.cc.txt b/src/test/unit/rx_rx_unittest.cc.txt deleted file mode 100644 index 6b1d263ef70..00000000000 --- a/src/test/unit/rx_rx_unittest.cc.txt +++ /dev/null @@ -1,191 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include - -extern "C" { - #include "platform.h" - - #include "rx/rx.h" - #include "io/rc_controls.h" - #include "common/maths.h" - - uint32_t rcModeActivationMask; - - void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions); - void rxResetFlightChannelStatus(void); - bool rxHaveValidFlightChannels(void); - bool isPulseValid(uint16_t pulseDuration); - void rxUpdateFlightChannelStatus(uint8_t channel, uint16_t pulseDuration); -} - -#include "unittest_macros.h" -#include "gtest/gtest.h" - -#define DE_ACTIVATE_ALL_BOXES 0 - -typedef struct testData_s { - bool isPPMDataBeingReceived; - bool isPWMDataBeingReceived; -} testData_t; - -static testData_t testData; - -TEST(RxTest, TestValidFlightChannels) -{ - // given - memset(&testData, 0, sizeof(testData)); - rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be OFF - - // and - rxConfig_t rxConfig; - modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; - - memset(&rxConfig, 0, sizeof(rxConfig)); - rxConfig.rx_min_usec = 1000; - rxConfig.rx_max_usec = 2000; - - memset(&modeActivationConditions, 0, sizeof(modeActivationConditions)); - modeActivationConditions[0].auxChannelIndex = 0; - modeActivationConditions[0].modeId = BOXARM; - modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN); - modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(1600); - - // when - rxInit(&rxConfig, modeActivationConditions); - - // then (ARM channel should be positioned just 1 step above active range to init to OFF) - EXPECT_EQ(1625, rcData[modeActivationConditions[0].auxChannelIndex + NON_AUX_CHANNEL_COUNT]); - - // given - rxResetFlightChannelStatus(); - - // and - for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) { - bool validPulse = isPulseValid(1500); - rxUpdateFlightChannelStatus(channelIndex, validPulse); - } - - // then - EXPECT_TRUE(rxHaveValidFlightChannels()); -} - -TEST(RxTest, TestInvalidFlightChannels) -{ - // given - memset(&testData, 0, sizeof(testData)); - - // and - rxConfig_t rxConfig; - modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; - - memset(&rxConfig, 0, sizeof(rxConfig)); - rxConfig.rx_min_usec = 1000; - rxConfig.rx_max_usec = 2000; - - memset(&modeActivationConditions, 0, sizeof(modeActivationConditions)); - modeActivationConditions[0].auxChannelIndex = 0; - modeActivationConditions[0].modeId = BOXARM; - modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(1400); - modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX); - - // and - uint16_t channelPulses[MAX_SUPPORTED_RC_CHANNEL_COUNT]; - memset(&channelPulses, 1500, sizeof(channelPulses)); - - // and - rxInit(&rxConfig, modeActivationConditions); - - // then (ARM channel should be positioned just 1 step below active range to init to OFF) - EXPECT_EQ(1375, rcData[modeActivationConditions[0].auxChannelIndex + NON_AUX_CHANNEL_COUNT]); - - // and - for (uint8_t stickChannelIndex = 0; stickChannelIndex < STICK_CHANNEL_COUNT; stickChannelIndex++) { - - // given - rxResetFlightChannelStatus(); - - for (uint8_t otherStickChannelIndex = 0; otherStickChannelIndex < STICK_CHANNEL_COUNT; otherStickChannelIndex++) { - channelPulses[otherStickChannelIndex] = rxConfig.rx_min_usec; - } - channelPulses[stickChannelIndex] = rxConfig.rx_min_usec - 1; - - // when - for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) { - bool validPulse = isPulseValid(channelPulses[channelIndex]); - rxUpdateFlightChannelStatus(channelIndex, validPulse); - } - - // then - EXPECT_FALSE(rxHaveValidFlightChannels()); - - // given - rxResetFlightChannelStatus(); - - for (uint8_t otherStickChannelIndex = 0; otherStickChannelIndex < STICK_CHANNEL_COUNT; otherStickChannelIndex++) { - channelPulses[otherStickChannelIndex] = rxConfig.rx_max_usec; - } - channelPulses[stickChannelIndex] = rxConfig.rx_max_usec + 1; - - // when - for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) { - bool validPulse = isPulseValid(channelPulses[channelIndex]); - rxUpdateFlightChannelStatus(channelIndex, validPulse); - } - - // then - EXPECT_FALSE(rxHaveValidFlightChannels()); - } -} - - -// STUBS - -extern "C" { - void failsafeOnValidDataFailed() {} - void failsafeOnValidDataReceived() {} - - void failsafeOnRxSuspend(uint32_t ) {} - void failsafeOnRxResume(void) {} - - uint32_t micros(void) { return 0; } - uint32_t millis(void) { return 0; } - - bool feature(uint32_t mask) { - UNUSED(mask); - return false; - } - - bool isPPMDataBeingReceived(void) { - return testData.isPPMDataBeingReceived; - } - - bool isPWMDataBeingReceived(void) { - return testData.isPWMDataBeingReceived; - } - - void resetPPMDataReceivedState(void) {} - - bool rxMspFrameComplete(void) { return false; } - - void rxMspInit(rxConfig_t *, rxRuntimeConfig_t *, rcReadRawDataPtr *) {} - - void rxPwmInit(rxRuntimeConfig_t *, rcReadRawDataPtr *) {} -}