diff --git a/.gitignore b/.gitignore index 6d7138e6bd5..887f6707141 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,7 @@ *.bak *.uvgui.* *.ubx +*.log .project .settings .cproject diff --git a/docs/Mavlink.md b/docs/Mavlink.md new file mode 100644 index 00000000000..e115abb6cc5 --- /dev/null +++ b/docs/Mavlink.md @@ -0,0 +1,190 @@ +# MAVLink INAV Implementation + +INAV has a partial implementation of MAVLink that is intended primarily for simple telemetry and operation. It supports RC, missions, telemetry and some features such as Guided mode; but it is very different from a compliant MAVLink spec vehicle such as Pixhawk or Ardupilot and important differences exist, as such it is not 100% compatible and cannot be expected to work the same way. The standard MAVLink header library is used in compilation. + +INAV supports up to 4 concurrent MAVLink telemetry ports (`MAX_MAVLINK_PORTS`), one endpoint per serial port configured with `FUNCTION_TELEMETRY_MAVLINK`. + +## Fundamental differences from ArduPilot/PX4 + +- **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. +- **Limited command support**: only a subset of commands is implemented; unsupported commands are `COMMAND_ACK`ed as `UNSUPPORTED`. +- **Mission handling**: uploads are rejected while armed (except legacy guided waypoint writes). Mission frames are validated per command and unsupported frames are rejected. +- **Mode reporting**: `custom_mode` approximates ArduPilot modes and may not match all INAV states. +- **Flow control expectations**: INAV honours `RADIO_STATUS.txbuf` to avoid overrunning radios; without it, packets are simply paced at 20 ms intervals. +- **Half‑duplex etiquette**: on a MAVLink serial RX port configured for `serialrx_halfduplex`, INAV waits one telemetry tick after any received frame before transmitting to reduce collisions. + + +### Usage guidance +- If you rely on RC via MAVLink, ensure the serial receiver type is set to `SERIALRX_MAVLINK`. +- If MAVLink RX and telemetry intentionally share one half-duplex wire, enable `serialrx_halfduplex` for that setup. +- To reduce bandwidth, lower the 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` - force MAVLink v1 when set to 1. +- Stream rates (Hz): Each group is polled up to 50 Hz; a rate of 0 disables the group. + - `mavlink_port{1-4}_ext_status_rate` + - `mavlink_port{1-4}_rc_chan_rate` + - `mavlink_port{1-4}_pos_rate` + - `mavlink_port{1-4}_extra1_rate` + - `mavlink_port{1-4}_extra2_rate` + - `mavlink_port{1-4}_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 `RADIO_STATUS` provides flow control. +- `mavlink_port{1-4}_radio_type`- scales `RADIO_STATUS` RSSI/SNR for **generic**, **ELRS**, or **SiK** links. +- `mavlink_port{1-4}_high_latency`- turns on Mavlink HIGH_LATENCY2 mode on this port + +INAV always transmits as MAVLink component `MAV_COMP_ID_AUTOPILOT1`. Attached MAVLink devices are remote components learned from incoming traffic; they are not modeled as local per-port FC identities. + + +## 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) | 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`, which is not forwarded). +- 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. + +## 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`: for v1, `RC_CHANNELS`: for v2. Up to 18 input channels plus RSSI mapped to MAVLink units. +- `GPS_RAW_INT`: for GNSS fix quality, HDOP/VDOP, velocity, and satellite count when a fix (or estimated fix) exists. +- `GLOBAL_POSITION_INT`: couples GPS position with INAV's altitude and velocity estimates. +- `GPS_GLOBAL_ORIGIN`: broadcasts the current home position. +- `ATTITUDE`: Roll, pitch, yaw, and angular rates. +- `VFR_HUD`: with airspeed (if a healthy pitot is available), ground speed, throttle, altitude, and climb rate. +- `HEARTBEAT`: encodes arming state and maps INAV flight modes onto ArduPilot-style `custom_mode`: values (see mappings below). +- `EXTENDED_SYS_STATE`: publishes landed state. +- `BATTERY_STATUS`: with per-cell voltages (cells 11‑14 in `voltages_ext`), current draw, consumed mAh/Wh, and remaining percentage when available. +- `SCALED_PRESSURE`: for IMU/baro temperature. +- `SYSTEM_TIME`: with `time_boot_ms = millis()` and `time_unix_usec = 0`. +- `STATUSTEXT`: when the OSD has a pending system message; severity follows OSD attributes (notice/warning/critical). +- Event/on-demand messages: `MISSION_ITEM_REACHED` when a mission item is reached, plus `AUTOPILOT_VERSION`, `PROTOCOL_VERSION`, `MESSAGE_INTERVAL`, `HOME_POSITION`, `AVAILABLE_MODES`, and `CURRENT_MODE`. + +## Supported Incoming Messages + +- `HEARTBEAT`: used to detect ADS‑B participants when `type` is `MAV_TYPE_ADSB`. +- `MISSION_COUNT`: starts an upload transaction (capped at `NAV_MAX_WAYPOINTS`). +- `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`: downloads active mission items; returns `MISSION_ACK` on bad sequence. +- `MISSION_CLEAR_ALL`: clears stored 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. +- `RADIO_STATUS` updates remote TX buffer level and scales RSSI/SNR according to `mavlink_port{1-4}_radio_type` (also feeds link stats for MAVLink RX receivers). +- `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 (INAV does not expose parameters over MAVLink). +- `TUNNEL` accepts private payload type `0x8001` for MSP-over-MAVLink on MAVLink2 links. + + +## Supported Commands + +Limited implementation of the Command protocol. + +- `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`, `MAV_FRAME_GLOBAL_RELATIVE_ALT_INT`; otherwise `UNSUPPORTED`. +- `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 home fix exists. +- `MAV_CMD_REQUEST_MESSAGE`: emits one selected message (`HEARTBEAT`, `SYS_STATUS`, `ATTITUDE`, `VFR_HUD`, `AVAILABLE_MODES`, `CURRENT_MODE`, `EXTENDED_SYS_STATE`, RC channels, `GPS_RAW_INT`, `GLOBAL_POSITION_INT`, `GPS_GLOBAL_ORIGIN`, `BATTERY_STATUS`, `SCALED_PRESSURE`, `SYSTEM_TIME`, and `HOME_POSITION` when available) or `UNSUPPORTED`. +- `MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES`: returns stub `AUTOPILOT_VERSION` (v2 only; v1 returns `UNSUPPORTED`) advertising `SET_POSITION_TARGET_GLOBAL_INT` and `SET_POSITION_TARGET_LOCAL_NED`. +- `MAV_CMD_REQUEST_PROTOCOL_VERSION`: returns stub `PROTOCOL_VERSION` (v2 only; v1 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. + +## Mode mappings (INAV → MAVLink/ArduPilot) + +`custom_mode` is derived from active INAV telemetry flight mode (`getFlightModeForTelemetry()`), then mapped per vehicle type. + +- **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 + +Partial compatibility with MAVLink mission planners such as QGC is implemented, however the differences between the two protocols means INAV cannot be programmed to it's full potential using only the MAVLINK mission protocol; only simple missions are possible. It is recommended to use MultiWii Planner or the INAV Configurator to program missions. + +## 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 but forces `p1=0`, `p2=0`, keeps only alt-mode bit in `p3`; leg speed and user bits are lost. +- POSHOLD_TIME / LOITER_TIME: loiter time `p1` OK; leg speed `p2` and user-action bits in `p3` are discarded both directions. +- LAND: lat/lon/alt OK; leg speed `p1`, ground elevation `p2`, and user-action bits in `p3` are cleared in both directions (only alt-mode bit is retained from frame on upload). +- RTH: RTH land-if-nonzero flag in `p1` is ignored both ways (always zeroed); user-action bits dropped; alt is sent only if the MAVLink frame is coordinate and returns with 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`) are lost, so MAVLink missions cannot fully conform to `MSP_navigation_messages.md`. + + +## 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/Settings.md b/docs/Settings.md index 39e3de90d52..b2f78f82bf3 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2652,9 +2652,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 | | --- | --- | --- | @@ -2662,9 +2662,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 | | --- | --- | --- | @@ -2672,9 +2672,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 | | --- | --- | --- | @@ -2682,9 +2682,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 | | --- | --- | --- | @@ -2692,9 +2692,19 @@ Rate of the extra3 message for MAVLink telemetry --- -### mavlink_min_txbuffer +### mavlink_port1_high_latency -Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits. +Enable MAVLink high-latency mode on port 1 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mavlink_port1_min_txbuffer + +Minimum percent of TX buffer space free for MAVLink port 1. Requires RADIO_STATUS messages. | Default | Min | Max | | --- | --- | --- | @@ -2702,9 +2712,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 | | --- | --- | --- | @@ -2712,9 +2722,9 @@ 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. | Default | Min | Max | | --- | --- | --- | @@ -2722,9 +2732,9 @@ Mavlink radio type. Affects how RSSI and LQ are reported on OSD. --- -### 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 | | --- | --- | --- | @@ -2732,6 +2742,96 @@ 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. + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + +### 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. + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + +### 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. + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + ### mavlink_sysid MAVLink System ID diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index e3fe2dc5b4d..a573591766b 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -9,7 +9,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i -**JSON file rev: 4** +**JSON file rev: 0** **Warning: Verification needed, exercise caution until completely verified for accuracy and cleared, especially for integer signs. Source-based generation/validation is forthcoming. Refer to source for absolute certainty** @@ -66,6 +66,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **variable_len**: Optional boolean, if true, message does not have a predefined fixed length and needs appropriate handling\ **variants**: Optional special case, message has different cases of reply/request. Key/description is not a strict expression or code; just a readable condition\ **not_implemented**: Optional special case, message is not implemented (never or deprecated)\ +**replaced_by**: Optional array of MSP message names that replace this command. Present when a command is deprecated and scheduled for removal. Empty array if no replacement is needed\ **notes**: String with details of message ## Data dict fields: @@ -419,8 +420,8 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i [8730 - MSP2_INAV_SET_GLOBAL_TARGET](#msp2_inav_set_global_target) [8731 - MSP2_INAV_NAV_TARGET](#msp2_inav_nav_target) [8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) -[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) -[12289 - MSP2_RX_BIND](#msp2_rx_bind) +[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) +[12289 - MSP2_RX_BIND](#msp2_rx_bind) ## `MSP_API_VERSION (1 / 0x1)` **Description:** Provides the MSP protocol version and the INAV API version. @@ -2213,7 +2214,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i | `armingFlags` | `uint16_t` | 2 | Bitmask | Bitmask: Flight controller arming flags (`armingFlags`). Note: Truncated to 16 bits | | `accCalibAxisFlags` | `uint8_t` | 1 | Bitmask | Bitmask: Accelerometer calibrated axes flags (`accGetCalibrationAxisFlags()`) | -**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. +**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. The `accCalibAxisFlags` field is not present in `MSP2_INAV_STATUS` but is available via `MSP_CALIBRATION_DATA`. ## `MSP_SENSOR_STATUS (151 / 0x97)` **Description:** Provides the hardware status for each individual sensor system. @@ -2280,6 +2281,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i | `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision (`gpsSol.hdop`) | | `eph` | `uint16_t` | 2 | cm | Estimated Horizontal Position Accuracy (`gpsSol.eph`) | | `epv` | `uint16_t` | 2 | cm | Estimated Vertical Position Accuracy (`gpsSol.epv`) | +| `hwVersion` | `uint32_t` | 4 | Version code | GPS hardware version (`gpsState.hwVersion`). Values: 500=UBLOX5, 600=UBLOX6, 700=UBLOX7, 800=UBLOX8, 900=UBLOX9, 1000=UBLOX10, 0=UNKNOWN | **Notes:** Requires `USE_GPS`. @@ -4650,14 +4652,14 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **Notes:** Requires `rxConfig()->receiverType == RX_TYPE_SERIAL`. Requires `USE_SERIALRX_CRSF` or `USE_SERIALRX_SRXL2`. Calls `crsfBind()` or `srxl2Bind()` respectively. Returns error if receiver type or provider is not supported for binding. ## `MSP2_RX_BIND (12289 / 0x3001)` -**Description:** Initiates binding for MSP receivers (mLRS). - +**Description:** Initiates binding for MSP receivers (mLRS). + **Request Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| | `port_id` | `uint8_t` | 1 | Port ID | | `reserved_for_custom_use` | `uint8_t[3]` | 3 | Reserved for custom use | - + **Reply Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| diff --git a/docs/development/msp/get_all_inav_enums_h.py b/docs/development/msp/get_all_inav_enums_h.py index 971d077d5f4..79c5c57276a 100644 --- a/docs/development/msp/get_all_inav_enums_h.py +++ b/docs/development/msp/get_all_inav_enums_h.py @@ -16,6 +16,7 @@ 'flight', 'fc', 'drivers', + 'mavlink', ] def strip_comments(text: str) -> str: diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index ffe24b3f24c..c23774ce6e2 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -12,7 +12,8 @@ "ACC_ICM42605": "8", "ACC_BMI270": "9", "ACC_LSM6DXX": "10", - "ACC_FAKE": "11", + "ACC_ICM45686": "11", + "ACC_FAKE": "12", "ACC_MAX": "ACC_FAKE" }, "accEvent_t": { @@ -801,53 +802,54 @@ "DEVHW_ICM42605": "7", "DEVHW_BMI270": "8", "DEVHW_LSM6D": "9", - "DEVHW_MPU9250": "10", - "DEVHW_BMP085": "11", - "DEVHW_BMP280": "12", - "DEVHW_MS5611": "13", - "DEVHW_MS5607": "14", - "DEVHW_LPS25H": "15", - "DEVHW_SPL06": "16", - "DEVHW_BMP388": "17", - "DEVHW_DPS310": "18", - "DEVHW_B2SMPB": "19", - "DEVHW_HMC5883": "20", - "DEVHW_AK8963": "21", - "DEVHW_AK8975": "22", - "DEVHW_IST8310_0": "23", - "DEVHW_IST8310_1": "24", - "DEVHW_IST8308": "25", - "DEVHW_QMC5883": "26", - "DEVHW_QMC5883P": "27", - "DEVHW_MAG3110": "28", - "DEVHW_LIS3MDL": "29", - "DEVHW_RM3100": "30", - "DEVHW_VCM5883": "31", - "DEVHW_MLX90393": "32", - "DEVHW_LM75_0": "33", - "DEVHW_LM75_1": "34", - "DEVHW_LM75_2": "35", - "DEVHW_LM75_3": "36", - "DEVHW_LM75_4": "37", - "DEVHW_LM75_5": "38", - "DEVHW_LM75_6": "39", - "DEVHW_LM75_7": "40", - "DEVHW_DS2482": "41", - "DEVHW_MAX7456": "42", - "DEVHW_SRF10": "43", - "DEVHW_VL53L0X": "44", - "DEVHW_VL53L1X": "45", - "DEVHW_US42": "46", - "DEVHW_TOF10120_I2C": "47", - "DEVHW_TERARANGER_EVO_I2C": "48", - "DEVHW_MS4525": "49", - "DEVHW_DLVR": "50", - "DEVHW_M25P16": "51", - "DEVHW_W25N": "52", - "DEVHW_UG2864": "53", - "DEVHW_SDCARD": "54", - "DEVHW_IRLOCK": "55", - "DEVHW_PCF8574": "56" + "DEVHW_ICM45686": "10", + "DEVHW_MPU9250": "11", + "DEVHW_BMP085": "12", + "DEVHW_BMP280": "13", + "DEVHW_MS5611": "14", + "DEVHW_MS5607": "15", + "DEVHW_LPS25H": "16", + "DEVHW_SPL06": "17", + "DEVHW_BMP388": "18", + "DEVHW_DPS310": "19", + "DEVHW_B2SMPB": "20", + "DEVHW_HMC5883": "21", + "DEVHW_AK8963": "22", + "DEVHW_AK8975": "23", + "DEVHW_IST8310_0": "24", + "DEVHW_IST8310_1": "25", + "DEVHW_IST8308": "26", + "DEVHW_QMC5883": "27", + "DEVHW_QMC5883P": "28", + "DEVHW_MAG3110": "29", + "DEVHW_LIS3MDL": "30", + "DEVHW_RM3100": "31", + "DEVHW_VCM5883": "32", + "DEVHW_MLX90393": "33", + "DEVHW_LM75_0": "34", + "DEVHW_LM75_1": "35", + "DEVHW_LM75_2": "36", + "DEVHW_LM75_3": "37", + "DEVHW_LM75_4": "38", + "DEVHW_LM75_5": "39", + "DEVHW_LM75_6": "40", + "DEVHW_LM75_7": "41", + "DEVHW_DS2482": "42", + "DEVHW_MAX7456": "43", + "DEVHW_SRF10": "44", + "DEVHW_VL53L0X": "45", + "DEVHW_VL53L1X": "46", + "DEVHW_US42": "47", + "DEVHW_TOF10120_I2C": "48", + "DEVHW_TERARANGER_EVO_I2C": "49", + "DEVHW_MS4525": "50", + "DEVHW_DLVR": "51", + "DEVHW_M25P16": "52", + "DEVHW_W25N": "53", + "DEVHW_UG2864": "54", + "DEVHW_SDCARD": "55", + "DEVHW_IRLOCK": "56", + "DEVHW_PCF8574": "57" }, "deviceFlags_e": { "_source": "inav/src/main/drivers/bus.h", @@ -1528,7 +1530,8 @@ "GYRO_ICM42605": "8", "GYRO_BMI270": "9", "GYRO_LSM6DXX": "10", - "GYRO_FAKE": "11" + "GYRO_ICM45686": "11", + "GYRO_FAKE": "12" }, "HardwareMotorTypes_e": { "_source": "inav/src/main/drivers/pwm_esc_detect.h", @@ -1862,7 +1865,8 @@ "LED_MODE_ANGLE": "3", "LED_MODE_MAG": "4", "LED_MODE_BARO": "5", - "LED_SPECIAL": "6" + "LED_MODE_LOITER": "6", + "LED_SPECIAL": "7" }, "ledOverlayId_e": { "_source": "inav/src/main/io/ledstrip.h", @@ -2238,16 +2242,47 @@ "MAG_FAKE": "16", "MAG_MAX": "MAG_FAKE" }, + "mavFrameSupportMask_e": { + "_source": "inav/src/main/mavlink/mavlink_mission.h", + "MAV_FRAME_SUPPORTED_NONE": "0", + "MAV_FRAME_SUPPORTED_GLOBAL": "(1 << 0)", + "MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT": "(1 << 1)", + "MAV_FRAME_SUPPORTED_GLOBAL_INT": "(1 << 2)", + "MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT": "(1 << 3)" + }, "mavlinkAutopilotType_e": { "_source": "inav/src/main/telemetry/telemetry.h", "MAVLINK_AUTOPILOT_GENERIC": "0", "MAVLINK_AUTOPILOT_ARDUPILOT": "1" }, + "mavlinkFcDispatchResult_e": { + "_source": "inav/src/main/fc/fc_mavlink.h", + "MAVLINK_FC_DISPATCH_NOT_HANDLED": "0", + "MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY": "1", + "MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY": "2" + }, + "mavlinkPeriodicMessage_e": { + "_source": "inav/src/main/mavlink/mavlink_types.h", + "MAVLINK_PERIODIC_MESSAGE_HEARTBEAT": "0", + "MAVLINK_PERIODIC_MESSAGE_SYS_STATUS": "1", + "MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE": "2", + "MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS": "3", + "MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT": "4", + "MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT": "5", + "MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN": "6", + "MAVLINK_PERIODIC_MESSAGE_ATTITUDE": "7", + "MAVLINK_PERIODIC_MESSAGE_VFR_HUD": "8", + "MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS": "9", + "MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE": "10", + "MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME": "11", + "MAVLINK_PERIODIC_MESSAGE_COUNT": "12" + }, "mavlinkRadio_e": { "_source": "inav/src/main/telemetry/telemetry.h", "MAVLINK_RADIO_GENERIC": "0", "MAVLINK_RADIO_ELRS": "1", - "MAVLINK_RADIO_SIK": "2" + "MAVLINK_RADIO_SIK": "2", + "MAVLINK_RADIO_NONE": "3" }, "measurementSteps_e": { "_source": "inav/src/main/drivers/rangefinder/rangefinder_vl53l0x.c", @@ -2327,8 +2362,7 @@ "MULTI_FUNC_3": "3", "MULTI_FUNC_4": "4", "MULTI_FUNC_5": "5", - "MULTI_FUNC_6": "6", - "MULTI_FUNC_END": "7" + "MULTI_FUNC_END": "6" }, "multiFunctionFlags_e": { "_source": "inav/src/main/fc/multifunction.h", @@ -3293,9 +3327,8 @@ "_source": "inav/src/main/io/displayport_msp_osd.c", "SD_3016": "0", "HD_5018": "1", - "HD_3016": "2", - "HD_6022": "3", - "HD_5320": "4" + "HD_6022": "2", + "HD_5320": "3" }, "resourceOwner_e": { "_source": "inav/src/main/drivers/resource.h", @@ -3831,7 +3864,7 @@ "THR_HI": "(2 << (2 * THROTTLE))" }, "systemState_e": { - "_source": "inav/src/main/fc/fc_init.c", + "_source": "inav/src/main/fc/fc_init.h", "SYSTEM_STATE_INITIALISING": "0", "SYSTEM_STATE_CONFIG_LOADED": "(1 << 0)", "SYSTEM_STATE_SENSORS_READY": "(1 << 1)", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index af53d1c6b9a..1e7ba6db1de 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -173,7 +173,10 @@ - [ltm_modes_e](#enum-ltm_modes_e) - [ltmUpdateRate_e](#enum-ltmupdaterate_e) - [magSensor_e](#enum-magsensor_e) +- [mavFrameSupportMask_e](#enum-mavframesupportmask_e) - [mavlinkAutopilotType_e](#enum-mavlinkautopilottype_e) +- [mavlinkFcDispatchResult_e](#enum-mavlinkfcdispatchresult_e) +- [mavlinkPeriodicMessage_e](#enum-mavlinkperiodicmessage_e) - [mavlinkRadio_e](#enum-mavlinkradio_e) - [measurementSteps_e](#enum-measurementsteps_e) - [mixerProfileATRequest_e](#enum-mixerprofileatrequest_e) @@ -370,7 +373,8 @@ | `ACC_ICM42605` | 8 | | | `ACC_BMI270` | 9 | | | `ACC_LSM6DXX` | 10 | | -| `ACC_FAKE` | 11 | | +| `ACC_ICM45686` | 11 | | +| `ACC_FAKE` | 12 | | | `ACC_MAX` | ACC_FAKE | | --- @@ -1407,53 +1411,54 @@ | `DEVHW_ICM42605` | 7 | | | `DEVHW_BMI270` | 8 | | | `DEVHW_LSM6D` | 9 | | -| `DEVHW_MPU9250` | 10 | | -| `DEVHW_BMP085` | 11 | | -| `DEVHW_BMP280` | 12 | | -| `DEVHW_MS5611` | 13 | | -| `DEVHW_MS5607` | 14 | | -| `DEVHW_LPS25H` | 15 | | -| `DEVHW_SPL06` | 16 | | -| `DEVHW_BMP388` | 17 | | -| `DEVHW_DPS310` | 18 | | -| `DEVHW_B2SMPB` | 19 | | -| `DEVHW_HMC5883` | 20 | | -| `DEVHW_AK8963` | 21 | | -| `DEVHW_AK8975` | 22 | | -| `DEVHW_IST8310_0` | 23 | | -| `DEVHW_IST8310_1` | 24 | | -| `DEVHW_IST8308` | 25 | | -| `DEVHW_QMC5883` | 26 | | -| `DEVHW_QMC5883P` | 27 | | -| `DEVHW_MAG3110` | 28 | | -| `DEVHW_LIS3MDL` | 29 | | -| `DEVHW_RM3100` | 30 | | -| `DEVHW_VCM5883` | 31 | | -| `DEVHW_MLX90393` | 32 | | -| `DEVHW_LM75_0` | 33 | | -| `DEVHW_LM75_1` | 34 | | -| `DEVHW_LM75_2` | 35 | | -| `DEVHW_LM75_3` | 36 | | -| `DEVHW_LM75_4` | 37 | | -| `DEVHW_LM75_5` | 38 | | -| `DEVHW_LM75_6` | 39 | | -| `DEVHW_LM75_7` | 40 | | -| `DEVHW_DS2482` | 41 | | -| `DEVHW_MAX7456` | 42 | | -| `DEVHW_SRF10` | 43 | | -| `DEVHW_VL53L0X` | 44 | | -| `DEVHW_VL53L1X` | 45 | | -| `DEVHW_US42` | 46 | | -| `DEVHW_TOF10120_I2C` | 47 | | -| `DEVHW_TERARANGER_EVO_I2C` | 48 | | -| `DEVHW_MS4525` | 49 | | -| `DEVHW_DLVR` | 50 | | -| `DEVHW_M25P16` | 51 | | -| `DEVHW_W25N` | 52 | | -| `DEVHW_UG2864` | 53 | | -| `DEVHW_SDCARD` | 54 | | -| `DEVHW_IRLOCK` | 55 | | -| `DEVHW_PCF8574` | 56 | | +| `DEVHW_ICM45686` | 10 | | +| `DEVHW_MPU9250` | 11 | | +| `DEVHW_BMP085` | 12 | | +| `DEVHW_BMP280` | 13 | | +| `DEVHW_MS5611` | 14 | | +| `DEVHW_MS5607` | 15 | | +| `DEVHW_LPS25H` | 16 | | +| `DEVHW_SPL06` | 17 | | +| `DEVHW_BMP388` | 18 | | +| `DEVHW_DPS310` | 19 | | +| `DEVHW_B2SMPB` | 20 | | +| `DEVHW_HMC5883` | 21 | | +| `DEVHW_AK8963` | 22 | | +| `DEVHW_AK8975` | 23 | | +| `DEVHW_IST8310_0` | 24 | | +| `DEVHW_IST8310_1` | 25 | | +| `DEVHW_IST8308` | 26 | | +| `DEVHW_QMC5883` | 27 | | +| `DEVHW_QMC5883P` | 28 | | +| `DEVHW_MAG3110` | 29 | | +| `DEVHW_LIS3MDL` | 30 | | +| `DEVHW_RM3100` | 31 | | +| `DEVHW_VCM5883` | 32 | | +| `DEVHW_MLX90393` | 33 | | +| `DEVHW_LM75_0` | 34 | | +| `DEVHW_LM75_1` | 35 | | +| `DEVHW_LM75_2` | 36 | | +| `DEVHW_LM75_3` | 37 | | +| `DEVHW_LM75_4` | 38 | | +| `DEVHW_LM75_5` | 39 | | +| `DEVHW_LM75_6` | 40 | | +| `DEVHW_LM75_7` | 41 | | +| `DEVHW_DS2482` | 42 | | +| `DEVHW_MAX7456` | 43 | | +| `DEVHW_SRF10` | 44 | | +| `DEVHW_VL53L0X` | 45 | | +| `DEVHW_VL53L1X` | 46 | | +| `DEVHW_US42` | 47 | | +| `DEVHW_TOF10120_I2C` | 48 | | +| `DEVHW_TERARANGER_EVO_I2C` | 49 | | +| `DEVHW_MS4525` | 50 | | +| `DEVHW_DLVR` | 51 | | +| `DEVHW_M25P16` | 52 | | +| `DEVHW_W25N` | 53 | | +| `DEVHW_UG2864` | 54 | | +| `DEVHW_SDCARD` | 55 | | +| `DEVHW_IRLOCK` | 56 | | +| `DEVHW_PCF8574` | 57 | | --- ## `deviceFlags_e` @@ -2489,7 +2494,8 @@ | `GYRO_ICM42605` | 8 | | | `GYRO_BMI270` | 9 | | | `GYRO_LSM6DXX` | 10 | | -| `GYRO_FAKE` | 11 | | +| `GYRO_ICM45686` | 11 | | +| `GYRO_FAKE` | 12 | | --- ## `HardwareMotorTypes_e` @@ -2925,7 +2931,8 @@ | `LED_MODE_ANGLE` | 3 | | | `LED_MODE_MAG` | 4 | | | `LED_MODE_BARO` | 5 | | -| `LED_SPECIAL` | 6 | | +| `LED_MODE_LOITER` | 6 | | +| `LED_SPECIAL` | 7 | | --- ## `ledOverlayId_e` @@ -3374,6 +3381,19 @@ | `MAG_FAKE` | 16 | | | `MAG_MAX` | MAG_FAKE | | +--- +## `mavFrameSupportMask_e` + +> Source: ../../../src/main/mavlink/mavlink_mission.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MAV_FRAME_SUPPORTED_NONE` | 0 | | +| `MAV_FRAME_SUPPORTED_GLOBAL` | (1 << 0) | | +| `MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT` | (1 << 1) | | +| `MAV_FRAME_SUPPORTED_GLOBAL_INT` | (1 << 2) | | +| `MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT` | (1 << 3) | | + --- ## `mavlinkAutopilotType_e` @@ -3384,6 +3404,38 @@ | `MAVLINK_AUTOPILOT_GENERIC` | 0 | | | `MAVLINK_AUTOPILOT_ARDUPILOT` | 1 | | +--- +## `mavlinkFcDispatchResult_e` + +> Source: ../../../src/main/fc/fc_mavlink.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MAVLINK_FC_DISPATCH_NOT_HANDLED` | 0 | | +| `MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY` | 1 | | +| `MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY` | 2 | | + +--- +## `mavlinkPeriodicMessage_e` + +> Source: ../../../src/main/mavlink/mavlink_types.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MAVLINK_PERIODIC_MESSAGE_HEARTBEAT` | 0 | | +| `MAVLINK_PERIODIC_MESSAGE_SYS_STATUS` | 1 | | +| `MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE` | 2 | | +| `MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS` | 3 | | +| `MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT` | 4 | | +| `MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT` | 5 | | +| `MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN` | 6 | | +| `MAVLINK_PERIODIC_MESSAGE_ATTITUDE` | 7 | | +| `MAVLINK_PERIODIC_MESSAGE_VFR_HUD` | 8 | | +| `MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS` | 9 | | +| `MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE` | 10 | | +| `MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME` | 11 | | +| `MAVLINK_PERIODIC_MESSAGE_COUNT` | 12 | | + --- ## `mavlinkRadio_e` @@ -3394,6 +3446,7 @@ | `MAVLINK_RADIO_GENERIC` | 0 | | | `MAVLINK_RADIO_ELRS` | 1 | | | `MAVLINK_RADIO_SIK` | 2 | | +| `MAVLINK_RADIO_NONE` | 3 | | --- ## `measurementSteps_e` @@ -3533,8 +3586,7 @@ | `MULTI_FUNC_3` | 3 | | | `MULTI_FUNC_4` | 4 | | | `MULTI_FUNC_5` | 5 | | -| `MULTI_FUNC_6` | 6 | | -| `MULTI_FUNC_END` | 7 | | +| `MULTI_FUNC_END` | 6 | | --- ## `multiFunctionFlags_e` @@ -4821,9 +4873,8 @@ |---|---:|---| | `SD_3016` | 0 | | | `HD_5018` | 1 | | -| `HD_3016` | 2 | | -| `HD_6022` | 3 | | -| `HD_5320` | 4 | | +| `HD_6022` | 2 | | +| `HD_5320` | 3 | | --- ## `resourceOwner_e` @@ -5629,7 +5680,7 @@ --- ## `systemState_e` -> Source: ../../../src/main/fc/fc_init.h +> Source: ../../../src/main/fc/fc_init.c | Enumerator | Value | Condition | |---|---:|---| @@ -5643,7 +5694,7 @@ --- ## `systemState_e` -> Source: ../../../src/main/fc/fc_init.c +> Source: ../../../src/main/fc/fc_init.h | Enumerator | Value | Condition | |---|---:|---| diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum index ae689c4b8eb..60780228071 100644 --- a/docs/development/msp/msp_messages.checksum +++ b/docs/development/msp/msp_messages.checksum @@ -1 +1 @@ -6e8861395a4275489a12012e6985d733 +fd564ac26b85f236c4cf3f41bfa0e7c1 diff --git a/docs/development/msp/rev b/docs/development/msp/rev index bf0d87ab1b2..c227083464f 100644 --- a/docs/development/msp/rev +++ b/docs/development/msp/rev @@ -1 +1 @@ -4 \ No newline at end of file +0 \ No newline at end of file diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 531d43a92fd..b8303915eaf 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -292,6 +292,8 @@ main_sources(COMMON_SRC fc/fc_tasks.c fc/fc_tasks.h fc/fc_hardfaults.c + fc/fc_mavlink.c + fc/fc_mavlink.h fc/fc_msp.c fc/fc_msp.h fc/fc_msp_box.c @@ -393,6 +395,25 @@ main_sources(COMMON_SRC io/osd/custom_elements.c + mavlink/mavlink_command.c + mavlink/mavlink_command.h + mavlink/mavlink_guided.c + mavlink/mavlink_guided.h + mavlink/mavlink_internal.h + mavlink/mavlink_mission.c + mavlink/mavlink_mission.h + mavlink/mavlink_modes.c + mavlink/mavlink_modes.h + mavlink/mavlink_types.h + mavlink/mavlink_ports.c + mavlink/mavlink_ports.h + mavlink/mavlink_routing.c + mavlink/mavlink_routing.h + mavlink/mavlink_runtime.c + mavlink/mavlink_runtime.h + mavlink/mavlink_streams.c + mavlink/mavlink_streams.h + msp/msp_serial.c msp/msp_serial.h diff --git a/src/main/fc/fc_mavlink.c b/src/main/fc/fc_mavlink.c new file mode 100644 index 00000000000..f59b91cd690 --- /dev/null +++ b/src/main/fc/fc_mavlink.c @@ -0,0 +1,288 @@ +#include "mavlink/mavlink_internal.h" + +#include "fc/fc_mavlink.h" + +#include "mavlink/mavlink_command.h" +#include "mavlink/mavlink_guided.h" +#include "mavlink/mavlink_mission.h" +#include "mavlink/mavlink_runtime.h" +#include "mavlink/mavlink_streams.h" + +#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); + mavlinkRxHandleMessage(&msg); + return true; +} + +static bool handleIncoming_PARAM_REQUEST_LIST(void) { + mavlink_param_request_list_t msg; + mavlink_msg_param_request_list_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != 0 && msg.target_system != mavSystemId) { + return false; + } + if (msg.target_component != 0 && msg.target_component != mavComponentId) { + return false; + } + + mavlink_msg_param_value_pack(mavSystemId, mavComponentId, &mavSendMsg, 0, 0, 0, 0, 0); + mavlinkSendMessage(); + return true; +} + +static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { + switch(mavActiveConfig->radio_type) { + case MAVLINK_RADIO_NONE: + break; + 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; + break; + case MAVLINK_RADIO_ELRS: + rxLinkStatistics.uplinkRSSI = -msg->remrssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100); + 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; + break; + } +} + +static bool handleIncoming_RADIO_STATUS(void) { + mavlink_radio_status_t msg; + mavlink_msg_radio_status_decode(&mavlinkContext.recvMsg, &msg); + if (msg.txbuf > 0) { + mavActivePort->txbuffValid = true; + mavActivePort->txbuffFree = msg.txbuf; + } else { + mavActivePort->txbuffValid = false; + mavActivePort->txbuffFree = 100; + } + + if (rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { + mavlinkParseRxStats(&msg); + } + + return true; +} + +#ifdef USE_ADSB +static bool handleIncoming_ADSB_VEHICLE(void) { + mavlink_adsb_vehicle_t msg; + mavlink_msg_adsb_vehicle_decode(&mavlinkContext.recvMsg, &msg); + + adsbVehicleValues_t* vehicle = getVehicleForFill(); + if(vehicle != NULL){ + vehicle->icao = msg.ICAO_address; + vehicle->gps.lat = msg.lat; + vehicle->gps.lon = msg.lon; + vehicle->alt = (int32_t)(msg.altitude / 10); + vehicle->horVelocity = msg.hor_velocity; + vehicle->heading = msg.heading; + vehicle->flags = msg.flags; + vehicle->altitudeType = msg.altitude_type; + memcpy(&(vehicle->callsign), msg.callsign, sizeof(vehicle->callsign)); + vehicle->emitterType = msg.emitter_type; + vehicle->tslc = msg.tslc; + + adsbNewVehicle(vehicle); + } + + return true; +} +#endif + +mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIndex) +{ + UNUSED(ingressPortIndex); + + switch (mavlinkContext.recvMsg.msgid) { + case MAVLINK_MSG_ID_HEARTBEAT: + return mavlinkHandleIncomingHeartbeat() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + return handleIncoming_PARAM_REQUEST_LIST() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + return mavlinkHandleIncomingMissionClearAll() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_COUNT: + return mavlinkHandleIncomingMissionCount() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_ITEM: + return mavlinkHandleIncomingMissionItem() ? MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY : MAVLINK_FC_DISPATCH_NOT_HANDLED; + case MAVLINK_MSG_ID_MISSION_ITEM_INT: + 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_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; +#endif + 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; + } +} + + +#endif diff --git a/src/main/fc/fc_mavlink.h b/src/main/fc/fc_mavlink.h new file mode 100644 index 00000000000..dd4543318c9 --- /dev/null +++ b/src/main/fc/fc_mavlink.h @@ -0,0 +1,11 @@ +#pragma once + +#include "mavlink/mavlink_types.h" + +typedef enum { + MAVLINK_FC_DISPATCH_NOT_HANDLED = 0, + MAVLINK_FC_DISPATCH_HANDLED_NO_ACTIVITY, + MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY, +} mavlinkFcDispatchResult_e; + +mavlinkFcDispatchResult_e mavlinkFcDispatchIncomingMessage(uint8_t ingressPortIndex); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 53b59f5d490..5db845fdce1 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4225,7 +4225,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; } - if (navigationSetAltitudeTargetWithDatum((geoAltitudeDatumFlag_e)sbufReadU8(src), (int32_t)sbufReadU32(src))) { + uint8_t setAltDatum = (geoAltitudeDatumFlag_e)sbufReadU8(src); + int32_t setNewAlt = sbufReadU32(src); + if (navigationSetAltitudeTargetWithDatum(setAltDatum, setNewAlt)) { *ret = MSP_RESULT_ACK; break; } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 58274b1b660..769c53b6b33 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -199,7 +199,7 @@ tables: values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e - name: mavlink_radio_type - values: ["GENERIC", "ELRS", "SIK"] + values: ["GENERIC", "ELRS", "SIK", "NONE"] enum: mavlinkRadio_e - name: mavlink_autopilot_type values: ["GENERIC", "ARDUPILOT"] @@ -3229,80 +3229,136 @@ groups: type: int16_t min: INT16_MIN max: INT16_MAX - - name: mavlink_ext_status_rate - field: mavlink.extended_status_rate - description: "Rate of the extended status message for MAVLink telemetry" + - name: mavlink_port1_ext_status_rate + field: mavlink[0].extended_status_rate + description: "Rate of the extended status message for MAVLink telemetry on port 1" type: uint8_t min: 0 max: 255 default_value: 2 - name: mavlink_autopilot_type - field: mavlink.autopilot_type + field: mavlink_common.autopilot_type description: "Autopilot type to advertise for MAVLink telemetry" table: mavlink_autopilot_type default_value: "GENERIC" type: uint8_t - - name: mavlink_rc_chan_rate - description: "Rate of the RC channels message for MAVLink telemetry" - field: mavlink.rc_channels_rate + - name: mavlink_port1_rc_chan_rate + description: "Rate of the RC channels message for MAVLink telemetry on port 1" + field: mavlink[0].rc_channels_rate type: uint8_t min: 0 max: 255 default_value: 1 - - name: mavlink_pos_rate - description: "Rate of the position message for MAVLink telemetry" - field: mavlink.position_rate + - name: mavlink_port1_pos_rate + description: "Rate of the position message for MAVLink telemetry on port 1" + field: mavlink[0].position_rate type: uint8_t min: 0 max: 255 default_value: 2 - - name: mavlink_extra1_rate - description: "Rate of the extra1 message for MAVLink telemetry" - field: mavlink.extra1_rate + - name: mavlink_port1_extra1_rate + description: "Rate of the extra1 message for MAVLink telemetry on port 1" + field: mavlink[0].extra1_rate type: uint8_t min: 0 max: 255 default_value: 2 - - name: mavlink_extra2_rate - description: "Rate of the extra2 message for MAVLink telemetry" - field: mavlink.extra2_rate + - name: mavlink_port1_extra2_rate + description: "Rate of the extra2 message for MAVLink telemetry on port 1" + field: mavlink[0].extra2_rate type: uint8_t min: 0 max: 255 default_value: 2 - - name: mavlink_extra3_rate - description: "Rate of the extra3 message for MAVLink telemetry" - field: mavlink.extra3_rate + - name: mavlink_port1_extra3_rate + description: "Rate of the extra3 message for MAVLink telemetry on port 1" + field: mavlink[0].extra3_rate type: uint8_t min: 0 max: 255 default_value: 1 - name: mavlink_version - field: mavlink.version + field: mavlink_common.version description: "Version of MAVLink to use" type: uint8_t min: 1 max: 2 default_value: 2 - - name: mavlink_min_txbuffer - field: mavlink.min_txbuff - description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." + - name: mavlink_port1_min_txbuffer + field: mavlink[0].min_txbuff + description: "Minimum percent of TX buffer space free for MAVLink port 1. Requires RADIO_STATUS messages." default_value: 33 min: 0 max: 100 - - name: mavlink_radio_type - description: "Mavlink radio type. Affects how RSSI and LQ are reported on OSD." - field: mavlink.radio_type + - name: mavlink_port1_radio_type + description: "MAVLink radio type for port 1. Affects RSSI and LQ reporting on OSD." + field: mavlink[0].radio_type table: mavlink_radio_type default_value: "GENERIC" type: uint8_t - name: mavlink_sysid - field: mavlink.sysid + field: mavlink_common.sysid description: "MAVLink System ID" type: uint8_t min: 1 max: 255 default_value: 1 + - name: mavlink_port1_high_latency + field: mavlink[0].high_latency + description: "Enable MAVLink high-latency mode on port 1" + type: bool + default_value: OFF + - name: mavlink_port2_min_txbuffer + field: mavlink[1].min_txbuff + description: "Minimum percent of TX buffer space free for MAVLink port 2. Requires RADIO_STATUS messages." + default_value: 33 + min: 0 + max: 100 + - name: mavlink_port2_radio_type + description: "MAVLink radio type for port 2. Affects RSSI and LQ reporting on OSD." + field: mavlink[1].radio_type + table: mavlink_radio_type + default_value: "GENERIC" + type: uint8_t + - name: mavlink_port2_high_latency + field: mavlink[1].high_latency + description: "Enable MAVLink high-latency mode on port 2" + type: bool + default_value: OFF + - name: mavlink_port3_min_txbuffer + field: mavlink[2].min_txbuff + description: "Minimum percent of TX buffer space free for MAVLink port 3. Requires RADIO_STATUS messages." + default_value: 33 + min: 0 + max: 100 + - name: mavlink_port3_radio_type + description: "MAVLink radio type for port 3. Affects RSSI and LQ reporting on OSD." + field: mavlink[2].radio_type + table: mavlink_radio_type + default_value: "GENERIC" + type: uint8_t + - name: mavlink_port3_high_latency + field: mavlink[2].high_latency + description: "Enable MAVLink high-latency mode on port 3" + type: bool + default_value: OFF + - name: mavlink_port4_min_txbuffer + field: mavlink[3].min_txbuff + description: "Minimum percent of TX buffer space free for MAVLink port 4. Requires RADIO_STATUS messages." + default_value: 33 + min: 0 + max: 100 + - name: mavlink_port4_radio_type + description: "MAVLink radio type for port 4. Affects RSSI and LQ reporting on OSD." + field: mavlink[3].radio_type + table: mavlink_radio_type + default_value: "GENERIC" + type: uint8_t + - name: mavlink_port4_high_latency + field: mavlink[3].high_latency + description: "Enable MAVLink high-latency mode on port 4" + type: bool + default_value: OFF - name: PG_OSD_CONFIG type: osdConfig_t diff --git a/src/main/mavlink/mavlink_command.c b/src/main/mavlink/mavlink_command.c new file mode 100644 index 00000000000..94127c1fe7d --- /dev/null +++ b/src/main/mavlink/mavlink_command.c @@ -0,0 +1,245 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_command.h" +#include "mavlink/mavlink_guided.h" +#include "mavlink/mavlink_runtime.h" +#include "mavlink/mavlink_streams.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +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; +} + +static void mavlinkSendCommandAck(uint16_t command, MAV_RESULT result, uint8_t ackTargetSystem, uint8_t ackTargetComponent) +{ + mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + command, + result, + 0, + 0, + ackTargetSystem, + ackTargetComponent); + mavlinkSendMessage(); +} + +static bool handleIncoming_COMMAND( + uint8_t targetSystem, + uint8_t targetComponent, + uint8_t ackTargetSystem, + uint8_t ackTargetComponent, + uint16_t command, + uint8_t frame, + float param1, + float param2, + float param3, + float param4, + float latitudeDeg, + float longitudeDeg, + float altitudeMeters) +{ + if (!mavlinkIsLocalTarget(targetSystem, targetComponent)) { + return false; + } + UNUSED(param3); + + switch (command) { + case MAV_CMD_DO_REPOSITION: + 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)) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + if (isnan(latitudeDeg) || isnan(longitudeDeg)) { + mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); + return true; + } + + if (isGCSValid()) { + navWaypoint_t wp = {0}; + wp.action = NAV_WP_ACTION_WAYPOINT; + wp.lat = (int32_t)(latitudeDeg * 1e7f); + wp.lon = (int32_t)(longitudeDeg * 1e7f); + wp.alt = (int32_t)(altitudeMeters * 100.0f); + if (!isnan(param4) && param4 >= 0.0f && param4 < 360.0f) { + wp.p1 = (int16_t)param4; + } else { + wp.p1 = 0; + } + wp.p2 = 0; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + wp.flag = 0; + + setWaypoint(255, &wp); + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendCommandAck(command, MAV_RESULT_DENIED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_DO_CHANGE_ALTITUDE: + { + const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame((uint8_t)lrintf(param2), param1); + mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_CONDITION_YAW: + { + 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; + MAV_RESULT result = MAV_RESULT_UNSUPPORTED; + + if (mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { + if (param2 == 0.0f) { + mavlinkSetMessageOverrideIntervalUs(periodicMessage, 0); + result = MAV_RESULT_ACCEPTED; + } else if (param2 < 0.0f) { + mavlinkSetMessageOverrideIntervalUs(periodicMessage, -1); + result = MAV_RESULT_ACCEPTED; + } else { + uint32_t intervalUs = (uint32_t)param2; + if (intervalUs > 0) { + const uint32_t minIntervalUs = 1000000UL / TELEMETRY_MAVLINK_MAXRATE; + if (intervalUs < minIntervalUs) { + intervalUs = minIntervalUs; + } + + mavlinkSetMessageOverrideIntervalUs(periodicMessage, (int32_t)intervalUs); + result = MAV_RESULT_ACCEPTED; + } + } + } + + mavlinkSendCommandAck(command, result, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_GET_MESSAGE_INTERVAL: + { + mavlinkPeriodicMessage_e periodicMessage; + if (!mavlinkPeriodicMessageFromMessageId((uint16_t)param1, &periodicMessage)) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + mavlink_msg_message_interval_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + (uint16_t)param1, + mavlinkMessageIntervalUs(periodicMessage)); + mavlinkSendMessage(); + + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + return true; + } + case MAV_CMD_CONTROL_HIGH_LATENCY: + if (param1 == 0.0f || param1 == 1.0f) { + if (mavlinkGetProtocolVersion() == 1 && param1 > 0.5f) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } + + mavActivePort->highLatencyEnabled = param1 > 0.5f; + if (mavActivePort->highLatencyEnabled) { + mavActivePort->lastHighLatencyMessageUs = 0; + } + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_REQUEST_PROTOCOL_VERSION: + if (mavlinkGetProtocolVersion() == 1) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendProtocolVersion(); + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: + if (mavlinkGetProtocolVersion() == 1) { + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendAutopilotVersion(); + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } + return true; + case MAV_CMD_REQUEST_MESSAGE: + { + const bool sent = mavlinkSendRequestedMessage((uint16_t)param1); + mavlinkSendCommandAck(command, sent ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } +#ifdef USE_GPS + case MAV_CMD_GET_HOME_POSITION: + if (mavlinkSendRequestedMessage(MAVLINK_MSG_ID_HOME_POSITION)) { + mavlinkSendCommandAck(command, MAV_RESULT_ACCEPTED, ackTargetSystem, ackTargetComponent); + } else { + mavlinkSendCommandAck(command, MAV_RESULT_FAILED, ackTargetSystem, ackTargetComponent); + } + return true; +#endif + default: + mavlinkSendCommandAck(command, MAV_RESULT_UNSUPPORTED, ackTargetSystem, ackTargetComponent); + return true; + } +} + +bool mavlinkHandleIncomingCommandInt(void) +{ + mavlink_command_int_t msg; + mavlink_msg_command_int_decode(&mavlinkContext.recvMsg, &msg); + + return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, msg.command, msg.frame, msg.param1, msg.param2, msg.param3, msg.param4, (float)msg.x / 1e7f, (float)msg.y / 1e7f, msg.z); +} + +bool mavlinkHandleIncomingCommandLong(void) +{ + mavlink_command_long_t msg; + mavlink_msg_command_long_decode(&mavlinkContext.recvMsg, &msg); + + // COMMAND_LONG has no frame field; location commands are WGS84 global by definition. + return handleIncoming_COMMAND(msg.target_system, msg.target_component, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, msg.command, MAV_FRAME_GLOBAL, msg.param1, msg.param2, msg.param3, msg.param4, msg.param5, msg.param6, msg.param7); +} + +#endif diff --git a/src/main/mavlink/mavlink_command.h b/src/main/mavlink/mavlink_command.h new file mode 100644 index 00000000000..fb468e696f7 --- /dev/null +++ b/src/main/mavlink/mavlink_command.h @@ -0,0 +1,7 @@ +#pragma once + +#include +#include + +bool mavlinkHandleIncomingCommandLong(void); +bool mavlinkHandleIncomingCommandInt(void); diff --git a/src/main/mavlink/mavlink_guided.c b/src/main/mavlink/mavlink_guided.c new file mode 100644 index 00000000000..9bdb719c716 --- /dev/null +++ b/src/main/mavlink/mavlink_guided.c @@ -0,0 +1,153 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_guided.h" +#include "mavlink/mavlink_runtime.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask) +{ + switch (frame) { + case MAV_FRAME_GLOBAL: + return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL; + case MAV_FRAME_GLOBAL_INT: + return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_INT; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT; + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + return allowedMask & MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT; + default: + return false; + } +} + +bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame) +{ + return frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_INT; +} + +MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters) +{ +#ifdef USE_BARO + geoAltitudeDatumFlag_e datum; + + switch (frame) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_INT: + datum = NAV_WP_MSL_DATUM; + break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + datum = NAV_WP_TAKEOFF_DATUM; + break; + default: + return MAV_RESULT_UNSUPPORTED; + } + + const int32_t targetAltitudeCm = (int32_t)lrintf(altitudeMeters * 100.0f); + return navigationSetAltitudeTargetWithDatum(datum, targetAltitudeCm) ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED; +#else + UNUSED(frame); + UNUSED(altitudeMeters); + return MAV_RESULT_UNSUPPORTED; +#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_guided.h b/src/main/mavlink/mavlink_guided.h new file mode 100644 index 00000000000..e160134d29b --- /dev/null +++ b/src/main/mavlink/mavlink_guided.h @@ -0,0 +1,13 @@ +#pragma once + +#include +#include + +#include "mavlink/mavlink_mission.h" + +bool mavlinkFrameIsSupported(uint8_t frame, mavFrameSupportMask_e allowedMask); +bool mavlinkFrameUsesAbsoluteAltitude(uint8_t frame); +MAV_RESULT mavlinkSetAltitudeTargetFromFrame(uint8_t frame, float altitudeMeters); + +bool mavlinkHandleIncomingSetPositionTargetGlobalInt(void); +bool mavlinkHandleIncomingSetPositionTargetLocalNed(void); diff --git a/src/main/mavlink/mavlink_internal.h b/src/main/mavlink/mavlink_internal.h new file mode 100644 index 00000000000..ecc9b54998f --- /dev/null +++ b/src/main/mavlink/mavlink_internal.h @@ -0,0 +1,131 @@ +#pragma once + +#include "platform.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +#include +#include +#include +#include + +#include "build/build_config.h" +#include "build/debug.h" +#include "build/version.h" + +#include "common/axis.h" +#include "common/color.h" +#include "common/maths.h" +#include "common/utils.h" +#include "common/string_light.h" + +#include "config/feature.h" + +#include "drivers/serial.h" +#include "drivers/time.h" +#include "drivers/display.h" +#include "drivers/osd_symbols.h" + +#include "fc/config.h" +#include "fc/fc_core.h" +#include "fc/fc_msp.h" +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/mixer_profile.h" +#include "flight/pid.h" +#include "flight/servos.h" +#include "flight/wind_estimator.h" + +#include "io/adsb.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/serial.h" +#include "io/osd.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "rx/rx.h" +#include "rx/mavlink.h" + +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/gyro.h" +#include "sensors/pitotmeter.h" +#include "sensors/diagnostics.h" +#include "sensors/sensors.h" +#include "sensors/temperature.h" +#include "sensors/esc_sensor.h" + +#include "mavlink/mavlink_types.h" +#include "telemetry/telemetry.h" + +#include "blackbox/blackbox_io.h" + +#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_TIMEOUT_MS 10000 + +typedef struct mavlinkContext_s { + mavlinkPortRuntime_t portStates[MAX_MAVLINK_PORTS]; + 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; + mavlink_message_t sendMsg; + mavlink_message_t recvMsg; + uint8_t systemId; + uint8_t autopilotType; + uint8_t componentId; + uint8_t tunnelReplyPayloadBuf[MSP_PORT_OUTBUF_SIZE]; + uint8_t tunnelFrameBuf[MAVLINK_TUNNEL_MSP_FRAMEBUF_SIZE]; + int incomingMissionWpCount; + int incomingMissionWpSequence; + uint8_t incomingMissionSourceSystem; + uint8_t incomingMissionSourceComponent; + timeMs_t incomingMissionLastActivityMs; +} mavlinkContext_t; + +extern mavlinkContext_t mavlinkContext; + +#define mavPortStates (mavlinkContext.portStates) +#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) +#define mavSendMsg (mavlinkContext.sendMsg) +#define mavSystemId (mavlinkContext.systemId) +#define mavAutopilotType (mavlinkContext.autopilotType) +#define mavComponentId (mavlinkContext.componentId) +#define mavTunnelReplyPayloadBuf (mavlinkContext.tunnelReplyPayloadBuf) +#define mavTunnelFrameBuf (mavlinkContext.tunnelFrameBuf) +#define incomingMissionWpCount (mavlinkContext.incomingMissionWpCount) +#define incomingMissionWpSequence (mavlinkContext.incomingMissionWpSequence) +#define incomingMissionSourceSystem (mavlinkContext.incomingMissionSourceSystem) +#define incomingMissionSourceComponent (mavlinkContext.incomingMissionSourceComponent) +#define incomingMissionLastActivityMs (mavlinkContext.incomingMissionLastActivityMs) + +#endif diff --git a/src/main/mavlink/mavlink_mission.c b/src/main/mavlink/mavlink_mission.c new file mode 100644 index 00000000000..70fcc9a06ea --- /dev/null +++ b/src/main/mavlink/mavlink_mission.c @@ -0,0 +1,619 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_guided.h" +#include "mavlink/mavlink_mission.h" +#include "mavlink/mavlink_runtime.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +void mavlinkSendPendingMissionItemReached(void) +{ + uint16_t seq; + if (!navigationConsumeWaypointReached(&seq)) { + return; + } + + uint8_t sendMask = 0; + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if (mavPortStates[portIndex].telemetryEnabled && mavPortStates[portIndex].port) { + sendMask |= MAVLINK_PORT_MASK(portIndex); + } + } + + if (sendMask == 0) { + return; + } + + mavSendMask = sendMask; + mavlink_msg_mission_item_reached_pack(mavlinkGetCommonConfig()->sysid, MAV_COMP_ID_AUTOPILOT1, &mavSendMsg, seq); + mavlinkSendMessage(); + mavSendMask = 0; +} + +uint8_t mavlinkWaypointFrame(const navWaypoint_t *wp, bool useIntMessages) +{ + switch (wp->action) { + case NAV_WP_ACTION_RTH: + case NAV_WP_ACTION_JUMP: + case NAV_WP_ACTION_SET_HEAD: + return MAV_FRAME_MISSION; + default: + break; + } + + if ((wp->p3 & NAV_WP_ALTMODE) == NAV_WP_ALTMODE) { + return useIntMessages ? MAV_FRAME_GLOBAL_INT : MAV_FRAME_GLOBAL; + } + + return useIntMessages ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_RELATIVE_ALT; +} + + +static void mavlinkSendMissionAck(MAV_MISSION_RESULT result) +{ + mavlink_msg_mission_ack_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + result, + MAV_MISSION_TYPE_MISSION, + 0 + ); + mavlinkSendMessage(); +} + +static void mavlinkResetIncomingMissionTransaction(void) +{ + incomingMissionWpCount = 0; + incomingMissionWpSequence = 0; + incomingMissionSourceSystem = 0; + incomingMissionSourceComponent = 0; + incomingMissionLastActivityMs = 0; +} + +static void mavlinkStartIncomingMissionTransaction(int missionCount) +{ + incomingMissionWpCount = missionCount; + incomingMissionWpSequence = 0; + incomingMissionSourceSystem = mavlinkContext.recvMsg.sysid; + incomingMissionSourceComponent = mavlinkContext.recvMsg.compid; + incomingMissionLastActivityMs = millis(); +} + +static void mavlinkTouchIncomingMissionTransaction(void) +{ + incomingMissionLastActivityMs = millis(); +} + +static bool mavlinkIsIncomingMissionTransactionActive(void) +{ + if (incomingMissionWpCount <= 0) { + return false; + } + + if ((timeMs_t)(millis() - incomingMissionLastActivityMs) > MAVLINK_MISSION_UPLOAD_TIMEOUT_MS) { + mavlinkResetIncomingMissionTransaction(); + return false; + } + + return true; +} + +static bool mavlinkIsIncomingMissionTransactionOwner(void) +{ + return mavlinkContext.recvMsg.sysid == incomingMissionSourceSystem && + mavlinkContext.recvMsg.compid == incomingMissionSourceComponent; +} + +static bool mavlinkHandleArmedGuidedMissionItem( + uint8_t current, + uint8_t frame, + mavFrameSupportMask_e allowedFrames, + int32_t latitudeE7, + int32_t longitudeE7, + float altitudeMeters) +{ + if (!isGCSValid()) { + mavlinkSendMissionAck(MAV_MISSION_ERROR); + return true; + } + + if (!mavlinkFrameIsSupported(frame, allowedFrames)) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); + return true; + } + + if (current == 2) { + navWaypoint_t wp = {0}; + wp.action = NAV_WP_ACTION_WAYPOINT; + wp.lat = latitudeE7; + wp.lon = longitudeE7; + wp.alt = (int32_t)lrintf(altitudeMeters * 100.0f); + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + + setWaypoint(255, &wp); + + mavlinkSendMissionAck(MAV_MISSION_ACCEPTED); + return true; + } + + if (current == 3) { + const MAV_RESULT result = mavlinkSetAltitudeTargetFromFrame(frame, altitudeMeters); + MAV_MISSION_RESULT response = MAV_MISSION_ERROR; + if (result == MAV_RESULT_ACCEPTED) {response = MAV_MISSION_ACCEPTED;} + else if (result == MAV_RESULT_UNSUPPORTED) {response = MAV_MISSION_UNSUPPORTED;} + mavlinkSendMissionAck(response); + return true; + } + + mavlinkSendMissionAck(MAV_MISSION_ERROR); + return true; +} + +static bool mavlinkHandleMissionItemCommon( + bool useIntMessages, + uint8_t frame, + uint16_t command, + uint8_t autocontinue, + uint16_t seq, + float param1, + float param2, + float param3, + float param4, + int32_t lat, + int32_t lon, + float altMeters) +{ + if (!mavlinkIsIncomingMissionTransactionActive() || !mavlinkIsIncomingMissionTransactionOwner()) { + mavlinkSendMissionAck(MAV_MISSION_INVALID_SEQUENCE); + return true; + } + + mavlinkTouchIncomingMissionTransaction(); + + const bool lastMissionItem = incomingMissionWpCount > 0 && ((int)seq + 1 >= incomingMissionWpCount); + + if (autocontinue == 0 && !lastMissionItem) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED); + return true; + } + + UNUSED(param3); + + navWaypoint_t wp = {0}; + + switch (command) { + case MAV_CMD_NAV_WAYPOINT: + if (!mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); + return true; + } + wp.action = NAV_WP_ACTION_WAYPOINT; + wp.lat = lat; + wp.lon = lon; + wp.alt = (int32_t)(altMeters * 100.0f); + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + break; + + case MAV_CMD_NAV_LOITER_TIME: + if (!mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); + return true; + } + wp.action = NAV_WP_ACTION_HOLD_TIME; + wp.lat = lat; + wp.lon = lon; + wp.alt = (int32_t)(altMeters * 100.0f); + wp.p1 = (int16_t)lrintf(param1); + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + { + const bool coordinateFrame = mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT); + + if (!coordinateFrame && frame != MAV_FRAME_MISSION) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); + return true; + } + wp.action = NAV_WP_ACTION_RTH; + wp.alt = coordinateFrame ? (int32_t)(altMeters * 100.0f) : 0; + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + break; + } + + case MAV_CMD_NAV_LAND: + if (!mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); + return true; + } + wp.action = NAV_WP_ACTION_LAND; + wp.lat = lat; + wp.lon = lon; + wp.alt = (int32_t)(altMeters * 100.0f); + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + break; + + case MAV_CMD_DO_JUMP: + if (frame != MAV_FRAME_MISSION) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); + return true; + } + if (param1 < 0.0f) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED); + return true; + } + wp.action = NAV_WP_ACTION_JUMP; + wp.p1 = (int16_t)lrintf(param1 + 1.0f); + wp.p2 = (int16_t)lrintf(param2); + break; + + case MAV_CMD_DO_SET_ROI: + if (param1 != MAV_ROI_LOCATION || + !mavlinkFrameIsSupported(frame, + MAV_FRAME_SUPPORTED_GLOBAL | + MAV_FRAME_SUPPORTED_GLOBAL_INT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT | + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT)) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED); + return true; + } + wp.action = NAV_WP_ACTION_SET_POI; + wp.lat = lat; + wp.lon = lon; + wp.alt = (int32_t)(altMeters * 100.0f); + wp.p3 = mavlinkFrameUsesAbsoluteAltitude(frame) ? NAV_WP_ALTMODE : 0; + break; + + case MAV_CMD_CONDITION_YAW: + if (frame != MAV_FRAME_MISSION) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED_FRAME); + return true; + } + if (param4 != 0.0f) { + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED); + return true; + } + wp.action = NAV_WP_ACTION_SET_HEAD; + wp.p1 = (int16_t)lrintf(param1); + break; + + default: + mavlinkSendMissionAck(MAV_MISSION_UNSUPPORTED); + return true; + } + + if (seq == incomingMissionWpSequence) { + incomingMissionWpSequence++; + wp.flag = (incomingMissionWpSequence >= incomingMissionWpCount) ? NAV_WP_FLAG_LAST : 0; + setWaypoint(incomingMissionWpSequence, &wp); + + if (incomingMissionWpSequence >= incomingMissionWpCount) { + if (isWaypointListValid()) { + mavlinkSendMissionAck(MAV_MISSION_ACCEPTED); + } else { + mavlinkSendMissionAck(MAV_MISSION_INVALID); + } + mavlinkResetIncomingMissionTransaction(); + } else { + if (useIntMessages) { + mavlink_msg_mission_request_int_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + incomingMissionWpSequence, + MAV_MISSION_TYPE_MISSION); + } else { + mavlink_msg_mission_request_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + incomingMissionWpSequence, + MAV_MISSION_TYPE_MISSION); + } + mavlinkSendMessage(); + } + } else { + if (seq + 1 == incomingMissionWpSequence) { + mavlinkTouchIncomingMissionTransaction(); + if (useIntMessages) { + mavlink_msg_mission_request_int_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + incomingMissionWpSequence, + MAV_MISSION_TYPE_MISSION); + } else { + mavlink_msg_mission_request_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + incomingMissionWpSequence, + MAV_MISSION_TYPE_MISSION); + } + mavlinkSendMessage(); + } else { + mavlinkSendMissionAck(MAV_MISSION_INVALID_SEQUENCE); + } + } + + return true; +} + +bool mavlinkHandleIncomingMissionClearAll(void) +{ + mavlink_mission_clear_all_t msg; + mavlink_msg_mission_clear_all_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system == mavSystemId) { + resetWaypointList(); + mavlinkResetIncomingMissionTransaction(); + mavlinkSendMissionAck(MAV_MISSION_ACCEPTED); + return true; + } + + return false; +} + +bool mavlinkHandleIncomingMissionCount(void) +{ + mavlink_mission_count_t msg; + mavlink_msg_mission_count_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system == mavSystemId) { + mavlinkResetIncomingMissionTransaction(); + if (ARMING_FLAG(ARMED)) { + mavlinkSendMissionAck(MAV_MISSION_ERROR); + return true; + } + if (msg.count == 0) { + resetWaypointList(); + mavlinkSendMissionAck(MAV_MISSION_ACCEPTED); + return true; + } + if (msg.count <= NAV_MAX_WAYPOINTS) { + mavlinkStartIncomingMissionTransaction(msg.count); + mavlink_msg_mission_request_int_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + mavlinkContext.recvMsg.sysid, + mavlinkContext.recvMsg.compid, + incomingMissionWpSequence, + MAV_MISSION_TYPE_MISSION); + mavlinkSendMessage(); + return true; + } + + mavlinkSendMissionAck(MAV_MISSION_NO_SPACE); + return true; + } + + return false; +} + +bool mavlinkHandleIncomingMissionItem(void) +{ + mavlink_mission_item_t msg; + mavlink_msg_mission_item_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + if (ARMING_FLAG(ARMED)) { + if (msg.command == MAV_CMD_NAV_WAYPOINT) { + return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, + MAV_FRAME_SUPPORTED_GLOBAL | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT, + (int32_t)lrintf(msg.x * 1e7f), (int32_t)lrintf(msg.y * 1e7f), msg.z); + } + + mavlinkSendMissionAck(MAV_MISSION_ERROR); + return true; + } + + return mavlinkHandleMissionItemCommon(false, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.param1, msg.param2, msg.param3, msg.param4, (int32_t)(msg.x * 1e7f), (int32_t)(msg.y * 1e7f), msg.z); +} + +bool mavlinkHandleIncomingMissionRequestList(void) +{ + mavlink_mission_request_list_t msg; + mavlink_msg_mission_request_list_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system == mavSystemId) { + mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + return false; +} + +bool mavlinkFillMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item) +{ + mavlinkMissionItemData_t data = {0}; + + data.frame = mavlinkWaypointFrame(wp, useIntMessages); + + switch (wp->action) { + case NAV_WP_ACTION_WAYPOINT: + data.command = MAV_CMD_NAV_WAYPOINT; + data.lat = wp->lat; + data.lon = wp->lon; + data.alt = wp->alt / 100.0f; + break; + + case NAV_WP_ACTION_HOLD_TIME: + data.command = MAV_CMD_NAV_LOITER_TIME; + data.param1 = wp->p1; + data.lat = wp->lat; + data.lon = wp->lon; + data.alt = wp->alt / 100.0f; + break; + + case NAV_WP_ACTION_RTH: + data.command = MAV_CMD_NAV_RETURN_TO_LAUNCH; + break; + + case NAV_WP_ACTION_LAND: + data.command = MAV_CMD_NAV_LAND; + data.lat = wp->lat; + data.lon = wp->lon; + data.alt = wp->alt / 100.0f; + break; + + case NAV_WP_ACTION_JUMP: + data.command = MAV_CMD_DO_JUMP; + data.param1 = (wp->p1 > 0) ? (float)(wp->p1 - 1) : 0.0f; + data.param2 = wp->p2; + break; + + case NAV_WP_ACTION_SET_POI: + data.command = MAV_CMD_DO_SET_ROI; + data.param1 = MAV_ROI_LOCATION; + data.lat = wp->lat; + data.lon = wp->lon; + data.alt = wp->alt / 100.0f; + break; + + case NAV_WP_ACTION_SET_HEAD: + data.command = MAV_CMD_CONDITION_YAW; + data.param1 = wp->p1; + break; + + default: + return false; + } + + *item = data; + return true; +} + +bool mavlinkHandleIncomingMissionRequest(void) +{ + mavlink_mission_request_t msg; + mavlink_msg_mission_request_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + const int wpCount = getWaypointCount(); + + if (msg.seq < wpCount) { + navWaypoint_t wp; + getWaypoint(msg.seq + 1, &wp); + + mavlinkMissionItemData_t item; + if (mavlinkFillMissionItemFromWaypoint(&wp, false, &item)) { + mavlink_msg_mission_item_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, + msg.seq, + item.frame, + item.command, + 0, + 1, + item.param1, item.param2, item.param3, item.param4, + item.lat / 1e7f, + item.lon / 1e7f, + item.alt, + MAV_MISSION_TYPE_MISSION); + mavlinkSendMessage(); + } else { + mavlinkSendMissionAck(MAV_MISSION_ERROR); + } + } else { + mavlinkSendMissionAck(MAV_MISSION_INVALID_SEQUENCE); + } + + return true; +} + +bool mavlinkHandleIncomingMissionItemInt(void) +{ + mavlink_mission_item_int_t msg; + mavlink_msg_mission_item_int_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + if (ARMING_FLAG(ARMED)) { + if (msg.command == MAV_CMD_NAV_WAYPOINT) { + return mavlinkHandleArmedGuidedMissionItem(msg.current, msg.frame, + MAV_FRAME_SUPPORTED_GLOBAL_INT | MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT, + msg.x, msg.y, msg.z); + } + + mavlinkSendMissionAck(MAV_MISSION_ERROR); + return true; + } + + return mavlinkHandleMissionItemCommon(true, msg.frame, msg.command, msg.autocontinue, msg.seq, msg.param1, msg.param2, msg.param3, msg.param4, msg.x, msg.y, msg.z); +} + +bool mavlinkHandleIncomingMissionRequestInt(void) +{ + mavlink_mission_request_int_t msg; + mavlink_msg_mission_request_int_decode(&mavlinkContext.recvMsg, &msg); + + if (msg.target_system != mavSystemId) { + return false; + } + + const int wpCount = getWaypointCount(); + + if (msg.seq < wpCount) { + navWaypoint_t wp; + getWaypoint(msg.seq + 1, &wp); + + mavlinkMissionItemData_t item; + if (mavlinkFillMissionItemFromWaypoint(&wp, true, &item)) { + mavlink_msg_mission_item_int_pack(mavSystemId, mavComponentId, &mavSendMsg, mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid, + msg.seq, + item.frame, + item.command, + 0, + 1, + item.param1, item.param2, item.param3, item.param4, + item.lat, + item.lon, + item.alt, + MAV_MISSION_TYPE_MISSION); + mavlinkSendMessage(); + } else { + mavlinkSendMissionAck(MAV_MISSION_ERROR); + } + } else { + mavlinkSendMissionAck(MAV_MISSION_INVALID_SEQUENCE); + } + + return true; +} + +#endif diff --git a/src/main/mavlink/mavlink_mission.h b/src/main/mavlink/mavlink_mission.h new file mode 100644 index 00000000000..41010e85f07 --- /dev/null +++ b/src/main/mavlink/mavlink_mission.h @@ -0,0 +1,39 @@ +#pragma once + +#include +#include + +#include "navigation/navigation.h" + +#include "mavlink/mavlink_types.h" + +typedef enum { + MAV_FRAME_SUPPORTED_NONE = 0, + MAV_FRAME_SUPPORTED_GLOBAL = (1 << 0), + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT = (1 << 1), + MAV_FRAME_SUPPORTED_GLOBAL_INT = (1 << 2), + MAV_FRAME_SUPPORTED_GLOBAL_RELATIVE_ALT_INT = (1 << 3), +} mavFrameSupportMask_e; + +typedef struct mavlinkMissionItemData_s { + uint8_t frame; + uint16_t command; + float param1; + float param2; + float param3; + float param4; + int32_t lat; + int32_t lon; + float alt; +} mavlinkMissionItemData_t; + +uint8_t mavlinkWaypointFrame(const navWaypoint_t *wp, bool useIntMessages); +bool mavlinkFillMissionItemFromWaypoint(const navWaypoint_t *wp, bool useIntMessages, mavlinkMissionItemData_t *item); +void mavlinkSendPendingMissionItemReached(void); +bool mavlinkHandleIncomingMissionClearAll(void); +bool mavlinkHandleIncomingMissionCount(void); +bool mavlinkHandleIncomingMissionItem(void); +bool mavlinkHandleIncomingMissionRequestList(void); +bool mavlinkHandleIncomingMissionRequest(void); +bool mavlinkHandleIncomingMissionItemInt(void); +bool mavlinkHandleIncomingMissionRequestInt(void); diff --git a/src/main/mavlink/mavlink_modes.c b/src/main/mavlink/mavlink_modes.c new file mode 100644 index 00000000000..33ff0bfe1b9 --- /dev/null +++ b/src/main/mavlink/mavlink_modes.c @@ -0,0 +1,295 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_modes.h" +#include "mavlink/mavlink_runtime.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +typedef struct mavlinkModeDescriptor_s { + uint8_t customMode; + const char *name; +} mavlinkModeDescriptor_t; + +static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode) +{ + switch (flightMode) + { + case FLM_ACRO: return COPTER_MODE_ACRO; + case FLM_ACRO_AIR: return COPTER_MODE_ACRO; + case FLM_ANGLE: return COPTER_MODE_STABILIZE; + case FLM_HORIZON: return COPTER_MODE_STABILIZE; + case FLM_ANGLEHOLD: return COPTER_MODE_STABILIZE; + case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; + case FLM_POSITION_HOLD: + { + if (isGCSValid()) { + return COPTER_MODE_GUIDED; + } else { + return COPTER_MODE_POSHOLD; + } + } + case FLM_RTH: return COPTER_MODE_RTL; + case FLM_MISSION: return COPTER_MODE_AUTO; + case FLM_LAUNCH: return COPTER_MODE_THROW; + case FLM_FAILSAFE: + { + if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { + return COPTER_MODE_RTL; + } else if (failsafePhase() == FAILSAFE_LANDING) { + return COPTER_MODE_LAND; + } else { + return COPTER_MODE_RTL; + } + } + default: return COPTER_MODE_STABILIZE; + } +} + +static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode) +{ + switch (flightMode) + { + case FLM_MANUAL: return PLANE_MODE_MANUAL; + case FLM_ACRO: return PLANE_MODE_ACRO; + case FLM_ACRO_AIR: return PLANE_MODE_ACRO; + case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A; + case FLM_HORIZON: return PLANE_MODE_STABILIZE; + case FLM_ANGLEHOLD: return PLANE_MODE_STABILIZE; + case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; + case FLM_POSITION_HOLD: + { + if (isGCSValid()) { + return PLANE_MODE_GUIDED; + } else { + return PLANE_MODE_LOITER; + } + } + case FLM_RTH: return PLANE_MODE_RTL; + case FLM_MISSION: return PLANE_MODE_AUTO; + case FLM_CRUISE: return PLANE_MODE_CRUISE; + case FLM_LAUNCH: return PLANE_MODE_TAKEOFF; + case FLM_FAILSAFE: + { + if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { + return PLANE_MODE_RTL; + } else if (failsafePhase() == FAILSAFE_LANDING) { + return PLANE_MODE_AUTOLAND; + } else { + return PLANE_MODE_RTL; + } + } + default: return PLANE_MODE_MANUAL; + } +} + +static const mavlinkModeDescriptor_t planeModes[] = { + { PLANE_MODE_MANUAL, "MANUAL" }, + { PLANE_MODE_ACRO, "ACRO" }, + { PLANE_MODE_STABILIZE, "STABILIZE" }, + { PLANE_MODE_FLY_BY_WIRE_A, "FBWA" }, + { PLANE_MODE_FLY_BY_WIRE_B, "FBWB" }, + { PLANE_MODE_CRUISE, "CRUISE" }, + { PLANE_MODE_AUTO, "AUTO" }, + { PLANE_MODE_RTL, "RTL" }, + { PLANE_MODE_LOITER, "LOITER" }, + { PLANE_MODE_TAKEOFF, "TAKEOFF" }, + { PLANE_MODE_GUIDED, "GUIDED" }, +}; + +static const mavlinkModeDescriptor_t copterModes[] = { + { COPTER_MODE_ACRO, "ACRO" }, + { COPTER_MODE_STABILIZE, "STABILIZE" }, + { COPTER_MODE_ALT_HOLD, "ALT_HOLD" }, + { COPTER_MODE_POSHOLD, "POSHOLD" }, + { COPTER_MODE_LOITER, "LOITER" }, + { COPTER_MODE_AUTO, "AUTO" }, + { COPTER_MODE_GUIDED, "GUIDED" }, + { COPTER_MODE_RTL, "RTL" }, + { COPTER_MODE_LAND, "LAND" }, + { COPTER_MODE_BRAKE, "BRAKE" }, + { COPTER_MODE_THROW, "THROW" }, + { COPTER_MODE_SMART_RTL, "SMART_RTL" }, + { COPTER_MODE_DRIFT, "DRIFT" }, +}; + +static bool mavlinkPlaneModeIsConfigured(uint8_t customMode) +{ + switch ((APM_PLANE_MODE)customMode) { + case PLANE_MODE_MANUAL: + return isModeActivationConditionPresent(BOXMANUAL); + case PLANE_MODE_ACRO: + return true; + case PLANE_MODE_STABILIZE: + return isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXANGLEHOLD); + case PLANE_MODE_FLY_BY_WIRE_A: + return isModeActivationConditionPresent(BOXANGLE); + case PLANE_MODE_FLY_BY_WIRE_B: + return isModeActivationConditionPresent(BOXNAVALTHOLD); + case PLANE_MODE_CRUISE: + return isModeActivationConditionPresent(BOXNAVCRUISE) || + (isModeActivationConditionPresent(BOXNAVCOURSEHOLD) && + isModeActivationConditionPresent(BOXNAVALTHOLD)); + case PLANE_MODE_AUTO: + return isModeActivationConditionPresent(BOXNAVWP); + case PLANE_MODE_RTL: + return isModeActivationConditionPresent(BOXNAVRTH); + case PLANE_MODE_GUIDED: + return isModeActivationConditionPresent(BOXNAVPOSHOLD) && + isModeActivationConditionPresent(BOXGCSNAV); + case PLANE_MODE_LOITER: + return isModeActivationConditionPresent(BOXNAVPOSHOLD); + case PLANE_MODE_TAKEOFF: + return isModeActivationConditionPresent(BOXNAVLAUNCH); + default: + return false; + } +} + +static bool mavlinkCopterModeIsConfigured(uint8_t customMode) +{ + switch ((APM_COPTER_MODE)customMode) { + case COPTER_MODE_ACRO: + return true; + case COPTER_MODE_STABILIZE: + return isModeActivationConditionPresent(BOXANGLE) || + isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXANGLEHOLD); + case COPTER_MODE_ALT_HOLD: + return isModeActivationConditionPresent(BOXNAVALTHOLD); + case COPTER_MODE_GUIDED: + return isModeActivationConditionPresent(BOXNAVPOSHOLD) && + isModeActivationConditionPresent(BOXGCSNAV); + case COPTER_MODE_POSHOLD: + return isModeActivationConditionPresent(BOXNAVPOSHOLD); + case COPTER_MODE_RTL: + return isModeActivationConditionPresent(BOXNAVRTH); + case COPTER_MODE_AUTO: + return isModeActivationConditionPresent(BOXNAVWP); + case COPTER_MODE_THROW: + return isModeActivationConditionPresent(BOXNAVLAUNCH); + case COPTER_MODE_BRAKE: + return isModeActivationConditionPresent(BOXBRAKING); + default: + return false; + } +} + +static void mavlinkSendAvailableModes(const mavlinkModeDescriptor_t *modes, uint8_t count, uint8_t currentCustom, + bool (*isModeConfigured)(uint8_t customMode)) +{ + uint8_t availableCount = 0; + for (uint8_t i = 0; i < count; i++) { + if (isModeConfigured(modes[i].customMode)) { + availableCount++; + } + } + + if (availableCount == 0) { + return; + } + + uint8_t modeIndex = 0; + for (uint8_t i = 0; i < count; i++) { + if (!isModeConfigured(modes[i].customMode)) { + continue; + } + + modeIndex++; + + mavlink_msg_available_modes_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + availableCount, + modeIndex, + MAV_STANDARD_MODE_NON_STANDARD, + modes[i].customMode, + 0, + modes[i].name); + + mavlinkSendMessage(); + + if (modes[i].customMode == currentCustom) { + const mavlinkModeSelection_t modeSelection = { + .flightMode = FLM_MANUAL, + .customMode = currentCustom, + }; + mavlinkSendCurrentMode(&modeSelection); + } + } +} + +bool mavlinkIsFixedWingVehicle(void) +{ + return STATE(FIXED_WING_LEGACY) || mixerConfig()->platformType == PLATFORM_AIRPLANE; +} + +uint8_t mavlinkGetVehicleType(void) +{ + switch (mixerConfig()->platformType) + { + case PLATFORM_MULTIROTOR: + return MAV_TYPE_QUADROTOR; + case PLATFORM_TRICOPTER: + return MAV_TYPE_TRICOPTER; + case PLATFORM_AIRPLANE: + return MAV_TYPE_FIXED_WING; + case PLATFORM_ROVER: + return MAV_TYPE_GROUND_ROVER; + case PLATFORM_BOAT: + return MAV_TYPE_SURFACE_BOAT; + case PLATFORM_HELICOPTER: + return MAV_TYPE_HELICOPTER; + default: + return MAV_TYPE_GENERIC; + } +} + +uint8_t mavlinkGetAutopilotEnum(void) +{ + if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { + return MAV_AUTOPILOT_ARDUPILOTMEGA; + } + + return MAV_AUTOPILOT_GENERIC; +} + +mavlinkModeSelection_t mavlinkSelectMode(void) +{ + mavlinkModeSelection_t modeSelection; + modeSelection.flightMode = getFlightModeForTelemetry(); + + if (mavlinkIsFixedWingVehicle()) { + modeSelection.customMode = (uint8_t)inavToArduPlaneMap(modeSelection.flightMode); + } else { + modeSelection.customMode = (uint8_t)inavToArduCopterMap(modeSelection.flightMode); + } + + return modeSelection; +} + +void mavlinkSendAvailableModesForCurrentMode(void) +{ + const mavlinkModeSelection_t modeSelection = mavlinkSelectMode(); + + if (mavlinkIsFixedWingVehicle()) { + mavlinkSendAvailableModes(planeModes, (uint8_t)ARRAYLEN(planeModes), modeSelection.customMode, mavlinkPlaneModeIsConfigured); + } else { + mavlinkSendAvailableModes(copterModes, (uint8_t)ARRAYLEN(copterModes), modeSelection.customMode, mavlinkCopterModeIsConfigured); + } +} + +void mavlinkSendCurrentMode(const mavlinkModeSelection_t *modeSelection) +{ + mavlink_msg_current_mode_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + MAV_STANDARD_MODE_NON_STANDARD, + modeSelection->customMode, + modeSelection->customMode); + mavlinkSendMessage(); +} + +#endif diff --git a/src/main/mavlink/mavlink_modes.h b/src/main/mavlink/mavlink_modes.h new file mode 100644 index 00000000000..a793f4a834f --- /dev/null +++ b/src/main/mavlink/mavlink_modes.h @@ -0,0 +1,82 @@ +#pragma once + +#include +#include + +#include "fc/runtime_config.h" + +/** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ +typedef enum APM_PLANE_MODE +{ + PLANE_MODE_MANUAL=0, + PLANE_MODE_CIRCLE=1, + PLANE_MODE_STABILIZE=2, + PLANE_MODE_TRAINING=3, + PLANE_MODE_ACRO=4, + PLANE_MODE_FLY_BY_WIRE_A=5, + PLANE_MODE_FLY_BY_WIRE_B=6, + PLANE_MODE_CRUISE=7, + PLANE_MODE_AUTOTUNE=8, + PLANE_MODE_AUTO=10, + PLANE_MODE_RTL=11, + PLANE_MODE_LOITER=12, + PLANE_MODE_TAKEOFF=13, + PLANE_MODE_AVOID_ADSB=14, + PLANE_MODE_GUIDED=15, + PLANE_MODE_INITIALIZING=16, + PLANE_MODE_QSTABILIZE=17, + PLANE_MODE_QHOVER=18, + PLANE_MODE_QLOITER=19, + PLANE_MODE_QLAND=20, + PLANE_MODE_QRTL=21, + PLANE_MODE_QAUTOTUNE=22, + PLANE_MODE_QACRO=23, + PLANE_MODE_THERMAL=24, + PLANE_MODE_LOITER_ALT_QLAND=25, + PLANE_MODE_AUTOLAND=26, + PLANE_MODE_ENUM_END=27, +} APM_PLANE_MODE; + +/** @brief A mapping of copter flight modes for custom_mode field of heartbeat. */ +typedef enum APM_COPTER_MODE +{ + COPTER_MODE_STABILIZE=0, + COPTER_MODE_ACRO=1, + COPTER_MODE_ALT_HOLD=2, + COPTER_MODE_AUTO=3, + COPTER_MODE_GUIDED=4, + COPTER_MODE_LOITER=5, + COPTER_MODE_RTL=6, + COPTER_MODE_CIRCLE=7, + COPTER_MODE_LAND=9, + COPTER_MODE_DRIFT=11, + COPTER_MODE_SPORT=13, + COPTER_MODE_FLIP=14, + COPTER_MODE_AUTOTUNE=15, + COPTER_MODE_POSHOLD=16, + COPTER_MODE_BRAKE=17, + COPTER_MODE_THROW=18, + COPTER_MODE_AVOID_ADSB=19, + COPTER_MODE_GUIDED_NOGPS=20, + COPTER_MODE_SMART_RTL=21, + COPTER_MODE_FLOWHOLD=22, + COPTER_MODE_FOLLOW=23, + COPTER_MODE_ZIGZAG=24, + COPTER_MODE_SYSTEMID=25, + COPTER_MODE_AUTOROTATE=26, + COPTER_MODE_AUTO_RTL=27, + COPTER_MODE_TURTLE=28, + COPTER_MODE_ENUM_END=29, +} APM_COPTER_MODE; + +typedef struct mavlinkModeSelection_s { + flightModeForTelemetry_e flightMode; + uint8_t customMode; +} mavlinkModeSelection_t; + +bool mavlinkIsFixedWingVehicle(void); +uint8_t mavlinkGetVehicleType(void); +uint8_t mavlinkGetAutopilotEnum(void); +mavlinkModeSelection_t mavlinkSelectMode(void); +void mavlinkSendAvailableModesForCurrentMode(void); +void mavlinkSendCurrentMode(const mavlinkModeSelection_t *modeSelection); diff --git a/src/main/mavlink/mavlink_ports.c b/src/main/mavlink/mavlink_ports.c new file mode 100644 index 00000000000..c3b9f81706e --- /dev/null +++ b/src/main/mavlink/mavlink_ports.c @@ -0,0 +1,75 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_ports.h" +#include "mavlink/mavlink_runtime.h" +#include "mavlink/mavlink_streams.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex) +{ + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + if (state->port) { + closeSerialPort(state->port); + } + + state->port = NULL; + state->telemetryEnabled = false; + state->txbuffValid = false; + state->txbuffFree = 100; + state->lastMavlinkMessageUs = 0; + state->lastRxFrameUs = 0; + state->lastHighLatencyMessageUs = 0; + state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; + state->txSeq = 0; + state->txDroppedFrames = 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)); + resetMspPort(&mavTunnelMspPorts[portIndex], NULL); + mavTunnelRemoteSystemIds[portIndex] = 0; + mavTunnelRemoteComponentIds[portIndex] = 0; +} + +void configureMAVLinkTelemetryPort(uint8_t portIndex) +{ + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + if (!state->portConfig) { + return; + } + + baudRate_e baudRateIndex = state->portConfig->telemetry_baudrateIndex; + if (baudRateIndex == BAUD_AUTO) { + // default rate for minimOSD + baudRateIndex = BAUD_57600; + } + + state->port = openSerialPort(state->portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_PORT_MODE, SERIAL_NOT_INVERTED); + if (!state->port) { + return; + } + + state->telemetryEnabled = true; + state->txbuffValid = false; + state->txbuffFree = 100; + state->lastMavlinkMessageUs = 0; + state->lastRxFrameUs = 0; + state->lastHighLatencyMessageUs = 0; + state->highLatencyEnabled = mavlinkGetPortConfig(portIndex)->high_latency; + state->txSeq = 0; + state->txDroppedFrames = 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)); + resetMspPort(&mavTunnelMspPorts[portIndex], NULL); + mavTunnelRemoteSystemIds[portIndex] = 0; + mavTunnelRemoteComponentIds[portIndex] = 0; +} + +#endif diff --git a/src/main/mavlink/mavlink_ports.h b/src/main/mavlink/mavlink_ports.h new file mode 100644 index 00000000000..c0b325a354c --- /dev/null +++ b/src/main/mavlink/mavlink_ports.h @@ -0,0 +1,4 @@ +#pragma once + +void configureMAVLinkTelemetryPort(uint8_t portIndex); +void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex); diff --git a/src/main/mavlink/mavlink_routing.c b/src/main/mavlink/mavlink_routing.c new file mode 100644 index 00000000000..f0b0b373d11 --- /dev/null +++ b/src/main/mavlink/mavlink_routing.c @@ -0,0 +1,172 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_routing.h" +#include "mavlink/mavlink_runtime.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid) +{ + return sysid == mavlinkGetCommonConfig()->sysid && compid == MAV_COMP_ID_AUTOPILOT1; +} + +void mavlinkLearnRoute(uint8_t ingressPortIndex) +{ + if (mavlinkContext.recvMsg.sysid == 0 || mavlinkIsFromLocalIdentity(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid)) { + return; + } + + for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) { + mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex]; + if (route->sysid == mavlinkContext.recvMsg.sysid && route->compid == mavlinkContext.recvMsg.compid) { + route->ingressPortIndex = ingressPortIndex; + return; + } + } + + if (mavRouteCount >= MAVLINK_MAX_ROUTES) { + return; + } + + mavRouteTable[mavRouteCount].sysid = mavlinkContext.recvMsg.sysid; + mavRouteTable[mavRouteCount].compid = mavlinkContext.recvMsg.compid; + mavRouteTable[mavRouteCount].ingressPortIndex = ingressPortIndex; + mavRouteCount++; +} + +void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent) +{ + *targetSystem = -1; + *targetComponent = -1; + + const mavlink_msg_entry_t *msgEntry = mavlink_get_msg_entry(msg->msgid); + if (!msgEntry) { + return; + } + + if (msgEntry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { + *targetSystem = _MAV_RETURN_uint8_t(msg, msgEntry->target_system_ofs); + } + + if (msgEntry->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) { + *targetComponent = _MAV_RETURN_uint8_t(msg, msgEntry->target_component_ofs); + } +} + +static uint8_t mavlinkComputeForwardMask(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent) +{ + uint8_t mask = 0; + + if (targetSystem <= 0) { + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if (portIndex == ingressPortIndex) { + continue; + } + + const mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } + + mask |= MAVLINK_PORT_MASK(portIndex); + } + return mask; + } + + for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) { + const mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex]; + + if (route->sysid != targetSystem) { + continue; + } + if (targetComponent > 0 && route->compid != targetComponent) { + continue; + } + if (route->ingressPortIndex == ingressPortIndex || route->ingressPortIndex >= mavPortCount) { + continue; + } + + const mavlinkPortRuntime_t *state = &mavPortStates[route->ingressPortIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } + + mask |= MAVLINK_PORT_MASK(route->ingressPortIndex); + } + + return mask; +} + +void mavlinkForwardMessage(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent) +{ + if (mavlinkContext.recvMsg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + return; + } + + uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; + const int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavlinkContext.recvMsg); + if (msgLength <= 0) { + return; + } + + const uint8_t forwardMask = mavlinkComputeForwardMask(ingressPortIndex, targetSystem, targetComponent); + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if ((forwardMask & MAVLINK_PORT_MASK(portIndex)) == 0) { + continue; + } + + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (serialTxBytesFree(state->port) < (uint32_t)msgLength) { + state->txDroppedFrames++; + continue; + } + + serialBeginWrite(state->port); + serialWriteBuf(state->port, mavBuffer, msgLength); + serialEndWrite(state->port); + } +} + +int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t targetComponent, uint8_t ingressPortIndex) +{ + if (targetSystem <= 0) { + return ingressPortIndex; + } + + if ((uint8_t)targetSystem != mavlinkGetCommonConfig()->sysid) { + return -1; + } + + if (targetComponent > 0 && (uint8_t)targetComponent != MAV_COMP_ID_AUTOPILOT1) { + return -1; + } + + if (ingressPortIndex < mavPortCount) { + return ingressPortIndex; + } + + return mavPortCount > 0 ? 0 : -1; +} + +bool mavlinkShouldFanOutLocalBroadcast(const mavlink_message_t *msg) +{ + if (msg->msgid == MAVLINK_MSG_ID_REQUEST_DATA_STREAM) { + return true; + } + + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + mavlink_command_long_t cmd; + mavlink_msg_command_long_decode(msg, &cmd); + return cmd.command == MAV_CMD_SET_MESSAGE_INTERVAL || cmd.command == MAV_CMD_CONTROL_HIGH_LATENCY; + } + + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_INT) { + mavlink_command_int_t cmd; + mavlink_msg_command_int_decode(msg, &cmd); + return cmd.command == MAV_CMD_SET_MESSAGE_INTERVAL || cmd.command == MAV_CMD_CONTROL_HIGH_LATENCY; + } + + return false; +} + +#endif diff --git a/src/main/mavlink/mavlink_routing.h b/src/main/mavlink/mavlink_routing.h new file mode 100644 index 00000000000..ae63f372416 --- /dev/null +++ b/src/main/mavlink/mavlink_routing.h @@ -0,0 +1,13 @@ +#pragma once + +#include +#include + +#include "mavlink/mavlink_types.h" + +bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid); +void mavlinkLearnRoute(uint8_t ingressPortIndex); +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); +bool mavlinkShouldFanOutLocalBroadcast(const mavlink_message_t *msg); diff --git a/src/main/mavlink/mavlink_runtime.c b/src/main/mavlink/mavlink_runtime.c new file mode 100644 index 00000000000..6905f0a0209 --- /dev/null +++ b/src/main/mavlink/mavlink_runtime.c @@ -0,0 +1,295 @@ +#include "mavlink/mavlink_internal.h" + +#include "fc/fc_mavlink.h" + +#include "mavlink/mavlink_mission.h" +#include "mavlink/mavlink_ports.h" +#include "mavlink/mavlink_routing.h" +#include "mavlink/mavlink_runtime.h" +#include "mavlink/mavlink_streams.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +mavlinkContext_t mavlinkContext; + +const mavlinkTelemetryPortConfig_t *mavlinkGetPortConfig(uint8_t portIndex) +{ + return &telemetryConfig()->mavlink[portIndex]; +} + +const mavlinkTelemetryCommonConfig_t *mavlinkGetCommonConfig(void) +{ + return &telemetryConfig()->mavlink_common; +} + +uint8_t mavlinkGetProtocolVersion(void) +{ + return mavlinkGetCommonConfig()->version; +} + +static void mavlinkApplyActivePortOutputVersion(void) +{ + mavlink_status_t *chanState = mavlink_get_channel_status(MAVLINK_COMM_0); + if (mavlinkGetProtocolVersion() == 1) { + chanState->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } else { + chanState->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } +} + +void mavlinkSetActivePortContext(uint8_t portIndex) +{ + mavActivePort = &mavPortStates[portIndex]; + mavActiveConfig = mavlinkGetPortConfig(portIndex); + const mavlinkTelemetryCommonConfig_t *commonConfig = mavlinkGetCommonConfig(); + mavAutopilotType = commonConfig->autopilot_type; + mavSystemId = commonConfig->sysid; + mavComponentId = MAV_COMP_ID_AUTOPILOT1; + mavlinkApplyActivePortOutputVersion(); +} + +void mavlinkRuntimeFreePorts(void) +{ + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + freeMAVLinkTelemetryPortByIndex(portIndex); + } + + mavSendMask = 0; + mavRouteCount = 0; +} + +void mavlinkRuntimeInit(void) +{ + memset(&mavlinkContext, 0, sizeof(mavlinkContext)); + mavSystemId = 1; + mavComponentId = MAV_COMP_ID_AUTOPILOT1; + + const serialPortConfig_t *serialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); + while (serialPortConfig && mavPortCount < MAX_MAVLINK_PORTS) { + mavlinkPortRuntime_t *state = &mavPortStates[mavPortCount]; + state->portConfig = serialPortConfig; + state->portSharing = determinePortSharing(serialPortConfig, FUNCTION_TELEMETRY_MAVLINK); + state->txbuffFree = 100; + state->highLatencyEnabled = mavlinkGetPortConfig(mavPortCount)->high_latency; + configureMAVLinkStreamRates(mavPortCount); + + mavPortCount++; + serialPortConfig = findNextSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); + } +} + +void mavlinkRuntimeCheckState(void) +{ + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + bool newTelemetryEnabledValue = telemetryDetermineEnabledState(state->portSharing); + if ((state->portConfig->functionMask & FUNCTION_RX_SERIAL) && + rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { + newTelemetryEnabledValue = true; + } + + if (newTelemetryEnabledValue == state->telemetryEnabled) { + continue; + } + + if (newTelemetryEnabledValue) { + configureMAVLinkTelemetryPort(portIndex); + if (state->telemetryEnabled) { + configureMAVLinkStreamRates(portIndex); + } + } else { + freeMAVLinkTelemetryPortByIndex(portIndex); + } + } +} + +void mavlinkSendMessage(void) +{ + const mavlink_msg_entry_t *msgEntry = mavlink_get_msg_entry(mavSendMsg.msgid); + if (!msgEntry) { + return; + } + + uint8_t sendMask = mavSendMask; + if (sendMask == 0) { + if (mavActivePort) { + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if (&mavPortStates[portIndex] == mavActivePort) { + sendMask = MAVLINK_PORT_MASK(portIndex); + break; + } + } + } else if (mavPortCount == 1 && mavPortStates[0].telemetryEnabled && mavPortStates[0].port) { + sendMask = MAVLINK_PORT_MASK(0); + } + } + + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + if ((sendMask & MAVLINK_PORT_MASK(portIndex)) == 0) { + continue; + } + + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } + + mavlink_status_t txStatus = { 0 }; + txStatus.current_tx_seq = state->txSeq; + if (mavlinkGetProtocolVersion() == 1) { + txStatus.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } + + mavlink_message_t txMsg = mavSendMsg; + mavlink_finalize_message_buffer( + &txMsg, + txMsg.sysid, + txMsg.compid, + &txStatus, + msgEntry->min_msg_len, + txMsg.len, + msgEntry->crc_extra + ); + state->txSeq = txStatus.current_tx_seq; + + uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; + const int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &txMsg); + if (msgLength <= 0) { + continue; + } + + // Drop the frame on this port if there is no room; do not block telemetry task. + if (serialTxBytesFree(state->port) < (uint32_t)msgLength) { + state->txDroppedFrames++; + continue; + } + + serialBeginWrite(state->port); + serialWriteBuf(state->port, mavBuffer, msgLength); + serialEndWrite(state->port); + } +} + +static bool processMAVLinkIncomingTelemetry(uint8_t ingressPortIndex, timeUs_t currentTimeUs) +{ + mavlinkPortRuntime_t *state = &mavPortStates[ingressPortIndex]; + + while (serialRxBytesWaiting(state->port) > 0) { + // Limit handling to one message per cycle + const char c = serialRead(state->port); + const uint8_t result = mavlink_parse_char(ingressPortIndex, c, &state->mavRecvMsg, &state->mavRecvStatus); + if (result == MAVLINK_FRAMING_OK) { + state->lastRxFrameUs = currentTimeUs; + mavlinkContext.recvMsg = state->mavRecvMsg; + + if (mavlinkIsFromLocalIdentity(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid)) { + return false; + } + + mavlinkLearnRoute(ingressPortIndex); + + int16_t targetSystem; + int16_t targetComponent; + mavlinkExtractTargets(&mavlinkContext.recvMsg, &targetSystem, &targetComponent); + mavlinkForwardMessage(ingressPortIndex, targetSystem, targetComponent); + + if (mavlinkContext.recvMsg.msgid == MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE) { + mavlinkSetActivePortContext(ingressPortIndex); + mavSendMask = MAVLINK_PORT_MASK(ingressPortIndex); + const mavlinkFcDispatchResult_e dispatchResult = mavlinkFcDispatchIncomingMessage(ingressPortIndex); + return dispatchResult == MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY; + } + + const int8_t localPortIndex = mavlinkResolveLocalPortForTarget(targetSystem, targetComponent, ingressPortIndex); + if (localPortIndex < 0) { + return false; + } + + uint8_t localPortMask = MAVLINK_PORT_MASK((uint8_t)localPortIndex); + const bool isLocalOrSystemBroadcast = targetSystem == 0 || ((targetSystem > 0) && ((uint8_t)targetSystem == mavlinkGetCommonConfig()->sysid)); + if (targetComponent == 0 && isLocalOrSystemBroadcast && mavlinkShouldFanOutLocalBroadcast(&mavlinkContext.recvMsg)) { + localPortMask = 0; + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + localPortMask |= MAVLINK_PORT_MASK(portIndex); + } + } + + bool handled = false; + for (uint8_t localIndex = 0; localIndex < mavPortCount; localIndex++) { + if (!(localPortMask & MAVLINK_PORT_MASK(localIndex))) { + continue; + } + + mavlinkSetActivePortContext(localIndex); + mavSendMask = MAVLINK_PORT_MASK(ingressPortIndex); + + const mavlinkFcDispatchResult_e dispatchResult = mavlinkFcDispatchIncomingMessage(ingressPortIndex); + handled = handled || dispatchResult == MAVLINK_FC_DISPATCH_HANDLED_ACTIVITY; + } + + return handled; + } + } + + return false; +} + +static bool isMAVLinkTelemetryHalfDuplex(uint8_t portIndex) +{ + const mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + return state->portConfig && + (state->portConfig->functionMask & FUNCTION_RX_SERIAL) && + rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK && + tristateWithDefaultOffIsActive(rxConfig()->halfDuplex); +} + +void mavlinkRuntimeHandle(timeUs_t currentTimeUs) +{ + mavlinkSendPendingMissionItemReached(); + + for (uint8_t portIndex = 0; portIndex < mavPortCount; portIndex++) { + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + if (!state->telemetryEnabled || !state->port) { + continue; + } + + mavlinkSetActivePortContext(portIndex); + + // Process incoming MAVLink on this port and forward when needed. + processMAVLinkIncomingTelemetry(portIndex, currentTimeUs); + + // Restore context back to this port before periodic send decisions. + mavlinkSetActivePortContext(portIndex); + bool shouldSendTelemetry = false; + const bool halfDuplexBackoff = isMAVLinkTelemetryHalfDuplex(portIndex) && + ((currentTimeUs - state->lastRxFrameUs) < TELEMETRY_MAVLINK_DELAY); + + if (halfDuplexBackoff) { + continue; + } + + if (state->txbuffValid) { + // Use flow control if available. + shouldSendTelemetry = state->txbuffFree >= mavActiveConfig->min_txbuff; + } else { + // If not, use blind frame pacing. + shouldSendTelemetry = (currentTimeUs - state->lastMavlinkMessageUs) >= TELEMETRY_MAVLINK_DELAY; + } + + if (!shouldSendTelemetry) { + continue; + } + + mavSendMask = MAVLINK_PORT_MASK(portIndex); + processMAVLinkTelemetry(currentTimeUs); + state->lastMavlinkMessageUs = currentTimeUs; + } + + mavSendMask = 0; +} + +#endif diff --git a/src/main/mavlink/mavlink_runtime.h b/src/main/mavlink/mavlink_runtime.h new file mode 100644 index 00000000000..da5e1394eac --- /dev/null +++ b/src/main/mavlink/mavlink_runtime.h @@ -0,0 +1,16 @@ +#pragma once + +#include "common/time.h" + +#include "telemetry/telemetry.h" + +void mavlinkRuntimeInit(void); +void mavlinkRuntimeHandle(timeUs_t currentTimeUs); +void mavlinkRuntimeCheckState(void); +void mavlinkRuntimeFreePorts(void); + +const mavlinkTelemetryPortConfig_t *mavlinkGetPortConfig(uint8_t portIndex); +const mavlinkTelemetryCommonConfig_t *mavlinkGetCommonConfig(void); +uint8_t mavlinkGetProtocolVersion(void); +void mavlinkSetActivePortContext(uint8_t portIndex); +void mavlinkSendMessage(void); diff --git a/src/main/mavlink/mavlink_streams.c b/src/main/mavlink/mavlink_streams.c new file mode 100644 index 00000000000..6dfb0ef47b8 --- /dev/null +++ b/src/main/mavlink/mavlink_streams.c @@ -0,0 +1,1220 @@ +#include "mavlink/mavlink_internal.h" + +#include "mavlink/mavlink_modes.h" +#include "mavlink/mavlink_runtime.h" +#include "mavlink/mavlink_streams.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) + +/* Secondary profile for ports 2..N: heartbeat only. */ +const uint8_t mavSecondaryRates[MAVLINK_STREAM_COUNT] = { + [MAV_DATA_STREAM_EXTENDED_STATUS] = 0, + [MAV_DATA_STREAM_RC_CHANNELS] = 0, + [MAV_DATA_STREAM_POSITION] = 0, + [MAV_DATA_STREAM_EXTRA1] = 0, + [MAV_DATA_STREAM_EXTRA2] = 0, + [MAV_DATA_STREAM_EXTRA3] = 0, + [MAV_DATA_STREAM_EXTENDED_SYS_STATE] = 0, + [MAV_DATA_STREAM_HEARTBEAT] = 1 +}; + +uint8_t mavlinkClampStreamRate(uint8_t rate) +{ + if (rate > TELEMETRY_MAVLINK_MAXRATE) { + return TELEMETRY_MAVLINK_MAXRATE; + } + + return rate; +} + +int32_t mavlinkRateToIntervalUs(uint8_t rate) +{ + rate = mavlinkClampStreamRate(rate); + if (rate == 0) { + return -1; + } + + return 1000000 / rate; +} + +bool mavlinkPeriodicMessageFromMessageId(uint16_t messageId, mavlinkPeriodicMessage_e *periodicMessage) +{ + switch (messageId) { + case MAVLINK_MSG_ID_HEARTBEAT: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_HEARTBEAT; + return true; + case MAVLINK_MSG_ID_SYS_STATUS: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SYS_STATUS; + return true; + case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE; + return true; + case MAVLINK_MSG_ID_RC_CHANNELS: + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: + case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS; + return true; + case MAVLINK_MSG_ID_GPS_RAW_INT: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT; + return true; + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT; + return true; + case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN; + return true; + case MAVLINK_MSG_ID_ATTITUDE: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_ATTITUDE; + return true; + case MAVLINK_MSG_ID_VFR_HUD: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_VFR_HUD; + return true; + case MAVLINK_MSG_ID_BATTERY_STATUS: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS; + return true; + case MAVLINK_MSG_ID_SCALED_PRESSURE: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE; + return true; + case MAVLINK_MSG_ID_SYSTEM_TIME: + *periodicMessage = MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME; + return true; + default: + return false; + } +} + +uint8_t mavlinkPeriodicMessageBaseStream(mavlinkPeriodicMessage_e periodicMessage) +{ + switch (periodicMessage) { + case MAVLINK_PERIODIC_MESSAGE_HEARTBEAT: + return MAV_DATA_STREAM_HEARTBEAT; + case MAVLINK_PERIODIC_MESSAGE_SYS_STATUS: + return MAV_DATA_STREAM_EXTENDED_STATUS; + case MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE: + return MAV_DATA_STREAM_EXTENDED_SYS_STATE; + case MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS: + return MAV_DATA_STREAM_RC_CHANNELS; + case MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT: + case MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT: + case MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN: + return MAV_DATA_STREAM_POSITION; + case MAVLINK_PERIODIC_MESSAGE_ATTITUDE: + return MAV_DATA_STREAM_EXTRA1; + case MAVLINK_PERIODIC_MESSAGE_VFR_HUD: + return MAV_DATA_STREAM_EXTRA2; + case MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS: + case MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE: + case MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME: + return MAV_DATA_STREAM_EXTRA3; + default: + return MAV_DATA_STREAM_ALL; + } +} + +static void mavlinkResetMessageSchedule(mavlinkPeriodicMessage_e periodicMessage) +{ + if (!mavActivePort) { + return; + } + + mavActivePort->mavMessageNextDue[periodicMessage] = 0; +} + +static void mavlinkResetMessagesForStream(uint8_t streamNum) +{ + if (!mavActivePort) { + return; + } + + for (uint8_t messageIndex = 0; messageIndex < MAVLINK_PERIODIC_MESSAGE_COUNT; messageIndex++) { + if (mavlinkPeriodicMessageBaseStream((mavlinkPeriodicMessage_e)messageIndex) == streamNum) { + mavActivePort->mavMessageNextDue[messageIndex] = 0; + } + } +} + +int32_t mavlinkMessageBaseIntervalUs(mavlinkPeriodicMessage_e periodicMessage) +{ + if (!mavActivePort) { + return -1; + } + + return mavlinkRateToIntervalUs(mavActivePort->mavRates[mavlinkPeriodicMessageBaseStream(periodicMessage)]); +} + +int32_t mavlinkMessageIntervalUs(mavlinkPeriodicMessage_e periodicMessage) +{ + if (!mavActivePort) { + return -1; + } + + const int32_t overrideIntervalUs = mavActivePort->mavMessageOverrideIntervalsUs[periodicMessage]; + if (overrideIntervalUs != 0) { + return overrideIntervalUs; + } + + return mavlinkMessageBaseIntervalUs(periodicMessage); +} + +void mavlinkSetMessageOverrideIntervalUs(mavlinkPeriodicMessage_e periodicMessage, int32_t intervalUs) +{ + if (!mavActivePort) { + return; + } + + mavActivePort->mavMessageOverrideIntervalsUs[periodicMessage] = intervalUs; + mavlinkResetMessageSchedule(periodicMessage); +} + +int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum, timeUs_t currentTimeUs) +{ + if (!mavActivePort || streamNum >= MAXSTREAMS) { + return 0; + } + + const uint8_t rate = mavlinkClampStreamRate(mavActivePort->mavRates[streamNum]); + if (rate == 0) { + return 0; + } + + const timeUs_t intervalUs = 1000000UL / rate; + if ((mavActivePort->mavStreamNextDue[streamNum] == 0) || (cmpTimeUs(currentTimeUs, mavActivePort->mavStreamNextDue[streamNum]) >= 0)) { + mavActivePort->mavStreamNextDue[streamNum] = currentTimeUs + intervalUs; + return 1; + } + + return 0; +} + +void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate) +{ + if (!mavActivePort || streamNum >= MAXSTREAMS) { + return; + } + mavActivePort->mavRates[streamNum] = mavlinkClampStreamRate(rate); + mavActivePort->mavStreamNextDue[streamNum] = 0; + mavlinkResetMessagesForStream(streamNum); +} + +int mavlinkMessageTrigger(mavlinkPeriodicMessage_e periodicMessage, timeUs_t currentTimeUs) +{ + if (!mavActivePort) { + return 0; + } + + const int32_t intervalUs = mavlinkMessageIntervalUs(periodicMessage); + if (intervalUs <= 0) { + return 0; + } + + if ((mavActivePort->mavMessageNextDue[periodicMessage] == 0) || (cmpTimeUs(currentTimeUs, mavActivePort->mavMessageNextDue[periodicMessage]) >= 0)) { + mavActivePort->mavMessageNextDue[periodicMessage] = currentTimeUs + intervalUs; + return 1; + } + + return 0; +} + +void configureMAVLinkStreamRates(uint8_t portIndex) +{ + const mavlinkTelemetryPortConfig_t *primaryConfig = &telemetryConfig()->mavlink[0]; + const uint8_t mavPrimaryRates[MAVLINK_STREAM_COUNT] = { + [MAV_DATA_STREAM_EXTENDED_STATUS] = primaryConfig->extended_status_rate, + [MAV_DATA_STREAM_RC_CHANNELS] = primaryConfig->rc_channels_rate, + [MAV_DATA_STREAM_POSITION] = primaryConfig->position_rate, + [MAV_DATA_STREAM_EXTRA1] = primaryConfig->extra1_rate, + [MAV_DATA_STREAM_EXTRA2] = primaryConfig->extra2_rate, + [MAV_DATA_STREAM_EXTRA3] = primaryConfig->extra3_rate, + [MAV_DATA_STREAM_EXTENDED_SYS_STATE] = primaryConfig->extra3_rate, + [MAV_DATA_STREAM_HEARTBEAT] = 1 + }; + + const uint8_t *selectedRates = (portIndex == 0) ? mavPrimaryRates : mavSecondaryRates; + mavlinkPortRuntime_t *state = &mavPortStates[portIndex]; + + for (uint8_t stream = 0; stream < MAVLINK_STREAM_COUNT; stream++) { + state->mavRates[stream] = selectedRates[stream]; + state->mavRatesConfigured[stream] = selectedRates[stream]; + state->mavStreamNextDue[stream] = 0; + } +} + +void mavlinkSendAutopilotVersion(void) +{ + if (mavlinkGetProtocolVersion() == 1) { + return; + } + + // Capabilities aligned with what we actually support. + uint64_t capabilities = 0; + capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; + capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; + capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT; + capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT; + capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED; + capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT; + + const uint32_t flightSwVersion = + ((uint32_t)ARDUPILOT_VERSION_MAJOR << 24) | + ((uint32_t)ARDUPILOT_VERSION_MINOR << 16) | + ((uint32_t)ARDUPILOT_VERSION_PATCH << 8); + + // Bare minimum: caps + IDs. Everything else 0 is fine. + mavlink_msg_autopilot_version_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + capabilities, + flightSwVersion, + 0, + 0, + 0, + 0ULL, + 0ULL, + 0ULL, + 0ULL, + 0ULL, + (uint64_t)mavSystemId, + 0ULL + ); + mavlinkSendMessage(); +} + +void mavlinkSendProtocolVersion(void) +{ + if (mavlinkGetProtocolVersion() == 1) { + return; + } + + uint8_t specHash[8] = {0}; + uint8_t libHash[8] = {0}; + const uint16_t protocolVersion = (uint16_t)mavlinkGetProtocolVersion() * 100; + + mavlink_msg_protocol_version_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + protocolVersion, + protocolVersion, + protocolVersion, + specHash, + libHash); + + mavlinkSendMessage(); +} + +void mavlinkSendExtendedSysState(void) +{ + uint8_t vtolState = MAV_VTOL_STATE_UNDEFINED; + uint8_t landedState; + + switch (NAV_Status.state) { + case MW_NAV_STATE_LAND_START: + case MW_NAV_STATE_LAND_IN_PROGRESS: + case MW_NAV_STATE_LAND_SETTLE: + case MW_NAV_STATE_LAND_START_DESCENT: + case MW_NAV_STATE_EMERGENCY_LANDING: + landedState = MAV_LANDED_STATE_LANDING; + break; + case MW_NAV_STATE_LANDED: + landedState = MAV_LANDED_STATE_ON_GROUND; + break; + default: + if (!ARMING_FLAG(ARMED) || STATE(LANDING_DETECTED)) { + landedState = MAV_LANDED_STATE_ON_GROUND; + } else { + landedState = MAV_LANDED_STATE_IN_AIR; + } + break; + } + + mavlink_msg_extended_sys_state_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + vtolState, + landedState); + + mavlinkSendMessage(); +} + +void mavlinkSendSystemStatus(void) +{ + // Receiver is assumed to be always present + uint32_t onboard_control_sensors_present = (MAV_SYS_STATUS_SENSOR_RC_RECEIVER); + // GYRO and RC are assumed as minimum requirements + uint32_t onboard_control_sensors_enabled = (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_RC_RECEIVER); + uint32_t onboard_control_sensors_health = 0; + + if (getHwGyroStatus() == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO; + // Missing presence will report as sensor unhealthy + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO; + } + + hardwareSensorStatus_e accStatus = getHwAccelerometerStatus(); + if (accStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } else if (accStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } else if (accStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } + + hardwareSensorStatus_e compassStatus = getHwCompassStatus(); + if (compassStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } else if (compassStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } else if (compassStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } + + hardwareSensorStatus_e baroStatus = getHwBarometerStatus(); + if (baroStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } else if (baroStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } else if (baroStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } + + hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus(); + if (pitotStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } else if (pitotStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } else if (pitotStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } + + hardwareSensorStatus_e gpsStatus = getHwGPSStatus(); + if (gpsStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; + } else if (gpsStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; + } else if (gpsStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; + } + + hardwareSensorStatus_e opFlowStatus = getHwOpticalFlowStatus(); + if (opFlowStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } else if (opFlowStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } else if (opFlowStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } + + hardwareSensorStatus_e rangefinderStatus = getHwRangefinderStatus(); + if (rangefinderStatus == HW_SENSOR_OK) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } else if (rangefinderStatus == HW_SENSOR_UNHEALTHY) { + onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } else if (rangefinderStatus == HW_SENSOR_UNAVAILABLE) { + onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + + if (rxIsReceivingSignal() && rxAreFlightChannelsValid()) { + onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + } + +#ifdef USE_BLACKBOX + // BLACKBOX is assumed enabled and present for boards with capability + onboard_control_sensors_present |= MAV_SYS_STATUS_LOGGING; + onboard_control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; + // Unhealthy only for cases with not enough space to record + if (!isBlackboxDeviceFull()) { + onboard_control_sensors_health |= MAV_SYS_STATUS_LOGGING; + } +#endif + + mavlink_msg_sys_status_pack(mavSystemId, mavComponentId, &mavSendMsg, + onboard_control_sensors_present, + onboard_control_sensors_enabled, + onboard_control_sensors_health, + constrain(averageSystemLoadPercent * 10, 0, 1000), + feature(FEATURE_VBAT) ? getBatteryVoltage() * 10 : 0, + isAmperageConfigured() ? getAmperage() : -1, + feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100, + 0, + 0, + 0, + 0, + 0, + 0, 0, 0, 0); + + mavlinkSendMessage(); +} + +void mavlinkSendRCChannelsAndRSSI(void) +{ +#define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) + if (mavlinkGetProtocolVersion() == 1) { + mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, + millis(), + 0, + GET_CHANNEL_VALUE(0), + GET_CHANNEL_VALUE(1), + GET_CHANNEL_VALUE(2), + GET_CHANNEL_VALUE(3), + GET_CHANNEL_VALUE(4), + GET_CHANNEL_VALUE(5), + GET_CHANNEL_VALUE(6), + GET_CHANNEL_VALUE(7), + scaleRange(getRSSI(), 0, 1023, 0, 254)); + } + else { + mavlink_msg_rc_channels_pack(mavSystemId, mavComponentId, &mavSendMsg, + millis(), + rxRuntimeConfig.channelCount, + GET_CHANNEL_VALUE(0), + GET_CHANNEL_VALUE(1), + GET_CHANNEL_VALUE(2), + GET_CHANNEL_VALUE(3), + GET_CHANNEL_VALUE(4), + GET_CHANNEL_VALUE(5), + GET_CHANNEL_VALUE(6), + GET_CHANNEL_VALUE(7), + GET_CHANNEL_VALUE(8), + GET_CHANNEL_VALUE(9), + GET_CHANNEL_VALUE(10), + GET_CHANNEL_VALUE(11), + GET_CHANNEL_VALUE(12), + GET_CHANNEL_VALUE(13), + GET_CHANNEL_VALUE(14), + GET_CHANNEL_VALUE(15), + GET_CHANNEL_VALUE(16), + GET_CHANNEL_VALUE(17), + scaleRange(getRSSI(), 0, 1023, 0, 254)); + } +#undef GET_CHANNEL_VALUE + + mavlinkSendMessage(); +} + +#if defined(USE_GPS) +static void mavlinkSendHomePosition(void) +{ + float q[4] = { 1.0f, 0.0f, 0.0f, 0.0f }; + + mavlink_msg_home_position_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + GPS_home.lat, + GPS_home.lon, + GPS_home.alt * 10, + 0.0f, + 0.0f, + 0.0f, + q, + 0.0f, + 0.0f, + 0.0f, + ((uint64_t) millis()) * 1000); + + mavlinkSendMessage(); +} + +void mavlinkSendGpsRawInt(timeUs_t currentTimeUs) +{ + uint8_t gpsFixType = 0; + + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) { + return; + } + + if (gpsSol.fixType == GPS_NO_FIX) { + gpsFixType = 1; + } else if (gpsSol.fixType == GPS_FIX_2D) { + gpsFixType = 2; + } else if (gpsSol.fixType == GPS_FIX_3D) { + gpsFixType = 3; + } + + mavlink_msg_gps_raw_int_pack(mavSystemId, mavComponentId, &mavSendMsg, + currentTimeUs, + gpsFixType, + gpsSol.llh.lat, + gpsSol.llh.lon, + gpsSol.llh.alt * 10, + gpsSol.eph, + gpsSol.epv, + gpsSol.groundSpeed, + gpsSol.groundCourse * 10, + gpsSol.numSat, + 0, + gpsSol.eph * 10, + gpsSol.epv * 10, + 0, + 0, + 0); + + mavlinkSendMessage(); +} + +void mavlinkSendGlobalPositionInt(timeUs_t currentTimeUs) +{ + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) { + return; + } + + mavlink_msg_global_position_int_pack(mavSystemId, mavComponentId, &mavSendMsg, + currentTimeUs / 1000, + gpsSol.llh.lat, + gpsSol.llh.lon, + gpsSol.llh.alt * 10, + getEstimatedActualPosition(Z) * 10, + getEstimatedActualVelocity(X), + getEstimatedActualVelocity(Y), + -getEstimatedActualVelocity(Z), + DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw) + ); + + mavlinkSendMessage(); +} + +void mavlinkSendGpsGlobalOrigin(void) +{ + mavlink_msg_gps_global_origin_pack(mavSystemId, mavComponentId, &mavSendMsg, + GPS_home.lat, + GPS_home.lon, + GPS_home.alt * 10, + ((uint64_t) millis()) * 1000); + + mavlinkSendMessage(); +} + +void mavlinkSendPosition(timeUs_t currentTimeUs) +{ + mavlinkSendGpsRawInt(currentTimeUs); + mavlinkSendGlobalPositionInt(currentTimeUs); + mavlinkSendGpsGlobalOrigin(); +} +#endif + +void mavlinkSendAttitude(void) +{ + mavlink_msg_attitude_pack(mavSystemId, mavComponentId, &mavSendMsg, + millis(), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.roll)), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(-attitude.values.pitch)), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.yaw)), + DEGREES_TO_RADIANS(gyro.gyroADCf[FD_ROLL]), + DEGREES_TO_RADIANS(gyro.gyroADCf[FD_PITCH]), + DEGREES_TO_RADIANS(gyro.gyroADCf[FD_YAW])); + + mavlinkSendMessage(); +} + +void mavlinkSendVfrHud(void) +{ + float mavAltitude = 0; + float mavGroundSpeed = 0; + float mavAirSpeed = 0; + float mavClimbRate = 0; + +#if defined(USE_GPS) + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { + mavGroundSpeed = gpsSol.groundSpeed / 100.0f; + } +#endif + +#if defined(USE_PITOT) + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + mavAirSpeed = getAirspeedEstimate() / 100.0f; + } +#endif + + mavAltitude = getEstimatedActualPosition(Z) / 100.0f; + mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; + + int16_t thr = getThrottlePercent(osdUsingScaledThrottle()); + mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavAirSpeed, + mavGroundSpeed, + DECIDEGREES_TO_DEGREES(attitude.values.yaw), + thr, + mavAltitude, + mavClimbRate); + + mavlinkSendMessage(); +} + +void mavlinkSendHeartbeat(void) +{ + uint8_t mavModes = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + const bool isPlane = mavlinkIsFixedWingVehicle(); + const mavlinkModeSelection_t modeSelection = mavlinkSelectMode(); + flightModeForTelemetry_e flm = modeSelection.flightMode; + uint8_t mavCustomMode = modeSelection.customMode; + uint8_t mavSystemType; + if (isPlane) { + mavSystemType = MAV_TYPE_FIXED_WING; + } + else { + mavSystemType = mavlinkGetVehicleType(); + } + + const bool manualInputAllowed = !(flm == FLM_MISSION || flm == FLM_RTH || flm == FLM_FAILSAFE); + if (manualInputAllowed) { + mavModes |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } + if (flm == FLM_POSITION_HOLD && isGCSValid()) { + mavModes |= MAV_MODE_FLAG_GUIDED_ENABLED; + } + else if (flm == FLM_MISSION || flm == FLM_RTH ) { + mavModes |= MAV_MODE_FLAG_AUTO_ENABLED; + } + else if (flm != FLM_MANUAL && flm != FLM_ACRO && flm != FLM_ACRO_AIR) { + mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + + if (ARMING_FLAG(ARMED)) { + mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + uint8_t mavSystemState = 0; + if (ARMING_FLAG(ARMED)) { + if (failsafeIsActive()) { + mavSystemState = MAV_STATE_CRITICAL; + } + else { + mavSystemState = MAV_STATE_ACTIVE; + } + } + else if (areSensorsCalibrating()) { + mavSystemState = MAV_STATE_CALIBRATING; + } + else { + mavSystemState = MAV_STATE_STANDBY; + } + + mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavSystemType, + mavlinkGetAutopilotEnum(), + mavModes, + mavCustomMode, + mavSystemState); + + mavlinkSendMessage(); +} + +void mavlinkSendBatteryStatus(void) +{ + uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; + uint16_t batteryVoltagesExt[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; + memset(batteryVoltages, UINT16_MAX, sizeof(batteryVoltages)); + memset(batteryVoltagesExt, 0, sizeof(batteryVoltagesExt)); + if (feature(FEATURE_VBAT)) { + uint8_t batteryCellCount = getBatteryCellCount(); + if (batteryCellCount > 0) { + for (int cell = 0; cell < batteryCellCount && cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN + MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; cell++) { + if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) { + batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; + } else { + batteryVoltagesExt[cell - MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN] = getBatteryAverageCellVoltage() * 10; + } + } + } + else { + batteryVoltages[0] = getBatteryVoltage() * 10; + } + } + else { + batteryVoltages[0] = 0; + } + + mavlink_msg_battery_status_pack(mavSystemId, mavComponentId, &mavSendMsg, + 0, + MAV_BATTERY_FUNCTION_UNKNOWN, + MAV_BATTERY_TYPE_UNKNOWN, + INT16_MAX, + batteryVoltages, + isAmperageConfigured() ? getAmperage() : -1, + isAmperageConfigured() ? getMAhDrawn() : -1, + isAmperageConfigured() ? getMWhDrawn() * 36 : -1, + feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1, + 0, + 0, + batteryVoltagesExt, + 0, + 0); + + mavlinkSendMessage(); +} + +void mavlinkSendScaledPressure(void) +{ + int16_t temperature; + sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); + mavlink_msg_scaled_pressure_pack(mavSystemId, mavComponentId, &mavSendMsg, + millis(), + 0, + 0, + temperature * 10, + 0); + + mavlinkSendMessage(); +} + +void mavlinkSendSystemTime(void) +{ + mavlink_msg_system_time_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + 0, + millis()); + + mavlinkSendMessage(); +} + +bool mavlinkSendStatusText(void) +{ +#ifdef USE_OSD + char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "}; + textAttributes_t elemAttr = osdGetSystemMessage(buff, sizeof(buff), false); + if (buff[0] != SYM_BLANK) { + MAV_SEVERITY severity = MAV_SEVERITY_NOTICE; + if (TEXT_ATTRIBUTES_HAVE_BLINK(elemAttr)) { + severity = MAV_SEVERITY_CRITICAL; + } else if TEXT_ATTRIBUTES_HAVE_INVERTED(elemAttr) { + severity = MAV_SEVERITY_WARNING; + } + + mavlink_msg_statustext_pack(mavSystemId, mavComponentId, &mavSendMsg, + (uint8_t)severity, + buff, + 0, + 0); + + mavlinkSendMessage(); + return true; + } +#endif + return false; +} + +void mavlinkSendBatteryTemperatureStatusText(void) +{ + mavlinkSendBatteryStatus(); + mavlinkSendScaledPressure(); + mavlinkSendStatusText(); +} + +static uint8_t mavlinkHeadingDeg2FromCd(int32_t headingCd) +{ + return (uint8_t)(wrap_36000(headingCd) / 200); +} + +static uint16_t mavlinkComputeHighLatencyFailures(void) +{ + uint16_t flags = 0; + +#if defined(USE_GPS) + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) || gpsSol.fixType == GPS_NO_FIX) { + flags |= HL_FAILURE_FLAG_GPS; + } +#endif + +#ifdef USE_PITOT + if (getHwPitotmeterStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; + } +#endif + + if (getHwBarometerStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; + } + + if (getHwAccelerometerStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_ACCEL; + } + + if (getHwGyroStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_GYRO; + } + + if (getHwCompassStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_3D_MAG; + } + + if (getHwRangefinderStatus() != HW_SENSOR_OK) { + flags |= HL_FAILURE_FLAG_TERRAIN; + } + + const batteryState_e batteryState = getBatteryState(); + if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { + flags |= HL_FAILURE_FLAG_BATTERY; + } + + if (!rxIsReceivingSignal() || !rxAreFlightChannelsValid()) { + flags |= HL_FAILURE_FLAG_RC_RECEIVER; + } + + return flags; +} + +void mavlinkSendHighLatency2(timeUs_t currentTimeUs) +{ + const mavlinkModeSelection_t modeSelection = mavlinkSelectMode(); + + int32_t latitude = 0; + int32_t longitude = 0; + int16_t altitude = (int16_t)constrain((int32_t)(getEstimatedActualPosition(Z) / 100), INT16_MIN, INT16_MAX); + int16_t targetAltitude = (int16_t)constrain((int32_t)(posControl.desiredState.pos.z / 100), INT16_MIN, INT16_MAX); + uint16_t targetDistance = 0; + uint16_t wpNum = 0; + uint8_t heading = mavlinkHeadingDeg2FromCd(DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw)); + uint8_t targetHeading = mavlinkHeadingDeg2FromCd(posControl.desiredState.yaw); + uint8_t groundspeed = 0; + uint8_t airspeed = 0; + uint8_t airspeedSp = 0; + uint8_t windspeed = 0; + uint8_t windHeading = 0; + uint8_t eph = UINT8_MAX; + uint8_t epv = UINT8_MAX; + int8_t temperatureAir = 0; + int8_t climbRate = (int8_t)constrain(lrintf(getEstimatedActualVelocity(Z) / 10.0f), INT8_MIN, INT8_MAX); + int8_t battery = feature(FEATURE_VBAT) ? (int8_t)calculateBatteryPercentage() : -1; + +#if defined(USE_GPS) + if (sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + ) { + latitude = gpsSol.llh.lat; + longitude = gpsSol.llh.lon; + altitude = (int16_t)constrain((int32_t)(gpsSol.llh.alt / 100), INT16_MIN, INT16_MAX); + + const int32_t desiredAltCm = lrintf(posControl.desiredState.pos.z); + const int32_t targetAltCm = GPS_home.alt + desiredAltCm; + targetAltitude = (int16_t)constrain(targetAltCm / 100, INT16_MIN, INT16_MAX); + + groundspeed = (uint8_t)constrain(lrintf(gpsSol.groundSpeed / 20.0f), 0, UINT8_MAX); + + if (gpsSol.flags.validEPE) { + eph = (uint8_t)constrain((gpsSol.eph + 5) / 10, 0, UINT8_MAX); + epv = (uint8_t)constrain((gpsSol.epv + 5) / 10, 0, UINT8_MAX); + } + + if (posControl.activeWaypointIndex >= 0) { + wpNum = (uint16_t)posControl.activeWaypointIndex; + targetDistance = (uint16_t)constrain(lrintf(posControl.wpDistance / 1000.0f), 0, UINT16_MAX); + } + } +#endif + +#if defined(USE_PITOT) + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + airspeed = (uint8_t)constrain(lrintf(getAirspeedEstimate() / 20.0f), 0, UINT8_MAX); + airspeedSp = airspeed; + } +#endif + + if (airspeedSp == 0) { + const float desiredVelXY = calc_length_pythagorean_2D(posControl.desiredState.vel.x, posControl.desiredState.vel.y); + airspeedSp = (uint8_t)constrain(lrintf(desiredVelXY / 20.0f), 0, UINT8_MAX); + } + + if (airspeed == 0) { + airspeed = groundspeed; + } + +#ifdef USE_WIND_ESTIMATOR + if (isEstimatedWindSpeedValid()) { + uint16_t windAngleCd = 0; + const float windHoriz = getEstimatedHorizontalWindSpeed(&windAngleCd); + windspeed = (uint8_t)constrain(lrintf(windHoriz / 20.0f), 0, UINT8_MAX); + windHeading = mavlinkHeadingDeg2FromCd(windAngleCd); + } +#endif + + int16_t temperature; + sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); + temperatureAir = (int8_t)constrain(temperature, INT8_MIN, INT8_MAX); + + const uint16_t failureFlags = mavlinkComputeHighLatencyFailures(); + + mavlink_msg_high_latency2_pack( + mavSystemId, + mavComponentId, + &mavSendMsg, + (uint32_t)(currentTimeUs / 1000), + mavlinkGetVehicleType(), + mavlinkGetAutopilotEnum(), + modeSelection.customMode, + latitude, + longitude, + altitude, + targetAltitude, + heading, + targetHeading, + targetDistance, + (uint8_t)constrain(getThrottlePercent(osdUsingScaledThrottle()), 0, 100), + airspeed, + airspeedSp, + groundspeed, + windspeed, + windHeading, + eph, + epv, + temperatureAir, + climbRate, + battery, + wpNum, + failureFlags, + 0, + 0, + 0); + + mavlinkSendMessage(); +} + +bool mavlinkSendRequestedMessage(uint16_t messageId) +{ + switch (messageId) { + case MAVLINK_MSG_ID_HEARTBEAT: + mavlinkSendHeartbeat(); + return true; + case MAVLINK_MSG_ID_AUTOPILOT_VERSION: + if (mavlinkGetProtocolVersion() != 1) { + mavlinkSendAutopilotVersion(); + return true; + } + return false; + case MAVLINK_MSG_ID_PROTOCOL_VERSION: + if (mavlinkGetProtocolVersion() != 1) { + mavlinkSendProtocolVersion(); + return true; + } + return false; + case MAVLINK_MSG_ID_SYS_STATUS: + mavlinkSendSystemStatus(); + return true; + case MAVLINK_MSG_ID_ATTITUDE: + mavlinkSendAttitude(); + return true; + case MAVLINK_MSG_ID_VFR_HUD: + mavlinkSendVfrHud(); + return true; + case MAVLINK_MSG_ID_AVAILABLE_MODES: + mavlinkSendAvailableModesForCurrentMode(); + return true; + case MAVLINK_MSG_ID_CURRENT_MODE: + { + const mavlinkModeSelection_t modeSelection = mavlinkSelectMode(); + mavlinkSendCurrentMode(&modeSelection); + return true; + } + case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: + mavlinkSendExtendedSysState(); + return true; + case MAVLINK_MSG_ID_RC_CHANNELS: + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: + case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: + mavlinkSendRCChannelsAndRSSI(); + return true; +#ifdef USE_GPS + case MAVLINK_MSG_ID_GPS_RAW_INT: + mavlinkSendGpsRawInt(micros()); + return true; + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + mavlinkSendGlobalPositionInt(micros()); + return true; + case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN: + mavlinkSendGpsGlobalOrigin(); + return true; + case MAVLINK_MSG_ID_HOME_POSITION: + if (STATE(GPS_FIX_HOME)) { + mavlinkSendHomePosition(); + return true; + } + return false; +#endif + case MAVLINK_MSG_ID_BATTERY_STATUS: + mavlinkSendBatteryStatus(); + return true; + case MAVLINK_MSG_ID_SCALED_PRESSURE: + mavlinkSendScaledPressure(); + return true; + case MAVLINK_MSG_ID_SYSTEM_TIME: + mavlinkSendSystemTime(); + return true; + case MAVLINK_MSG_ID_STATUSTEXT: + return mavlinkSendStatusText(); + default: + return false; + } +} + +bool mavlinkHandleIncomingHeartbeat(void) +{ + mavlink_heartbeat_t msg; + mavlink_msg_heartbeat_decode(&mavlinkContext.recvMsg, &msg); + + switch (msg.type) { +#ifdef USE_ADSB + case MAV_TYPE_ADSB: + return adsbHeartbeat(); +#endif + default: + break; + } + + return false; +} + +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 mavlinkHandleIncomingRequestDataStream(void) +{ + mavlink_request_data_stream_t msg; + mavlink_msg_request_data_stream_decode(&mavlinkContext.recvMsg, &msg); + + if (!mavlinkIsLocalTarget(msg.target_system, msg.target_component)) { + return false; + } + + uint8_t rate = 0; + if (msg.start_stop != 0) { + rate = (uint8_t)msg.req_message_rate; + if (rate > TELEMETRY_MAVLINK_MAXRATE) { + rate = TELEMETRY_MAVLINK_MAXRATE; + } + } + + if (msg.req_stream_id == MAV_DATA_STREAM_ALL) { + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_STATUS, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_RC_CHANNELS, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_POSITION, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA1, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA2, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTRA3, rate); + mavlinkSetStreamRate(MAV_DATA_STREAM_EXTENDED_SYS_STATE, rate); + return true; + } + + mavlinkSetStreamRate(msg.req_stream_id, rate); + return true; +} + +void processMAVLinkTelemetry(timeUs_t currentTimeUs) +{ + if (mavActivePort->highLatencyEnabled && mavlinkGetProtocolVersion() != 1) { + if ((currentTimeUs - mavActivePort->lastHighLatencyMessageUs) >= TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US) { + mavlinkSendHighLatency2(currentTimeUs); + mavActivePort->lastHighLatencyMessageUs = currentTimeUs; + } + return; + } + + // is executed @ TELEMETRY_MAVLINK_MAXRATE rate + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SYS_STATUS, currentTimeUs)) { + mavlinkSendSystemStatus(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS, currentTimeUs)) { + mavlinkSendRCChannelsAndRSSI(); + } + +#ifdef USE_GPS + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT, currentTimeUs)) { + mavlinkSendGpsRawInt(currentTimeUs); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT, currentTimeUs)) { + mavlinkSendGlobalPositionInt(currentTimeUs); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN, currentTimeUs)) { + mavlinkSendGpsGlobalOrigin(); + } +#endif + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_ATTITUDE, currentTimeUs)) { + mavlinkSendAttitude(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_VFR_HUD, currentTimeUs)) { + mavlinkSendVfrHud(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_HEARTBEAT, currentTimeUs)) { + mavlinkSendHeartbeat(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE, currentTimeUs)) { + mavlinkSendExtendedSysState(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS, currentTimeUs)) { + mavlinkSendBatteryStatus(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE, currentTimeUs)) { + mavlinkSendScaledPressure(); + } + + if (mavlinkMessageTrigger(MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME, currentTimeUs)) { + mavlinkSendSystemTime(); + } + + if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3, currentTimeUs)) { + mavlinkSendStatusText(); + } +} + +#endif diff --git a/src/main/mavlink/mavlink_streams.h b/src/main/mavlink/mavlink_streams.h new file mode 100644 index 00000000000..c385ef35e9a --- /dev/null +++ b/src/main/mavlink/mavlink_streams.h @@ -0,0 +1,25 @@ +#pragma once + +#include "common/time.h" + +#include "mavlink/mavlink_types.h" + +extern const uint8_t mavSecondaryRates[MAVLINK_STREAM_COUNT]; + +void mavlinkSendAutopilotVersion(void); +void mavlinkSendProtocolVersion(void); +bool mavlinkSendRequestedMessage(uint16_t messageId); +uint8_t mavlinkClampStreamRate(uint8_t rate); +int32_t mavlinkRateToIntervalUs(uint8_t rate); +bool mavlinkPeriodicMessageFromMessageId(uint16_t messageId, mavlinkPeriodicMessage_e *periodicMessage); +uint8_t mavlinkPeriodicMessageBaseStream(mavlinkPeriodicMessage_e periodicMessage); +int32_t mavlinkMessageBaseIntervalUs(mavlinkPeriodicMessage_e periodicMessage); +int32_t mavlinkMessageIntervalUs(mavlinkPeriodicMessage_e periodicMessage); +void mavlinkSetMessageOverrideIntervalUs(mavlinkPeriodicMessage_e periodicMessage, int32_t intervalUs); +int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum, timeUs_t currentTimeUs); +void mavlinkSetStreamRate(uint8_t streamNum, uint8_t rate); +int mavlinkMessageTrigger(mavlinkPeriodicMessage_e periodicMessage, timeUs_t currentTimeUs); +void configureMAVLinkStreamRates(uint8_t portIndex); +void processMAVLinkTelemetry(timeUs_t currentTimeUs); +bool mavlinkHandleIncomingHeartbeat(void); +bool mavlinkHandleIncomingRequestDataStream(void); diff --git a/src/main/mavlink/mavlink_types.h b/src/main/mavlink/mavlink_types.h new file mode 100644 index 00000000000..70a2e5d1157 --- /dev/null +++ b/src/main/mavlink/mavlink_types.h @@ -0,0 +1,96 @@ +/* + * 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 . + */ + +#pragma once + +#include "common/time.h" + +#include "io/serial.h" + +#include "target/common.h" + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" +#ifndef MAVLINK_COMM_NUM_BUFFERS +#define MAVLINK_COMM_NUM_BUFFERS MAX_MAVLINK_PORTS +#endif +#include "common/mavlink.h" +#pragma GCC diagnostic pop + +#define TELEMETRY_MAVLINK_PORT_MODE MODE_RXTX +#define TELEMETRY_MAVLINK_MAXRATE 50 +#define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE) +#define TELEMETRY_MAVLINK_HIGH_LATENCY_INTERVAL_US (5 * 1000 * 1000) +#define MAV_DATA_STREAM_EXTENDED_SYS_STATE (MAV_DATA_STREAM_EXTRA3 + 1) +#define MAV_DATA_STREAM_HEARTBEAT (MAV_DATA_STREAM_EXTENDED_SYS_STATE + 1) +#define MAVLINK_STREAM_COUNT (MAV_DATA_STREAM_HEARTBEAT + 1) +#define ARDUPILOT_VERSION_MAJOR 4 +#define ARDUPILOT_VERSION_MINOR 6 +#define ARDUPILOT_VERSION_PATCH 3 +#define MAVLINK_MAX_ROUTES 32 +#define MAVLINK_PORT_MASK(portIndex) (1U << (portIndex)) +#define MAXSTREAMS MAVLINK_STREAM_COUNT + +typedef enum { + MAVLINK_PERIODIC_MESSAGE_HEARTBEAT, + MAVLINK_PERIODIC_MESSAGE_SYS_STATUS, + MAVLINK_PERIODIC_MESSAGE_EXTENDED_SYS_STATE, + MAVLINK_PERIODIC_MESSAGE_RC_CHANNELS, + MAVLINK_PERIODIC_MESSAGE_GPS_RAW_INT, + MAVLINK_PERIODIC_MESSAGE_GLOBAL_POSITION_INT, + MAVLINK_PERIODIC_MESSAGE_GPS_GLOBAL_ORIGIN, + MAVLINK_PERIODIC_MESSAGE_ATTITUDE, + MAVLINK_PERIODIC_MESSAGE_VFR_HUD, + MAVLINK_PERIODIC_MESSAGE_BATTERY_STATUS, + MAVLINK_PERIODIC_MESSAGE_SCALED_PRESSURE, + MAVLINK_PERIODIC_MESSAGE_SYSTEM_TIME, + MAVLINK_PERIODIC_MESSAGE_COUNT +} mavlinkPeriodicMessage_e; + +/** + * MAVLink requires angles to be in the range -Pi..Pi. + * This converts angles from a range of 0..Pi to -Pi..Pi + */ +#define RADIANS_TO_MAVLINK_RANGE(angle) (((angle) > M_PIf) ? ((angle) - (2 * M_PIf)) : (angle)) + +typedef struct mavlinkRouteEntry_s { + uint8_t sysid; + uint8_t compid; + uint8_t ingressPortIndex; +} mavlinkRouteEntry_t; + +typedef struct mavlinkPortRuntime_s { + serialPort_t *port; + const serialPortConfig_t *portConfig; + portSharing_e portSharing; + bool telemetryEnabled; + bool txbuffValid; + uint8_t txbuffFree; + timeUs_t lastMavlinkMessageUs; + timeUs_t lastRxFrameUs; + timeUs_t lastHighLatencyMessageUs; + bool highLatencyEnabled; + uint8_t mavRates[MAVLINK_STREAM_COUNT]; + uint8_t mavRatesConfigured[MAVLINK_STREAM_COUNT]; + timeUs_t mavStreamNextDue[MAVLINK_STREAM_COUNT]; + int32_t mavMessageOverrideIntervalsUs[MAVLINK_PERIODIC_MESSAGE_COUNT]; + timeUs_t mavMessageNextDue[MAVLINK_PERIODIC_MESSAGE_COUNT]; + uint8_t txSeq; + uint32_t txDroppedFrames; + mavlink_message_t mavRecvMsg; + mavlink_status_t mavRecvStatus; +} mavlinkPortRuntime_t; 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 a3752233149..33bd9ba4802 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1298,9 +1298,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP) || terrainFollowingToggled) { resetAltitudeController(navTerrainFollowingRequested()); setupAltitudeController(); - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset } + // POSHOLD is a 3D hold mode: always capture current altitude setpoint when entering. + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset + return NAV_FSM_EVENT_SUCCESS; } @@ -2073,6 +2075,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } +static void navMarkWaypointReached(int8_t waypointIndex); + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState) { UNUSED(previousState); @@ -2101,12 +2105,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga case NAV_WP_ACTION_HOLD_TIME: // Save the current time for the time the waypoint was reached posControl.wpReachedTime = millis(); + navMarkWaypointReached(posControl.activeWaypointIndex); return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME; } UNREACHABLE(); } +static void navMarkWaypointReached(int8_t waypointIndex) +{ + if (waypointIndex < posControl.startWpIndex) { + return; + } + + posControl.wpReachedSeq = (uint16_t)(waypointIndex - posControl.startWpIndex); + posControl.wpReachedNotificationPending = true; +} + static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState) { UNUSED(previousState); @@ -2124,6 +2139,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi if (posControl.wpAltitudeReached) { posControl.wpReachedTime = millis(); + navMarkWaypointReached(posControl.activeWaypointIndex); } else { return NAV_FSM_EVENT_NONE; } @@ -2163,6 +2179,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigatio { UNUSED(previousState); + if (!posControl.wpReachedNotificationPending) { + navMarkWaypointReached(posControl.activeWaypointIndex); + } + if (isLastMissionWaypoint()) { // Last waypoint reached return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; } @@ -3896,7 +3916,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; @@ -3951,6 +3971,7 @@ void resetWaypointList(void) posControl.waypointListValid = false; posControl.geoWaypointCount = 0; posControl.startWpIndex = 0; + posControl.wpReachedNotificationPending = false; #ifdef USE_MULTI_MISSION posControl.totalMultiMissionWpCount = 0; posControl.loadedMultiMissionIndex = 0; @@ -4013,6 +4034,7 @@ void loadSelectedMultiMission(uint8_t missionIndex) posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0; posControl.activeWaypointIndex = posControl.startWpIndex; + posControl.wpReachedNotificationPending = false; } bool updateWpMissionChange(void) @@ -5009,6 +5031,8 @@ void navigationInit(void) 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; @@ -5280,6 +5304,17 @@ const navigationPIDControllers_t* getNavigationPIDControllers(void) { return &posControl.pids; } +bool navigationConsumeWaypointReached(uint16_t *seq) +{ + if (!posControl.wpReachedNotificationPending) { + return false; + } + + *seq = posControl.wpReachedSeq; + posControl.wpReachedNotificationPending = false; + return true; +} + bool isAdjustingPosition(void) { return posControl.flags.isAdjustingPosition; } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 810fdc26ef8..572b8ff2cde 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -781,6 +781,7 @@ bool navigationIsFlyingAutonomousMode(void); bool navigationIsExecutingAnEmergencyLanding(void); bool navigationIsControllingAltitude(void); bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm); +bool navigationConsumeWaypointReached(uint16_t *seq); /* 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. */ diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index f6cf9329e98..b838e6e88ee 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -497,6 +497,8 @@ typedef struct { 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; #ifdef USE_FW_AUTOLAND /* Fixedwing autoland */ diff --git a/src/main/rx/mavlink.h b/src/main/rx/mavlink.h index 951b0eba105..c891039ddb8 100644 --- a/src/main/rx/mavlink.h +++ b/src/main/rx/mavlink.h @@ -17,9 +17,11 @@ #pragma once +#include "target/common.h" + #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" -#define MAVLINK_COMM_NUM_BUFFERS 1 +#define MAVLINK_COMM_NUM_BUFFERS MAX_MAVLINK_PORTS #include "common/mavlink.h" #pragma GCC diagnostic pop diff --git a/src/main/target/common.h b/src/main/target/common.h index af2170fe1b3..9f9cba8cfe8 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -31,6 +31,10 @@ #define DYNAMIC_HEAP_SIZE 2048 +#ifndef MAX_MAVLINK_PORTS +#define MAX_MAVLINK_PORTS 4 +#endif + #define I2C1_OVERCLOCK false #define I2C2_OVERCLOCK false #define USE_I2C_PULLUP // Enable built-in pullups on all boards in case external ones are too week @@ -226,4 +230,3 @@ #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER - diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 60f6aed67c3..fe019c6ec87 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -20,1439 +20,33 @@ * * Author: Konstantin Sharlaimov */ -#include -#include -#include -#include #include "platform.h" #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_MAVLINK) -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/axis.h" -#include "common/color.h" -#include "common/maths.h" -#include "common/utils.h" -#include "common/string_light.h" - -#include "config/feature.h" - -#include "drivers/serial.h" -#include "drivers/time.h" -#include "drivers/display.h" -#include "drivers/osd_symbols.h" - -#include "fc/config.h" -#include "fc/fc_core.h" -#include "fc/rc_controls.h" -#include "fc/rc_modes.h" -#include "fc/runtime_config.h" -#include "fc/settings.h" - -#include "flight/failsafe.h" -#include "flight/imu.h" -#include "flight/mixer_profile.h" -#include "flight/pid.h" -#include "flight/servos.h" - -#include "io/adsb.h" -#include "io/gps.h" -#include "io/ledstrip.h" -#include "io/serial.h" -#include "io/osd.h" - -#include "navigation/navigation.h" -#include "navigation/navigation_private.h" - -#include "rx/rx.h" -#include "rx/mavlink.h" - -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/battery.h" -#include "sensors/boardalignment.h" -#include "sensors/gyro.h" -#include "sensors/pitotmeter.h" -#include "sensors/diagnostics.h" -#include "sensors/sensors.h" -#include "sensors/temperature.h" -#include "sensors/esc_sensor.h" +#include "mavlink/mavlink_runtime.h" #include "telemetry/mavlink.h" -#include "telemetry/telemetry.h" - -#include "blackbox/blackbox_io.h" - -#include "scheduler/scheduler.h" - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-function" -#define MAVLINK_COMM_NUM_BUFFERS 1 -#include "common/mavlink.h" -#pragma GCC diagnostic pop - -#define TELEMETRY_MAVLINK_PORT_MODE MODE_RXTX -#define TELEMETRY_MAVLINK_MAXRATE 50 -#define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE) - -/** - * MAVLink requires angles to be in the range -Pi..Pi. - * This converts angles from a range of 0..Pi to -Pi..Pi - */ -#define RADIANS_TO_MAVLINK_RANGE(angle) (angle > M_PIf) ? angle - (2 * M_PIf) : angle - -/** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ -typedef enum APM_PLANE_MODE -{ - PLANE_MODE_MANUAL=0, - PLANE_MODE_CIRCLE=1, - PLANE_MODE_STABILIZE=2, - PLANE_MODE_TRAINING=3, - PLANE_MODE_ACRO=4, - PLANE_MODE_FLY_BY_WIRE_A=5, - PLANE_MODE_FLY_BY_WIRE_B=6, - PLANE_MODE_CRUISE=7, - PLANE_MODE_AUTOTUNE=8, - PLANE_MODE_AUTO=10, - PLANE_MODE_RTL=11, - PLANE_MODE_LOITER=12, - PLANE_MODE_TAKEOFF=13, - PLANE_MODE_AVOID_ADSB=14, - PLANE_MODE_GUIDED=15, - PLANE_MODE_INITIALIZING=16, - PLANE_MODE_QSTABILIZE=17, - PLANE_MODE_QHOVER=18, - PLANE_MODE_QLOITER=19, - PLANE_MODE_QLAND=20, - PLANE_MODE_QRTL=21, - PLANE_MODE_QAUTOTUNE=22, - PLANE_MODE_ENUM_END=23, -} APM_PLANE_MODE; - -/** @brief A mapping of copter flight modes for custom_mode field of heartbeat. */ -typedef enum APM_COPTER_MODE -{ - COPTER_MODE_STABILIZE=0, - COPTER_MODE_ACRO=1, - COPTER_MODE_ALT_HOLD=2, - COPTER_MODE_AUTO=3, - COPTER_MODE_GUIDED=4, - COPTER_MODE_LOITER=5, - COPTER_MODE_RTL=6, - COPTER_MODE_CIRCLE=7, - COPTER_MODE_LAND=9, - COPTER_MODE_DRIFT=11, - COPTER_MODE_SPORT=13, - COPTER_MODE_FLIP=14, - COPTER_MODE_AUTOTUNE=15, - COPTER_MODE_POSHOLD=16, - COPTER_MODE_BRAKE=17, - COPTER_MODE_THROW=18, - COPTER_MODE_AVOID_ADSB=19, - COPTER_MODE_GUIDED_NOGPS=20, - COPTER_MODE_SMART_RTL=21, - COPTER_MODE_ENUM_END=22, -} APM_COPTER_MODE; - -static serialPort_t *mavlinkPort = NULL; -static serialPortConfig_t *portConfig; - -static bool mavlinkTelemetryEnabled = false; -static portSharing_e mavlinkPortSharing; -static uint8_t txbuff_free = 100; -static bool txbuff_valid = false; - -/* MAVLink datastream rates in Hz */ -static uint8_t mavRates[] = { - [MAV_DATA_STREAM_EXTENDED_STATUS] = 2, // 2Hz - [MAV_DATA_STREAM_RC_CHANNELS] = 1, // 1Hz - [MAV_DATA_STREAM_POSITION] = 2, // 2Hz - [MAV_DATA_STREAM_EXTRA1] = 3, // 3Hz - [MAV_DATA_STREAM_EXTRA2] = 2, // 2Hz, HEARTBEATs are important - [MAV_DATA_STREAM_EXTRA3] = 1 // 1Hz -}; - -#define MAXSTREAMS (sizeof(mavRates) / sizeof(mavRates[0])) - -static timeUs_t lastMavlinkMessage = 0; -static uint8_t mavTicks[MAXSTREAMS]; -static mavlink_message_t mavSendMsg; -static mavlink_message_t mavRecvMsg; -static mavlink_status_t mavRecvStatus; - -// Set mavSystemId from telemetryConfig()->mavlink.sysid -static uint8_t mavSystemId = 1; -static uint8_t mavAutopilotType; -static uint8_t mavComponentId = MAV_COMP_ID_AUTOPILOT1; - -static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode) -{ - switch (flightMode) - { - case FLM_ACRO: return COPTER_MODE_ACRO; - case FLM_ACRO_AIR: return COPTER_MODE_ACRO; - case FLM_ANGLE: return COPTER_MODE_STABILIZE; - case FLM_HORIZON: return COPTER_MODE_STABILIZE; - case FLM_ANGLEHOLD: return COPTER_MODE_STABILIZE; - case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; - case FLM_POSITION_HOLD: - { - if (isGCSValid()) { - return COPTER_MODE_GUIDED; - } else { - return COPTER_MODE_POSHOLD; - } - } - case FLM_RTH: return COPTER_MODE_RTL; - case FLM_MISSION: return COPTER_MODE_AUTO; - case FLM_LAUNCH: return COPTER_MODE_THROW; - case FLM_FAILSAFE: - { - if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { - return COPTER_MODE_RTL; - } else if (failsafePhase() == FAILSAFE_LANDING) { - return COPTER_MODE_LAND; - } else { - // There is no valid mapping to ArduCopter - return COPTER_MODE_ENUM_END; - } - } - default: return COPTER_MODE_ENUM_END; - } -} - -static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode) -{ - switch (flightMode) - { - case FLM_MANUAL: return PLANE_MODE_MANUAL; - case FLM_ACRO: return PLANE_MODE_ACRO; - case FLM_ACRO_AIR: return PLANE_MODE_ACRO; - case FLM_ANGLE: return PLANE_MODE_FLY_BY_WIRE_A; - case FLM_HORIZON: return PLANE_MODE_STABILIZE; - case FLM_ANGLEHOLD: return PLANE_MODE_STABILIZE; - case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; - case FLM_POSITION_HOLD: - { - if (isGCSValid()) { - return PLANE_MODE_GUIDED; - } else { - return PLANE_MODE_LOITER; - } - } - case FLM_RTH: return PLANE_MODE_RTL; - case FLM_MISSION: return PLANE_MODE_AUTO; - case FLM_CRUISE: return PLANE_MODE_CRUISE; - case FLM_LAUNCH: return PLANE_MODE_TAKEOFF; - case FLM_FAILSAFE: - { - if (failsafePhase() == FAILSAFE_RETURN_TO_HOME) { - return PLANE_MODE_RTL; - } - else if (failsafePhase() == FAILSAFE_LANDING) { - return PLANE_MODE_AUTO; - } - else { - // There is no valid mapping to ArduPlane - return PLANE_MODE_ENUM_END; - } - } - default: return PLANE_MODE_ENUM_END; - } -} - -static int mavlinkStreamTrigger(enum MAV_DATA_STREAM streamNum) -{ - uint8_t rate = (uint8_t) mavRates[streamNum]; - if (rate == 0) { - return 0; - } - - if (mavTicks[streamNum] == 0) { - // we're triggering now, setup the next trigger point - if (rate > TELEMETRY_MAVLINK_MAXRATE) { - rate = TELEMETRY_MAVLINK_MAXRATE; - } - - mavTicks[streamNum] = (TELEMETRY_MAVLINK_MAXRATE / rate); - return 1; - } - - // count down at TASK_RATE_HZ - mavTicks[streamNum]--; - return 0; -} - -void freeMAVLinkTelemetryPort(void) -{ - closeSerialPort(mavlinkPort); - mavlinkPort = NULL; - mavlinkTelemetryEnabled = false; -} void initMAVLinkTelemetry(void) { - portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MAVLINK); - mavlinkPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_MAVLINK); -} - -void configureMAVLinkTelemetryPort(void) -{ - if (!portConfig) { - return; - } - - baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex; - if (baudRateIndex == BAUD_AUTO) { - // default rate for minimOSD - baudRateIndex = BAUD_57600; - } - - mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_PORT_MODE, SERIAL_NOT_INVERTED); - mavAutopilotType = telemetryConfig()->mavlink.autopilot_type; - mavSystemId = telemetryConfig()->mavlink.sysid; - - if (!mavlinkPort) { - return; - } - - mavlinkTelemetryEnabled = true; + mavlinkRuntimeInit(); } -static void configureMAVLinkStreamRates(void) +void handleMAVLinkTelemetry(timeUs_t currentTimeUs) { - mavRates[MAV_DATA_STREAM_EXTENDED_STATUS] = telemetryConfig()->mavlink.extended_status_rate; - mavRates[MAV_DATA_STREAM_RC_CHANNELS] = telemetryConfig()->mavlink.rc_channels_rate; - mavRates[MAV_DATA_STREAM_POSITION] = telemetryConfig()->mavlink.position_rate; - mavRates[MAV_DATA_STREAM_EXTRA1] = telemetryConfig()->mavlink.extra1_rate; - mavRates[MAV_DATA_STREAM_EXTRA2] = telemetryConfig()->mavlink.extra2_rate; - mavRates[MAV_DATA_STREAM_EXTRA3] = telemetryConfig()->mavlink.extra3_rate; + mavlinkRuntimeHandle(currentTimeUs); } void checkMAVLinkTelemetryState(void) { - bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing); - - if (newTelemetryEnabledValue == mavlinkTelemetryEnabled) { - return; - } - - if (newTelemetryEnabledValue) { - configureMAVLinkTelemetryPort(); - configureMAVLinkStreamRates(); - } else - freeMAVLinkTelemetryPort(); -} - -static void mavlinkSendMessage(void) -{ - uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; - - mavlink_status_t* chan_state = mavlink_get_channel_status(MAVLINK_COMM_0); - if (telemetryConfig()->mavlink.version == 1) { - chan_state->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - } else { - chan_state->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - } - - int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg); - - for (int i = 0; i < msgLength; i++) { - serialWrite(mavlinkPort, mavBuffer[i]); - } -} - -void mavlinkSendSystemStatus(void) -{ - // Receiver is assumed to be always present - uint32_t onboard_control_sensors_present = (MAV_SYS_STATUS_SENSOR_RC_RECEIVER); - // GYRO and RC are assumed as minimum requirements - uint32_t onboard_control_sensors_enabled = (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_RC_RECEIVER); - uint32_t onboard_control_sensors_health = 0; - - if (getHwGyroStatus() == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO; - // Missing presence will report as sensor unhealthy - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO; - } - - hardwareSensorStatus_e accStatus = getHwAccelerometerStatus(); - if (accStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - } else if (accStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - } else if (accStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL; - } - - hardwareSensorStatus_e compassStatus = getHwCompassStatus(); - if (compassStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } else if (compassStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } else if (compassStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } - - hardwareSensorStatus_e baroStatus = getHwBarometerStatus(); - if (baroStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } else if (baroStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } else if (baroStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } - - hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus(); - if (pitotStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } else if (pitotStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } else if (pitotStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } - - hardwareSensorStatus_e gpsStatus = getHwGPSStatus(); - if (gpsStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; - } else if (gpsStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; - } else if (gpsStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS; - } - - hardwareSensorStatus_e opFlowStatus = getHwOpticalFlowStatus(); - if (opFlowStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } else if (opFlowStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } else if (opFlowStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } - - hardwareSensorStatus_e rangefinderStatus = getHwRangefinderStatus(); - if (rangefinderStatus == HW_SENSOR_OK) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } else if (rangefinderStatus == HW_SENSOR_UNHEALTHY) { - onboard_control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } else if (rangefinderStatus == HW_SENSOR_UNAVAILABLE) { - onboard_control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - - if (rxIsReceivingSignal() && rxAreFlightChannelsValid()) { - onboard_control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } - -#ifdef USE_BLACKBOX - // BLACKBOX is assumed enabled and present for boards with capability - onboard_control_sensors_present |= MAV_SYS_STATUS_LOGGING; - onboard_control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; - // Unhealthy only for cases with not enough space to record - if (!isBlackboxDeviceFull()) { - onboard_control_sensors_health |= MAV_SYS_STATUS_LOGGING; - } -#endif - - mavlink_msg_sys_status_pack(mavSystemId, mavComponentId, &mavSendMsg, - // onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. - //Value of 0: not present. Value of 1: present. Indices according MAV_SYS_STATUS_SENSOR - onboard_control_sensors_present, - // onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled - onboard_control_sensors_enabled, - // onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error. - onboard_control_sensors_health, - // load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - constrain(averageSystemLoadPercent*10, 0, 1000), - // voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) - feature(FEATURE_VBAT) ? getBatteryVoltage() * 10 : 0, - // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - isAmperageConfigured() ? getAmperage() : -1, - // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery - feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100, - // drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - 0, - // errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - 0, - // errors_count1 Autopilot-specific errors - 0, - // errors_count2 Autopilot-specific errors - 0, - // errors_count3 Autopilot-specific errors - 0, - // errors_count4 Autopilot-specific errors - 0, 0, 0, 0); - - mavlinkSendMessage(); -} - -void mavlinkSendRCChannelsAndRSSI(void) -{ -#define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) - if (telemetryConfig()->mavlink.version == 1) { - mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - millis(), - // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - 0, - // chan1_raw RC channel 1 value, in microseconds - GET_CHANNEL_VALUE(0), - // chan2_raw RC channel 2 value, in microseconds - GET_CHANNEL_VALUE(1), - // chan3_raw RC channel 3 value, in microseconds - GET_CHANNEL_VALUE(2), - // chan4_raw RC channel 4 value, in microseconds - GET_CHANNEL_VALUE(3), - // chan5_raw RC channel 5 value, in microseconds - GET_CHANNEL_VALUE(4), - // chan6_raw RC channel 6 value, in microseconds - GET_CHANNEL_VALUE(5), - // chan7_raw RC channel 7 value, in microseconds - GET_CHANNEL_VALUE(6), - // chan8_raw RC channel 8 value, in microseconds - GET_CHANNEL_VALUE(7), - // rssi Receive signal strength indicator, 0: 0%, 254: 100% - //https://github.com/mavlink/mavlink/issues/1027 - scaleRange(getRSSI(), 0, 1023, 0, 254)); - } - else { - mavlink_msg_rc_channels_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - millis(), - // Total number of RC channels being received. - rxRuntimeConfig.channelCount, - // chan1_raw RC channel 1 value, in microseconds - GET_CHANNEL_VALUE(0), - // chan2_raw RC channel 2 value, in microseconds - GET_CHANNEL_VALUE(1), - // chan3_raw RC channel 3 value, in microseconds - GET_CHANNEL_VALUE(2), - // chan4_raw RC channel 4 value, in microseconds - GET_CHANNEL_VALUE(3), - // chan5_raw RC channel 5 value, in microseconds - GET_CHANNEL_VALUE(4), - // chan6_raw RC channel 6 value, in microseconds - GET_CHANNEL_VALUE(5), - // chan7_raw RC channel 7 value, in microseconds - GET_CHANNEL_VALUE(6), - // chan8_raw RC channel 8 value, in microseconds - GET_CHANNEL_VALUE(7), - // chan9_raw RC channel 9 value, in microseconds - GET_CHANNEL_VALUE(8), - // chan10_raw RC channel 10 value, in microseconds - GET_CHANNEL_VALUE(9), - // chan11_raw RC channel 11 value, in microseconds - GET_CHANNEL_VALUE(10), - // chan12_raw RC channel 12 value, in microseconds - GET_CHANNEL_VALUE(11), - // chan13_raw RC channel 13 value, in microseconds - GET_CHANNEL_VALUE(12), - // chan14_raw RC channel 14 value, in microseconds - GET_CHANNEL_VALUE(13), - // chan15_raw RC channel 15 value, in microseconds - GET_CHANNEL_VALUE(14), - // chan16_raw RC channel 16 value, in microseconds - GET_CHANNEL_VALUE(15), - // chan17_raw RC channel 17 value, in microseconds - GET_CHANNEL_VALUE(16), - // chan18_raw RC channel 18 value, in microseconds - GET_CHANNEL_VALUE(17), - // rssi Receive signal strength indicator, 0: 0%, 254: 100% - //https://github.com/mavlink/mavlink/issues/1027 - scaleRange(getRSSI(), 0, 1023, 0, 254)); - } -#undef GET_CHANNEL_VALUE - - mavlinkSendMessage(); -} - -#if defined(USE_GPS) -void mavlinkSendPosition(timeUs_t currentTimeUs) -{ - uint8_t gpsFixType = 0; - - if (!(sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - )) - return; - - if (gpsSol.fixType == GPS_NO_FIX) - gpsFixType = 1; - else if (gpsSol.fixType == GPS_FIX_2D) - gpsFixType = 2; - else if (gpsSol.fixType == GPS_FIX_3D) - gpsFixType = 3; - - mavlink_msg_gps_raw_int_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - currentTimeUs, - // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - gpsFixType, - // lat Latitude in 1E7 degrees - gpsSol.llh.lat, - // lon Longitude in 1E7 degrees - gpsSol.llh.lon, - // alt Altitude in 1E3 meters (millimeters) above MSL - gpsSol.llh.alt * 10, - // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - gpsSol.eph, - // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - gpsSol.epv, - // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - gpsSol.groundSpeed, - // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - gpsSol.groundCourse * 10, - // satellites_visible Number of satellites visible. If unknown, set to 255 - gpsSol.numSat, - // alt_ellipsoid Altitude (above WGS84, EGM96 ellipsoid). Positive for up - 0, - // h_acc Position uncertainty in mm, - gpsSol.eph * 10, - // v_acc Altitude uncertainty in mm, - gpsSol.epv * 10, - // vel_acc Speed uncertainty in mm (??) - 0, - // hdg_acc Heading uncertainty in degE5 - 0, - // yaw Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. - 0); - - mavlinkSendMessage(); - - // Global position - mavlink_msg_global_position_int_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - currentTimeUs, - // lat Latitude in 1E7 degrees - gpsSol.llh.lat, - // lon Longitude in 1E7 degrees - gpsSol.llh.lon, - // alt Altitude in 1E3 meters (millimeters) above MSL - gpsSol.llh.alt * 10, - // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) - getEstimatedActualPosition(Z) * 10, - // [cm/s] Ground X Speed (Latitude, positive north) - getEstimatedActualVelocity(X), - // [cm/s] Ground Y Speed (Longitude, positive east) - getEstimatedActualVelocity(Y), - // [cm/s] Ground Z Speed (Altitude, positive down) - getEstimatedActualVelocity(Z), - // [cdeg] Vehicle heading (yaw angle) (0.0..359.99 degrees, 0=north) - DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw) - ); - - mavlinkSendMessage(); - - mavlink_msg_gps_global_origin_pack(mavSystemId, mavComponentId, &mavSendMsg, - // latitude Latitude (WGS84), expressed as * 1E7 - GPS_home.lat, - // longitude Longitude (WGS84), expressed as * 1E7 - GPS_home.lon, - // altitude Altitude(WGS84), expressed as * 1000 - GPS_home.alt * 10, // FIXME - // time_usec Timestamp (microseconds since system boot) - // Use millis() * 1000 as micros() will overflow after 1.19 hours. - ((uint64_t) millis()) * 1000); - - mavlinkSendMessage(); -} -#endif - -void mavlinkSendAttitude(void) -{ - mavlink_msg_attitude_pack(mavSystemId, mavComponentId, &mavSendMsg, - // time_boot_ms Timestamp (milliseconds since system boot) - millis(), - // roll Roll angle (rad) - RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.roll)), - // pitch Pitch angle (rad) - RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(-attitude.values.pitch)), - // yaw Yaw angle (rad) - RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.yaw)), - // rollspeed Roll angular speed (rad/s) - gyro.gyroADCf[FD_ROLL], - // pitchspeed Pitch angular speed (rad/s) - gyro.gyroADCf[FD_PITCH], - // yawspeed Yaw angular speed (rad/s) - gyro.gyroADCf[FD_YAW]); - - mavlinkSendMessage(); -} - -void mavlinkSendHUDAndHeartbeat(void) -{ - float mavAltitude = 0; - float mavGroundSpeed = 0; - float mavAirSpeed = 0; - float mavClimbRate = 0; - -#if defined(USE_GPS) - // use ground speed if source available - if (sensors(SENSOR_GPS) -#ifdef USE_GPS_FIX_ESTIMATION - || STATE(GPS_ESTIMATED_FIX) -#endif - ) { - mavGroundSpeed = gpsSol.groundSpeed / 100.0f; - } -#endif - -#if defined(USE_PITOT) - if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { - mavAirSpeed = getAirspeedEstimate() / 100.0f; - } -#endif - - // select best source for altitude - mavAltitude = getEstimatedActualPosition(Z) / 100.0f; - mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; - - int16_t thr = getThrottlePercent(osdUsingScaledThrottle()); - mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg, - // airspeed Current airspeed in m/s - mavAirSpeed, - // groundspeed Current ground speed in m/s - mavGroundSpeed, - // heading Current heading in degrees, in compass units (0..360, 0=north) - DECIDEGREES_TO_DEGREES(attitude.values.yaw), - // throttle Current throttle setting in integer percent, 0 to 100 - thr, - // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate) - mavAltitude, - // climb Current climb rate in meters/second - mavClimbRate); - - mavlinkSendMessage(); - - - uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - if (ARMING_FLAG(ARMED)) - mavModes |= MAV_MODE_FLAG_SAFETY_ARMED; - - uint8_t mavSystemType; - switch (mixerConfig()->platformType) - { - case PLATFORM_MULTIROTOR: - mavSystemType = MAV_TYPE_QUADROTOR; - break; - case PLATFORM_TRICOPTER: - mavSystemType = MAV_TYPE_TRICOPTER; - break; - case PLATFORM_AIRPLANE: - mavSystemType = MAV_TYPE_FIXED_WING; - break; - case PLATFORM_ROVER: - mavSystemType = MAV_TYPE_GROUND_ROVER; - break; - case PLATFORM_BOAT: - mavSystemType = MAV_TYPE_SURFACE_BOAT; - break; - case PLATFORM_HELICOPTER: - mavSystemType = MAV_TYPE_HELICOPTER; - break; - default: - mavSystemType = MAV_TYPE_GENERIC; - break; - } - - flightModeForTelemetry_e flm = getFlightModeForTelemetry(); - uint8_t mavCustomMode; - - if (STATE(FIXED_WING_LEGACY)) { - mavCustomMode = (uint8_t)inavToArduPlaneMap(flm); - } - else { - mavCustomMode = (uint8_t)inavToArduCopterMap(flm); - } - - if (flm != FLM_MANUAL) { - mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - if (flm == FLM_POSITION_HOLD || flm == FLM_RTH || flm == FLM_MISSION) { - mavModes |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - - uint8_t mavSystemState = 0; - if (ARMING_FLAG(ARMED)) { - if (failsafeIsActive()) { - mavSystemState = MAV_STATE_CRITICAL; - } - else { - mavSystemState = MAV_STATE_ACTIVE; - } - } - else if (areSensorsCalibrating()) { - mavSystemState = MAV_STATE_CALIBRATING; - } - else { - mavSystemState = MAV_STATE_STANDBY; - } - - uint8_t mavType; - if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { - mavType = MAV_AUTOPILOT_ARDUPILOTMEGA; - } else { - mavType = MAV_AUTOPILOT_GENERIC; - } - - mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg, - // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - mavSystemType, - // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - mavType, - // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - mavModes, - // custom_mode A bitfield for use for autopilot-specific flags. - mavCustomMode, - // system_status System status flag, see MAV_STATE ENUM - mavSystemState); - - mavlinkSendMessage(); -} - -void mavlinkSendBatteryTemperatureStatusText(void) -{ - uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; - uint16_t batteryVoltagesExt[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; - memset(batteryVoltages, UINT16_MAX, sizeof(batteryVoltages)); - memset(batteryVoltagesExt, 0, sizeof(batteryVoltagesExt)); - if (feature(FEATURE_VBAT)) { - uint8_t batteryCellCount = getBatteryCellCount(); - if (batteryCellCount > 0) { - for (int cell=0; cell < batteryCellCount && cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN + MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; cell++) { - if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) { - batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; - } else { - batteryVoltagesExt[cell-MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN] = getBatteryAverageCellVoltage() * 10; - } - } - } - else { - batteryVoltages[0] = getBatteryVoltage() * 10; - } - } - else { - batteryVoltages[0] = 0; - } - - mavlink_msg_battery_status_pack(mavSystemId, mavComponentId, &mavSendMsg, - // id Battery ID - 0, - // battery_function Function of the battery - MAV_BATTERY_FUNCTION_UNKNOWN, - // type Type (chemistry) of the battery - MAV_BATTERY_TYPE_UNKNOWN, - // temperature Temperature of the battery in centi-degrees celsius. INT16_MAX for unknown temperature - INT16_MAX, - // voltages Battery voltage of cells, in millivolts (1 = 1 millivolt). Cells above the valid cell count for this battery should have the UINT16_MAX value. - batteryVoltages, - // current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - isAmperageConfigured() ? getAmperage() : -1, - // current_consumed Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate - isAmperageConfigured() ? getMAhDrawn() : -1, - // energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate - isAmperageConfigured() ? getMWhDrawn()*36 : -1, - // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery); - feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1, - // time_remaining Remaining battery time, 0: autopilot does not provide remaining battery time estimate - 0, // TODO this could easily be implemented - // charge_state State for extent of discharge, provided by autopilot for warning or external reactions - 0, - // voltages_ext Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. - batteryVoltagesExt, - // mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. - 0, - // fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). - 0); - - mavlinkSendMessage(); - - - int16_t temperature; - sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature); - mavlink_msg_scaled_pressure_pack(mavSystemId, mavComponentId, &mavSendMsg, - millis(), - 0, - 0, - temperature * 10, - 0); - - mavlinkSendMessage(); - - -// FIXME - Status text is limited to boards with USE_OSD -#ifdef USE_OSD - char buff[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = {" "}; - textAttributes_t elemAttr = osdGetSystemMessage(buff, sizeof(buff), false); - if (buff[0] != SYM_BLANK) { - MAV_SEVERITY severity = MAV_SEVERITY_NOTICE; - if (TEXT_ATTRIBUTES_HAVE_BLINK(elemAttr)) { - severity = MAV_SEVERITY_CRITICAL; - } else if TEXT_ATTRIBUTES_HAVE_INVERTED(elemAttr) { - severity = MAV_SEVERITY_WARNING; - } - - mavlink_msg_statustext_pack(mavSystemId, mavComponentId, &mavSendMsg, - (uint8_t)severity, - buff, - 0, - 0); - - mavlinkSendMessage(); - } -#endif - - -} - -void processMAVLinkTelemetry(timeUs_t currentTimeUs) -{ - // is executed @ TELEMETRY_MAVLINK_MAXRATE rate - if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTENDED_STATUS)) { - mavlinkSendSystemStatus(); - } - - if (mavlinkStreamTrigger(MAV_DATA_STREAM_RC_CHANNELS)) { - mavlinkSendRCChannelsAndRSSI(); - } - -#ifdef USE_GPS - if (mavlinkStreamTrigger(MAV_DATA_STREAM_POSITION)) { - mavlinkSendPosition(currentTimeUs); - } -#endif - - if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA1)) { - mavlinkSendAttitude(); - } - - if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA2)) { - mavlinkSendHUDAndHeartbeat(); - } - - if (mavlinkStreamTrigger(MAV_DATA_STREAM_EXTRA3)) { - mavlinkSendBatteryTemperatureStatusText(); - } - -} - -static bool handleIncoming_MISSION_CLEAR_ALL(void) -{ - mavlink_mission_clear_all_t msg; - mavlink_msg_mission_clear_all_decode(&mavRecvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - resetWaypointList(); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - return false; -} - -// Static state for MISSION UPLOAD transaction (starting with MISSION_COUNT) -static int incomingMissionWpCount = 0; -static int incomingMissionWpSequence = 0; - -static bool handleIncoming_MISSION_COUNT(void) -{ - mavlink_mission_count_t msg; - mavlink_msg_mission_count_decode(&mavRecvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - if (msg.count <= NAV_MAX_WAYPOINTS) { - incomingMissionWpCount = msg.count; // We need to know how many items to request - incomingMissionWpSequence = 0; - mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); - mavlinkSendMessage(); - return true; - } - else if (ARMING_FLAG(ARMED)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - } - - return false; -} - -static bool handleIncoming_MISSION_ITEM(void) -{ - mavlink_mission_item_t msg; - mavlink_msg_mission_item_decode(&mavRecvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - // Check supported values first - if (ARMING_FLAG(ARMED)) { - // Legacy Mission Planner BS for GUIDED - if (isGCSValid() && (msg.command == MAV_CMD_NAV_WAYPOINT) && (msg.current == 2)) { - if (!(msg.frame == MAV_FRAME_GLOBAL)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavRecvMsg.sysid, mavRecvMsg.compid, - MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - navWaypoint_t wp; - wp.action = NAV_WP_ACTION_WAYPOINT; - wp.lat = (int32_t)(msg.x * 1e7f); - wp.lon = (int32_t)(msg.y * 1e7f); - wp.alt = (int32_t)(msg.z * 100.0f); - wp.p1 = 0; - wp.p2 = 0; - wp.p3 = 0; - setWaypoint(255, &wp); - - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - mavRecvMsg.sysid, mavRecvMsg.compid, - MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - } - - if ((msg.autocontinue == 0) || (msg.command != MAV_CMD_NAV_WAYPOINT && msg.command != MAV_CMD_NAV_RETURN_TO_LAUNCH)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - if ((msg.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) && !(msg.frame == MAV_FRAME_MISSION && msg.command == MAV_CMD_NAV_RETURN_TO_LAUNCH)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - if (msg.seq == incomingMissionWpSequence) { - incomingMissionWpSequence++; - - navWaypoint_t wp; - wp.action = (msg.command == MAV_CMD_NAV_RETURN_TO_LAUNCH) ? NAV_WP_ACTION_RTH : NAV_WP_ACTION_WAYPOINT; - wp.lat = (int32_t)(msg.x * 1e7f); - wp.lon = (int32_t)(msg.y * 1e7f); - wp.alt = msg.z * 100.0f; - wp.p1 = 0; - wp.p2 = 0; - wp.p3 = 0; - wp.flag = (incomingMissionWpSequence >= incomingMissionWpCount) ? NAV_WP_FLAG_LAST : 0; - - setWaypoint(incomingMissionWpSequence, &wp); - - if (incomingMissionWpSequence >= incomingMissionWpCount) { - if (isWaypointListValid()) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - } - else { - mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); - mavlinkSendMessage(); - } - } - else { - // Wrong sequence number received - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - - return true; - } - - return false; -} - -static bool handleIncoming_MISSION_REQUEST_LIST(void) -{ - mavlink_mission_request_list_t msg; - mavlink_msg_mission_request_list_decode(&mavRecvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - return true; - } - - return false; + mavlinkRuntimeCheckState(); } -static bool handleIncoming_MISSION_REQUEST(void) -{ - mavlink_mission_request_t msg; - mavlink_msg_mission_request_decode(&mavRecvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - int wpCount = getWaypointCount(); - - if (msg.seq < wpCount) { - navWaypoint_t wp; - getWaypoint(msg.seq + 1, &wp); - - mavlink_msg_mission_item_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, - msg.seq, - wp.action == NAV_WP_ACTION_RTH ? MAV_FRAME_MISSION : MAV_FRAME_GLOBAL_RELATIVE_ALT, - wp.action == NAV_WP_ACTION_RTH ? MAV_CMD_NAV_RETURN_TO_LAUNCH : MAV_CMD_NAV_WAYPOINT, - 0, - 1, - 0, 0, 0, 0, - wp.lat / 1e7f, - wp.lon / 1e7f, - wp.alt / 100.0f, - MAV_MISSION_TYPE_MISSION); - mavlinkSendMessage(); - } - else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); - mavlinkSendMessage(); - } - - return true; - } - - return false; -} - -static bool handleIncoming_COMMAND_INT(void) -{ - mavlink_command_int_t msg; - mavlink_msg_command_int_decode(&mavRecvMsg, &msg); - - if (msg.target_system == mavSystemId) { - - if (msg.command == MAV_CMD_DO_REPOSITION) { - - if (!(msg.frame == MAV_FRAME_GLOBAL)) { //|| msg.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || msg.frame == MAV_FRAME_GLOBAL_TERRAIN_ALT)) { - - mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - msg.command, - MAV_RESULT_UNSUPPORTED, - 0, // progress - 0, // result_param2 - mavRecvMsg.sysid, - mavRecvMsg.compid); - mavlinkSendMessage(); - return true; - } - - if (isGCSValid()) { - navWaypoint_t wp; - wp.action = NAV_WP_ACTION_WAYPOINT; - wp.lat = (int32_t)msg.x; - wp.lon = (int32_t)msg.y; - wp.alt = msg.z * 100.0f; - if (!isnan(msg.param4) && msg.param4 >= 0.0f && msg.param4 < 360.0f) { - wp.p1 = (int16_t)msg.param4; - } else { - wp.p1 = 0; - } - wp.p2 = 0; // TODO: Alt modes - wp.p3 = 0; - wp.flag = 0; - - setWaypoint(255, &wp); - - mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - msg.command, - MAV_RESULT_ACCEPTED, - 0, // progress - 0, // result_param2 - mavRecvMsg.sysid, - mavRecvMsg.compid); - mavlinkSendMessage(); - } else { - mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - msg.command, - MAV_RESULT_DENIED, - 0, - 0, - mavRecvMsg.sysid, - mavRecvMsg.compid); - mavlinkSendMessage(); - } - } else { -#ifdef USE_BARO - if (msg.command == MAV_CMD_DO_CHANGE_ALTITUDE) { - const float altitudeMeters = msg.param1; - const uint8_t frame = (uint8_t)msg.frame; - - geoAltitudeDatumFlag_e datum; - switch (frame) { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_INT: - datum = NAV_WP_MSL_DATUM; - break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: - datum = NAV_WP_TAKEOFF_DATUM; - break; - default: - mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - msg.command, - MAV_RESULT_UNSUPPORTED, - 0, - 0, - mavRecvMsg.sysid, - mavRecvMsg.compid); - mavlinkSendMessage(); - return true; - } - - const int32_t targetAltitudeCm = (int32_t)lrintf(altitudeMeters * 100.0f); - const bool accepted = navigationSetAltitudeTargetWithDatum(datum, targetAltitudeCm); - mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - msg.command, - accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED, - 0, - 0, - mavRecvMsg.sysid, - mavRecvMsg.compid); - mavlinkSendMessage(); - return true; - } -#endif - mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, - msg.command, - MAV_RESULT_UNSUPPORTED, - 0, - 0, - mavRecvMsg.sysid, - mavRecvMsg.compid); - mavlinkSendMessage(); - } - return true; - } - return false; -} - - -static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { - mavlink_rc_channels_override_t msg; - mavlink_msg_rc_channels_override_decode(&mavRecvMsg, &msg); - // Don't check system ID because it's not configurable with systems like Crossfire - mavlinkRxHandleMessage(&msg); - return true; -} - -static bool handleIncoming_PARAM_REQUEST_LIST(void) { - mavlink_param_request_list_t msg; - mavlink_msg_param_request_list_decode(&mavRecvMsg, &msg); - - // Respond that we don't have any parameters to force Mission Planner to give up quickly - if (msg.target_system == mavSystemId) { - // mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); - mavlink_msg_param_value_pack(mavSystemId, mavComponentId, &mavSendMsg, 0, 0, 0, 0, 0); - mavlinkSendMessage(); - } - return true; -} - -static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { - switch(telemetryConfig()->mavlink.radio_type) { - case MAVLINK_RADIO_SIK: - // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html - 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; - break; - case MAVLINK_RADIO_ELRS: - rxLinkStatistics.uplinkRSSI = -msg->remrssi; - rxLinkStatistics.uplinkSNR = msg->noise; - rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100); - 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; - break; - } -} - -static bool handleIncoming_RADIO_STATUS(void) { - mavlink_radio_status_t msg; - mavlink_msg_radio_status_decode(&mavRecvMsg, &msg); - txbuff_valid = true; - txbuff_free = msg.txbuf; - - if (rxConfig()->receiverType == RX_TYPE_SERIAL && - rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { - mavlinkParseRxStats(&msg); - } - - return true; -} - -static bool handleIncoming_HEARTBEAT(void) { - mavlink_heartbeat_t msg; - mavlink_msg_heartbeat_decode(&mavRecvMsg, &msg); - - switch (msg.type) { -#ifdef USE_ADSB - case MAV_TYPE_ADSB: - return adsbHeartbeat(); -#endif - default: - break; - } - - return false; -} - -#ifdef USE_ADSB -static bool handleIncoming_ADSB_VEHICLE(void) { - mavlink_adsb_vehicle_t msg; - mavlink_msg_adsb_vehicle_decode(&mavRecvMsg, &msg); - - adsbVehicleValues_t* vehicle = getVehicleForFill(); - if(vehicle != NULL){ - vehicle->icao = msg.ICAO_address; - vehicle->gps.lat = msg.lat; - vehicle->gps.lon = msg.lon; - vehicle->alt = (int32_t)(msg.altitude / 10); - vehicle->horVelocity = msg.hor_velocity; - vehicle->heading = msg.heading; - vehicle->flags = msg.flags; - vehicle->altitudeType = msg.altitude_type; - memcpy(&(vehicle->callsign), msg.callsign, sizeof(vehicle->callsign)); - vehicle->emitterType = msg.emitter_type; - vehicle->tslc = msg.tslc; - - adsbNewVehicle(vehicle); - } - - return true; -} -#endif - -// Returns whether a message was processed -static bool processMAVLinkIncomingTelemetry(void) -{ - while (serialRxBytesWaiting(mavlinkPort) > 0) { - // Limit handling to one message per cycle - char c = serialRead(mavlinkPort); - uint8_t result = mavlink_parse_char(0, c, &mavRecvMsg, &mavRecvStatus); - if (result == MAVLINK_FRAMING_OK) { - switch (mavRecvMsg.msgid) { - case MAVLINK_MSG_ID_HEARTBEAT: - return handleIncoming_HEARTBEAT(); - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: - return handleIncoming_PARAM_REQUEST_LIST(); - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: - return handleIncoming_MISSION_CLEAR_ALL(); - case MAVLINK_MSG_ID_MISSION_COUNT: - return handleIncoming_MISSION_COUNT(); - case MAVLINK_MSG_ID_MISSION_ITEM: - return handleIncoming_MISSION_ITEM(); - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: - return handleIncoming_MISSION_REQUEST_LIST(); - - //TODO: - //case MAVLINK_MSG_ID_COMMAND_LONG; //up to 7 float parameters - //return handleIncoming_COMMAND_LONG(); - - case MAVLINK_MSG_ID_COMMAND_INT: //7 parameters: parameters 1-4, 7 are floats, and parameters 5,6 are scaled integers - return handleIncoming_COMMAND_INT(); - case MAVLINK_MSG_ID_MISSION_REQUEST: - return handleIncoming_MISSION_REQUEST(); - case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: - handleIncoming_RC_CHANNELS_OVERRIDE(); - // Don't set that we handled a message, otherwise RC channel packets will block telemetry messages - return false; -#ifdef USE_ADSB - case MAVLINK_MSG_ID_ADSB_VEHICLE: - return handleIncoming_ADSB_VEHICLE(); -#endif - case MAVLINK_MSG_ID_RADIO_STATUS: - handleIncoming_RADIO_STATUS(); - // Don't set that we handled a message, otherwise radio status packets will block telemetry messages. - return false; - default: - return false; - } - } - } - - return false; -} - -static bool isMAVLinkTelemetryHalfDuplex(void) { - return telemetryConfig()->halfDuplex || - (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK && tristateWithDefaultOffIsActive(rxConfig()->halfDuplex)); -} - -void handleMAVLinkTelemetry(timeUs_t currentTimeUs) +void freeMAVLinkTelemetryPort(void) { - if (!mavlinkTelemetryEnabled) { - return; - } - - if (!mavlinkPort) { - return; - } - - // Process incoming MAVLink - bool receivedMessage = processMAVLinkIncomingTelemetry(); - bool shouldSendTelemetry = false; - - // Determine whether to send telemetry back based on flow control / pacing - if (txbuff_valid) { - // Use flow control if available - shouldSendTelemetry = txbuff_free >= telemetryConfig()->mavlink.min_txbuff; - } else { - // If not, use blind frame pacing - and back off for collision avoidance if half-duplex - bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage); - shouldSendTelemetry = ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) && !halfDuplexBackoff; - } - - if (shouldSendTelemetry) { - processMAVLinkTelemetry(currentTimeUs); - lastMavlinkMessage = currentTimeUs; - } + mavlinkRuntimeFreePorts(); } #endif diff --git a/src/main/telemetry/mavlink.h b/src/main/telemetry/mavlink.h index f5b51e1e3ef..29f9f2fb8cd 100755 --- a/src/main/telemetry/mavlink.h +++ b/src/main/telemetry/mavlink.h @@ -17,9 +17,12 @@ #pragma once +#include "mavlink/mavlink_types.h" + +#include "common/time.h" + void initMAVLinkTelemetry(void); void handleMAVLinkTelemetry(timeUs_t currentTimeUs); void checkMAVLinkTelemetryState(void); void freeMAVLinkTelemetryPort(void); -void configureMAVLinkTelemetryPort(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 0d198eae997..408d5311711 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -86,19 +86,59 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .accEventThresholdNegX = SETTING_ACC_EVENT_THRESHOLD_NEG_X_DEFAULT, #endif - .mavlink = { +#if defined(USE_TELEMETRY_MAVLINK) + .mavlink_common = { .autopilot_type = SETTING_MAVLINK_AUTOPILOT_TYPE_DEFAULT, - .extended_status_rate = SETTING_MAVLINK_EXT_STATUS_RATE_DEFAULT, - .rc_channels_rate = SETTING_MAVLINK_RC_CHAN_RATE_DEFAULT, - .position_rate = SETTING_MAVLINK_POS_RATE_DEFAULT, - .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, - .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, - .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, .version = SETTING_MAVLINK_VERSION_DEFAULT, - .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, - .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT, - .sysid = SETTING_MAVLINK_SYSID_DEFAULT + .sysid = SETTING_MAVLINK_SYSID_DEFAULT, + }, + .mavlink = { + { + .extended_status_rate = SETTING_MAVLINK_PORT1_EXT_STATUS_RATE_DEFAULT, + .rc_channels_rate = SETTING_MAVLINK_PORT1_RC_CHAN_RATE_DEFAULT, + .position_rate = SETTING_MAVLINK_PORT1_POS_RATE_DEFAULT, + .extra1_rate = SETTING_MAVLINK_PORT1_EXTRA1_RATE_DEFAULT, + .extra2_rate = SETTING_MAVLINK_PORT1_EXTRA2_RATE_DEFAULT, + .extra3_rate = SETTING_MAVLINK_PORT1_EXTRA3_RATE_DEFAULT, + .min_txbuff = SETTING_MAVLINK_PORT1_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_PORT1_RADIO_TYPE_DEFAULT, + .high_latency = SETTING_MAVLINK_PORT1_HIGH_LATENCY_DEFAULT + }, + { + .extended_status_rate = 0, + .rc_channels_rate = 0, + .position_rate = 0, + .extra1_rate = 0, + .extra2_rate = 0, + .extra3_rate = 0, + .min_txbuff = SETTING_MAVLINK_PORT2_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_PORT2_RADIO_TYPE_DEFAULT, + .high_latency = SETTING_MAVLINK_PORT2_HIGH_LATENCY_DEFAULT + }, + { + .extended_status_rate = 0, + .rc_channels_rate = 0, + .position_rate = 0, + .extra1_rate = 0, + .extra2_rate = 0, + .extra3_rate = 0, + .min_txbuff = SETTING_MAVLINK_PORT3_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_PORT3_RADIO_TYPE_DEFAULT, + .high_latency = SETTING_MAVLINK_PORT3_HIGH_LATENCY_DEFAULT + }, + { + .extended_status_rate = 0, + .rc_channels_rate = 0, + .position_rate = 0, + .extra1_rate = 0, + .extra2_rate = 0, + .extra3_rate = 0, + .min_txbuff = SETTING_MAVLINK_PORT4_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_PORT4_RADIO_TYPE_DEFAULT, + .high_latency = SETTING_MAVLINK_PORT4_HIGH_LATENCY_DEFAULT + } } +#endif ); void telemetryInit(void) diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 7fb26781c11..9b1f0a6bca0 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -29,6 +29,7 @@ #include "config/parameter_group.h" #include "io/serial.h" +#include "target/common.h" typedef enum { LTM_RATE_NORMAL, @@ -45,14 +46,33 @@ typedef enum { MAVLINK_RADIO_GENERIC, MAVLINK_RADIO_ELRS, MAVLINK_RADIO_SIK, + MAVLINK_RADIO_NONE // Not a radio } mavlinkRadio_e; +typedef struct mavlinkTelemetryCommonConfig_s { + uint8_t autopilot_type; + uint8_t version; + uint8_t sysid; +} mavlinkTelemetryCommonConfig_t; + typedef enum { SMARTPORT_FUEL_UNIT_PERCENT, SMARTPORT_FUEL_UNIT_MAH, SMARTPORT_FUEL_UNIT_MWH } smartportFuelUnit_e; +typedef struct mavlinkTelemetryPortConfig_s { + uint8_t extended_status_rate; + uint8_t rc_channels_rate; + uint8_t position_rate; + uint8_t extra1_rate; + uint8_t extra2_rate; + uint8_t extra3_rate; + uint8_t min_txbuff; + uint8_t radio_type; + bool high_latency; +} mavlinkTelemetryPortConfig_t; + typedef struct telemetryConfig_s { uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_inverted; // Flip the default inversion of the protocol - Same as serialrx_inverted in rx.c, but for telemetry. @@ -76,24 +96,13 @@ typedef struct telemetryConfig_s { uint16_t accEventThresholdLow; uint16_t accEventThresholdNegX; #endif - struct { - uint8_t autopilot_type; - uint8_t extended_status_rate; - uint8_t rc_channels_rate; - uint8_t position_rate; - uint8_t extra1_rate; - uint8_t extra2_rate; - uint8_t extra3_rate; - uint8_t version; - uint8_t min_txbuff; - uint8_t radio_type; - uint8_t sysid; - } mavlink; + mavlinkTelemetryCommonConfig_t mavlink_common; + mavlinkTelemetryPortConfig_t mavlink[MAX_MAVLINK_PORTS]; } telemetryConfig_t; PG_DECLARE(telemetryConfig_t, telemetryConfig); -#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_IBUS) +#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_IBUS | FUNCTION_TELEMETRY_MAVLINK) extern serialPort_t *telemetrySharedPort; void telemetryInit(void); diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 2cb53b64ecb..e8de17e9959 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -43,6 +43,13 @@ set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TE set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c") set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER) +set_property(SOURCE mavlink_unittest.cc PROPERTY depends + "fc/fc_mavlink.c" "mavlink/mavlink_mission.c" "mavlink/mavlink_modes.c" "mavlink/mavlink_ports.c" "mavlink/mavlink_routing.c" "mavlink/mavlink_runtime.c" + "mavlink/mavlink_streams.c" "telemetry/mavlink.c" "common/crc.c" "common/maths.c" "common/streambuf.c" "msp/msp_serial.c") +set_property(SOURCE mavlink_unittest.cc PROPERTY definitions USE_TELEMETRY USE_TELEMETRY_MAVLINK) +set_property(SOURCE mavlink_unittest.cc PROPERTY includes + "${CMAKE_CURRENT_SOURCE_DIR}/../../../lib/main/MAVLink") + function(unit_test src) get_filename_component(basename ${src} NAME) string(REPLACE ".cc" "" name ${basename} ) @@ -51,6 +58,7 @@ function(unit_test src) list(TRANSFORM headers REPLACE "\.c$" ".h") list(APPEND deps ${headers}) get_property(defs SOURCE ${src} PROPERTY definitions) + get_property(extra_includes SOURCE ${src} PROPERTY includes) set(test_definitions "UNIT_TEST") if (defs) list(APPEND test_definitions ${defs}) @@ -60,6 +68,9 @@ function(unit_test src) set(gen_name ${name}_gen) get_generated_files_dir(gen ${gen_name}) target_include_directories(${name} PRIVATE . ${MAIN_DIR} ${gen}) + if (extra_includes) + target_include_directories(${name} PRIVATE ${extra_includes}) + endif() target_compile_definitions(${name} PRIVATE ${test_definitions}) target_compile_options(${name} PRIVATE -pthread -Wall -Wextra -Wno-extern-c-compat -ggdb3 -O0) enable_settings(${name} ${gen_name} OUTPUTS setting_files SETTINGS_CXX g++) diff --git a/src/test/unit/mavlink_unittest.cc b/src/test/unit/mavlink_unittest.cc new file mode 100644 index 00000000000..ba62200e3c7 --- /dev/null +++ b/src/test/unit/mavlink_unittest.cc @@ -0,0 +1,2198 @@ +/* + * 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 +#include + +extern "C" { + #include "platform.h" + + #include "build/debug.h" + + #include "common/axis.h" + #include "common/maths.h" + #include "common/streambuf.h" + #include "common/time.h" + + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" + + #include "drivers/display.h" + #include "drivers/osd_symbols.h" + #include "drivers/serial.h" + + #include "fc/config.h" + #include "fc/rc_modes.h" + #include "fc/runtime_config.h" + #include "fc/settings.h" + + #include "flight/failsafe.h" + #include "flight/imu.h" + #include "flight/mixer.h" + #include "flight/mixer_profile.h" + + #include "io/adsb.h" + #include "io/gps.h" + #include "io/osd.h" + + #include "msp/msp.h" + #include "msp/msp_protocol.h" + #include "msp/msp_serial.h" + + #include "navigation/navigation.h" +#ifdef __cplusplus + #define _Static_assert static_assert +#endif + #include "navigation/navigation_private.h" +#ifdef __cplusplus + #undef _Static_assert +#endif + + #include "rx/rx.h" + + #include "sensors/barometer.h" + #include "sensors/battery.h" + #include "sensors/diagnostics.h" + #include "sensors/gyro.h" + #include "sensors/pitotmeter.h" + #include "sensors/sensors.h" + #include "sensors/temperature.h" + + #include "mavlink/mavlink_types.h" + #include "telemetry/mavlink.h" + #include "telemetry/telemetry.h" + + void mavlinkSendAttitude(void); + void mavlinkSendHeartbeat(void); + void mavlinkSendBatteryTemperatureStatusText(void); + void mavlinkSendPosition(timeUs_t currentTimeUs); + + PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); + PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); + PG_REGISTER_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 0); +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +static serialPort_t testSerialPort; +static serialPortConfig_t testPortConfig; +static uint8_t serialRxBuffer[2048]; +static uint8_t serialTxBuffer[2048]; +static size_t serialRxLen; +static size_t serialRxPos; +static size_t serialTxLen; +static const uint8_t testTargetComponent = MAV_COMP_ID_AUTOPILOT1; +static const uint8_t testTunnelSourceSystem = 42; +static const uint8_t testTunnelSourceComponent = 200; +static const uint8_t testSimpleMspCommand = 90; +static const uint8_t testLargeReplyMspCommand = 91; +static const size_t testMspFrameBufSize = MSP_PORT_OUTBUF_SIZE + 16; +static timeMs_t fakeMillis; +static int mspCommandCallCount; +static int mspPassthroughDispatchCount; +static int mspRebootPostProcessCount; +static int waitForSerialPortToFinishTransmittingCalls; +static serialPort_t *lastPostProcessPort; + +const uint32_t baudRates[] = { + 0, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 250000, + 460800, 921600, 1000000, 1500000, 2000000, 2470000 +}; + +static navWaypoint_t lastWaypoint; +static int setWaypointCalls; +static int resetWaypointCalls; +static int mavlinkRxHandleCalls; +static bool gcsValid; +static int waypointCount; +static navWaypoint_t waypointStore[4]; +static float estimatedPosition[XYZ_AXIS_COUNT]; +static float estimatedVelocity[XYZ_AXIS_COUNT]; +static int altitudeTargetSetCalls; +static bool altitudeTargetSetResult; +static geoAltitudeDatumFlag_e lastAltitudeTargetDatum; +static int32_t lastAltitudeTargetCm; +static flightModeForTelemetry_e testFlightMode; +static uint32_t testSensorsMask; + +static void resetSerialBuffers(void) +{ + serialRxLen = 0; + serialRxPos = 0; + serialTxLen = 0; +} + +static std::vector makeMspV1Request(uint8_t cmd, const std::vector &payload = {}) +{ + std::vector frame = { + '$', 'M', '<', + static_cast(payload.size()), + cmd + }; + uint8_t checksum = frame[3] ^ frame[4]; + for (const uint8_t byte : payload) { + frame.push_back(byte); + checksum ^= byte; + } + frame.push_back(checksum); + return frame; +} + +static 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]; + int length = mavlink_msg_to_send_buffer(buffer, msg); + memcpy(&serialRxBuffer[serialRxLen], buffer, (size_t)length); + serialRxLen += (size_t)length; +} + +static void pushTunnelPayload(uint8_t payloadLength, const std::vector &payload, uint8_t targetComponent = testTargetComponent) +{ + uint8_t tunnelPayload[MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN] = { 0 }; + size_t copyLength = payload.size(); + if (copyLength > MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN) { + copyLength = MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN; + } + if (copyLength > 0) { + memcpy(tunnelPayload, payload.data(), copyLength); + } + + mavlink_message_t msg; + mavlink_msg_tunnel_pack( + testTunnelSourceSystem, + testTunnelSourceComponent, + &msg, + telemetryConfig()->mavlink_common.sysid, + targetComponent, + 0x8001, + payloadLength, + tunnelPayload); + pushRxMessage(&msg); +} + +static bool popTxMessage(mavlink_message_t *msg) +{ + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + for (size_t i = 0; i < serialTxLen; i++) { + if (mavlink_parse_char(0, serialTxBuffer[i], msg, &status) == MAVLINK_FRAMING_OK) { + return true; + } + } + return false; +} + +static bool findTxMessageById(uint32_t msgid, mavlink_message_t *match) +{ + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + + mavlink_message_t msg; + for (size_t i = 0; i < serialTxLen; i++) { + if (mavlink_parse_char(0, serialTxBuffer[i], &msg, &status) == MAVLINK_FRAMING_OK && msg.msgid == msgid) { + *match = msg; + return true; + } + } + + return false; +} + +static std::vector parseTxMessages(void) +{ + std::vector messages; + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + + mavlink_message_t msg; + for (size_t i = 0; i < serialTxLen; i++) { + if (mavlink_parse_char(0, serialTxBuffer[i], &msg, &status) == MAVLINK_FRAMING_OK) { + messages.push_back(msg); + } + } + + return messages; +} + +static std::vector collectTunnelPayload(const std::vector &messages) +{ + std::vector payload; + + for (const mavlink_message_t &msg : messages) { + EXPECT_EQ(msg.msgid, MAVLINK_MSG_ID_TUNNEL); + + mavlink_tunnel_t tunnel; + mavlink_msg_tunnel_decode(&msg, &tunnel); + + EXPECT_EQ(tunnel.payload_type, 0x8001); + EXPECT_EQ(tunnel.target_system, testTunnelSourceSystem); + EXPECT_EQ(tunnel.target_component, testTunnelSourceComponent); + + payload.insert(payload.end(), tunnel.payload, tunnel.payload + tunnel.payload_length); + } + + return payload; +} + +static void initMavlinkTestState(void) +{ + resetSerialBuffers(); + fakeMillis = 0; + setWaypointCalls = 0; + resetWaypointCalls = 0; + mavlinkRxHandleCalls = 0; + mspCommandCallCount = 0; + mspPassthroughDispatchCount = 0; + mspRebootPostProcessCount = 0; + waitForSerialPortToFinishTransmittingCalls = 0; + lastPostProcessPort = NULL; + gcsValid = true; + waypointCount = 0; + memset(estimatedPosition, 0, sizeof(estimatedPosition)); + memset(estimatedVelocity, 0, sizeof(estimatedVelocity)); + altitudeTargetSetCalls = 0; + altitudeTargetSetResult = true; + lastAltitudeTargetDatum = NAV_WP_TAKEOFF_DATUM; + lastAltitudeTargetCm = 0; + testFlightMode = FLM_MANUAL; + testSensorsMask = 0; + armingFlags = 0; + stateFlags = 0; + memset(&gpsSol, 0, sizeof(gpsSol)); + memset(&GPS_home, 0, sizeof(GPS_home)); + memset(waypointStore, 0, sizeof(waypointStore)); + memset(&rxLinkStatistics, 0, sizeof(rxLinkStatistics)); + posControl.wpReachedSeq = 0; + posControl.wpReachedNotificationPending = false; + + telemetryConfigMutable()->mavlink_common.sysid = 1; + telemetryConfigMutable()->mavlink_common.autopilot_type = MAVLINK_AUTOPILOT_ARDUPILOT; + telemetryConfigMutable()->mavlink_common.version = 2; + telemetryConfigMutable()->mavlink[0].extended_status_rate = 2; + telemetryConfigMutable()->mavlink[0].rc_channels_rate = 1; + telemetryConfigMutable()->mavlink[0].position_rate = 2; + telemetryConfigMutable()->mavlink[0].extra1_rate = 2; + telemetryConfigMutable()->mavlink[0].extra2_rate = 2; + telemetryConfigMutable()->mavlink[0].extra3_rate = 1; + telemetryConfigMutable()->mavlink[0].min_txbuff = 0; + telemetryConfigMutable()->halfDuplex = 0; + + rxConfigMutable()->receiverType = RX_TYPE_NONE; + rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; + rxConfigMutable()->halfDuplex = 0; + + systemConfigMutable()->current_mixer_profile_index = 0; + mixerProfilesMutable(0)->mixer_config.platformType = PLATFORM_AIRPLANE; + + rxRuntimeConfig.channelCount = 8; + + initMAVLinkTelemetry(); + 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, AttitudeUsesRadiansPerSecond) +{ + initMavlinkTestState(); + + attitude.values.roll = 100; + attitude.values.pitch = -200; + attitude.values.yaw = 450; + gyro.gyroADCf[FD_ROLL] = 90.0f; + gyro.gyroADCf[FD_PITCH] = -45.0f; + gyro.gyroADCf[FD_YAW] = 180.0f; + + mavlinkSendAttitude(); + + mavlink_message_t msg; + ASSERT_TRUE(popTxMessage(&msg)); + ASSERT_EQ(msg.msgid, MAVLINK_MSG_ID_ATTITUDE); + + mavlink_attitude_t att; + mavlink_msg_attitude_decode(&msg, &att); + + EXPECT_NEAR(att.rollspeed, DEGREES_TO_RADIANS(gyro.gyroADCf[FD_ROLL]), 1e-6f); + EXPECT_NEAR(att.pitchspeed, DEGREES_TO_RADIANS(gyro.gyroADCf[FD_PITCH]), 1e-6f); + EXPECT_NEAR(att.yawspeed, DEGREES_TO_RADIANS(gyro.gyroADCf[FD_YAW]), 1e-6f); +} + +TEST(MavlinkTelemetryTest, CommandLongRepositionUsesGlobalFrameAndParams) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_REPOSITION, + 0, + 0, 0, 0, 123.4f, + 37.5f, -122.25f, 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(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.lat, (int32_t)(37.5f * 1e7f)); + EXPECT_EQ(lastWaypoint.lon, (int32_t)(-122.25f * 1e7f)); + EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f)); + EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE); + EXPECT_EQ(lastWaypoint.p1, 123); +} + +TEST(MavlinkTelemetryTest, BroadcastCommandLongRepositionExecutesLocally) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 42, 200, &cmd, + 0, 0, + MAV_CMD_DO_REPOSITION, + 0, + 0, 0, 0, 123.4f, + 37.5f, -122.25f, 12.3f); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_COMMAND_ACK, &ackMsg)); + + 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(setWaypointCalls, 1); +} + +TEST(MavlinkTelemetryTest, SameSystemDifferentComponentIsNotDroppedAsLocalIdentity) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_long_pack( + 1, 42, &cmd, + 1, testTargetComponent, + MAV_CMD_DO_REPOSITION, + 0, + 0, 0, 0, 0, + 37.5f, -122.25f, 12.3f); + + pushRxMessage(&cmd); + handleMAVLinkTelemetry(1000); + + mavlink_message_t ackMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_COMMAND_ACK, &ackMsg)); + + 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(setWaypointCalls, 1); +} + +TEST(MavlinkTelemetryTest, CommandIntUnsupportedFrameIsRejected) +{ + initMavlinkTestState(); + + mavlink_message_t cmd; + mavlink_msg_command_int_pack( + 42, 200, &cmd, + 1, testTargetComponent, + MAV_FRAME_BODY_NED, + MAV_CMD_DO_REPOSITION, + 0, 0, + 0, 0, 0, 0, + 100000000, 200000000, 10.0f); + + 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_UNSUPPORTED); + EXPECT_EQ(setWaypointCalls, 0); +} + +TEST(MavlinkTelemetryTest, CommandIntRepositionScalesCoordinates) +{ + 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, 0, 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(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_NEAR((double)lastWaypoint.lon, -1222500000.0, 100.0); + EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f)); + EXPECT_EQ(lastWaypoint.p3, 0); + EXPECT_EQ(lastWaypoint.p1, 45); +} + +TEST(MavlinkTelemetryTest, MissionClearAllAcksAndResets) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_mission_clear_all_pack( + 42, 200, &msg, + 1, testTargetComponent, MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(resetWaypointCalls, 1); + + 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(ack.mission_type, MAV_MISSION_TYPE_MISSION); +} + +TEST(MavlinkTelemetryTest, MissionCountRequestsFirstItem) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_mission_count_pack( + 42, 200, &msg, + 1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t reqMsg; + ASSERT_TRUE(popTxMessage(&reqMsg)); + ASSERT_EQ(reqMsg.msgid, MAVLINK_MSG_ID_MISSION_REQUEST_INT); + + mavlink_mission_request_int_t req; + mavlink_msg_mission_request_int_decode(&reqMsg, &req); + + EXPECT_EQ(req.seq, 0); + EXPECT_EQ(req.mission_type, MAV_MISSION_TYPE_MISSION); +} + +TEST(MavlinkTelemetryTest, MissionCountWhileArmedIsRejected) +{ + initMavlinkTestState(); + ENABLE_ARMING_FLAG(ARMED); + + mavlink_message_t msg; + mavlink_msg_mission_count_pack( + 42, 200, &msg, + 1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + mavlink_message_t outMsg; + bool sawAck = false; + bool sawRequest = false; + + for (size_t i = 0; i < serialTxLen; i++) { + if (mavlink_parse_char(0, serialTxBuffer[i], &outMsg, &status) == MAVLINK_FRAMING_OK) { + if (outMsg.msgid == MAVLINK_MSG_ID_MISSION_ACK) { + mavlink_mission_ack_t ack; + mavlink_msg_mission_ack_decode(&outMsg, &ack); + EXPECT_EQ(ack.type, MAV_MISSION_ERROR); + sawAck = true; + } + if (outMsg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_INT) { + sawRequest = true; + } + } + } + + EXPECT_TRUE(sawAck); + EXPECT_FALSE(sawRequest); +} + +TEST(MavlinkTelemetryTest, MissionItemIntSingleItemAcksAccepted) +{ + initMavlinkTestState(); + + mavlink_message_t countMsg; + mavlink_msg_mission_count_pack( + 42, 200, &countMsg, + 1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0); + + pushRxMessage(&countMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t itemMsg; + mavlink_msg_mission_item_int_pack( + 42, 200, &itemMsg, + 1, testTargetComponent, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_NAV_WAYPOINT, 0, 1, + 0, 0, 0, 0, + 375000000, -1222500000, 12.3f, + MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&itemMsg); + handleMAVLinkTelemetry(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(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.lat, 375000000); + EXPECT_EQ(lastWaypoint.lon, -1222500000); + EXPECT_EQ(lastWaypoint.alt, (int32_t)(12.3f * 100.0f)); +} + +TEST(MavlinkTelemetryTest, MissionItemIntSingleFinalItemAllowsAutocontinueZero) +{ + initMavlinkTestState(); + + mavlink_message_t countMsg; + mavlink_msg_mission_count_pack( + 42, 200, &countMsg, + 1, testTargetComponent, 1, MAV_MISSION_TYPE_MISSION, 0); + + pushRxMessage(&countMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t itemMsg; + mavlink_msg_mission_item_int_pack( + 42, 200, &itemMsg, + 1, testTargetComponent, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_NAV_WAYPOINT, 0, 0, + 0, 0, 0, 0, + 375000000, -1222500000, 12.3f, + MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&itemMsg); + handleMAVLinkTelemetry(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); +} + +TEST(MavlinkTelemetryTest, MissionItemIntNonFinalAutocontinueZeroIsRejected) +{ + initMavlinkTestState(); + + mavlink_message_t countMsg; + mavlink_msg_mission_count_pack( + 42, 200, &countMsg, + 1, testTargetComponent, 2, MAV_MISSION_TYPE_MISSION, 0); + + pushRxMessage(&countMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t itemMsg; + mavlink_msg_mission_item_int_pack( + 42, 200, &itemMsg, + 1, testTargetComponent, 0, + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + MAV_CMD_NAV_WAYPOINT, 0, 0, + 0, 0, 0, 0, + 375000000, -1222500000, 12.3f, + MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&itemMsg); + handleMAVLinkTelemetry(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_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); + handleMAVLinkTelemetry(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); + handleMAVLinkTelemetry(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) +{ + initMavlinkTestState(); + waypointCount = 2; + + mavlink_message_t msg; + mavlink_msg_mission_request_list_pack( + 42, 200, &msg, + 1, testTargetComponent, MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t countMsg; + ASSERT_TRUE(popTxMessage(&countMsg)); + ASSERT_EQ(countMsg.msgid, MAVLINK_MSG_ID_MISSION_COUNT); + + mavlink_mission_count_t count; + mavlink_msg_mission_count_decode(&countMsg, &count); + + EXPECT_EQ(count.count, 2); + EXPECT_EQ(count.mission_type, MAV_MISSION_TYPE_MISSION); +} + +TEST(MavlinkTelemetryTest, MissionRequestSendsWaypoint) +{ + initMavlinkTestState(); + waypointCount = 1; + waypointStore[0].action = NAV_WP_ACTION_WAYPOINT; + waypointStore[0].lat = 375000000; + waypointStore[0].lon = -1222500000; + waypointStore[0].alt = 1234; + waypointStore[0].p3 = 0; + + mavlink_message_t msg; + mavlink_msg_mission_request_pack( + 42, 200, &msg, + 1, testTargetComponent, 0, MAV_MISSION_TYPE_MISSION); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t itemMsg; + ASSERT_TRUE(popTxMessage(&itemMsg)); + ASSERT_EQ(itemMsg.msgid, MAVLINK_MSG_ID_MISSION_ITEM); + + mavlink_mission_item_t item; + mavlink_msg_mission_item_decode(&itemMsg, &item); + + EXPECT_EQ(item.seq, 0); + EXPECT_EQ(item.command, MAV_CMD_NAV_WAYPOINT); + EXPECT_EQ(item.frame, MAV_FRAME_GLOBAL_RELATIVE_ALT); + EXPECT_NEAR(item.x, 37.5f, 1e-4f); + EXPECT_NEAR(item.y, -122.25f, 1e-4f); + EXPECT_NEAR(item.z, 12.34f, 1e-4f); +} + +TEST(MavlinkTelemetryTest, MissionItemReachedIsBroadcastOnceWhenPending) +{ + initMavlinkTestState(); + + posControl.wpReachedSeq = 3; + posControl.wpReachedNotificationPending = true; + + handleMAVLinkTelemetry(1000); + + mavlink_message_t reachedMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &reachedMsg)); + + mavlink_mission_item_reached_t reached; + mavlink_msg_mission_item_reached_decode(&reachedMsg, &reached); + + EXPECT_EQ(reached.seq, 3); + + resetSerialBuffers(); + handleMAVLinkTelemetry(1000); + + 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); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(setWaypointCalls, 1); + EXPECT_EQ(lastWaypoint.p3, NAV_WP_ALTMODE); +} + +TEST(MavlinkTelemetryTest, ParamRequestListRespondsWithEmptyParam) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_param_request_list_pack( + 42, 200, &msg, + 1, testTargetComponent); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t paramMsg; + ASSERT_TRUE(popTxMessage(¶mMsg)); + ASSERT_EQ(paramMsg.msgid, MAVLINK_MSG_ID_PARAM_VALUE); + + mavlink_param_value_t param; + mavlink_msg_param_value_decode(¶mMsg, ¶m); + + EXPECT_EQ(param.param_count, 0); + EXPECT_EQ(param.param_index, 0); +} + +TEST(MavlinkTelemetryTest, BroadcastParamRequestListRespondsWithEmptyParam) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_param_request_list_pack( + 42, 200, &msg, + 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t paramMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_PARAM_VALUE, ¶mMsg)); + + mavlink_param_value_t param; + mavlink_msg_param_value_decode(¶mMsg, ¶m); + + EXPECT_EQ(param.param_count, 0); + 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); + handleMAVLinkTelemetry(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); + handleMAVLinkTelemetry(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); + handleMAVLinkTelemetry(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); + handleMAVLinkTelemetry(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); + handleMAVLinkTelemetry(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); + handleMAVLinkTelemetry(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); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(altitudeTargetSetCalls, 0); +} + +TEST(MavlinkTelemetryTest, RequestDataStreamStopsStream) +{ + initMavlinkTestState(); + + mavlink_message_t setMsg; + mavlink_msg_request_data_stream_pack( + 42, 200, &setMsg, + 1, testTargetComponent, + MAV_DATA_STREAM_RC_CHANNELS, 10, 1); + + pushRxMessage(&setMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_RC_CHANNELS, + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(popTxMessage(&intervalMsg)); + ASSERT_EQ(intervalMsg.msgid, MAVLINK_MSG_ID_MESSAGE_INTERVAL); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_RC_CHANNELS); + EXPECT_EQ(interval.interval_us, 100000); + + serialTxLen = 0; + + mavlink_message_t stopMsg; + mavlink_msg_request_data_stream_pack( + 42, 200, &stopMsg, + 1, testTargetComponent, + MAV_DATA_STREAM_RC_CHANNELS, 0, 0); + + pushRxMessage(&stopMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + ASSERT_TRUE(popTxMessage(&intervalMsg)); + ASSERT_EQ(intervalMsg.msgid, MAVLINK_MSG_ID_MESSAGE_INTERVAL); + + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_RC_CHANNELS); + EXPECT_EQ(interval.interval_us, -1); +} + +TEST(MavlinkTelemetryTest, BroadcastRequestDataStreamAdjustsBaseInterval) +{ + initMavlinkTestState(); + + mavlink_message_t setMsg; + mavlink_msg_request_data_stream_pack( + 42, 200, &setMsg, + 0, 0, + MAV_DATA_STREAM_RC_CHANNELS, 10, 1); + + pushRxMessage(&setMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_RC_CHANNELS, + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_RC_CHANNELS); + EXPECT_EQ(interval.interval_us, 100000); +} + +TEST(MavlinkTelemetryTest, SetMessageIntervalOverridesOnlyRequestedPositionMessage) +{ + initMavlinkTestState(); + + mavlink_message_t setMsg; + mavlink_msg_command_long_pack( + 42, 200, &setMsg, + 1, testTargetComponent, + MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_GLOBAL_POSITION_INT, + 200000.0f, 0, 0, 0, 0, 0); + + pushRxMessage(&setMsg); + handleMAVLinkTelemetry(1000); + + const uint16_t messageIds[] = { + MAVLINK_MSG_ID_GLOBAL_POSITION_INT, + MAVLINK_MSG_ID_GPS_RAW_INT, + MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN + }; + const int32_t expectedIntervals[] = { + 200000, + 500000, + 500000 + }; + + for (size_t i = 0; i < ARRAYLEN(messageIds); i++) { + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)messageIds[i], + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, messageIds[i]); + EXPECT_EQ(interval.interval_us, expectedIntervals[i]); + } +} + +TEST(MavlinkTelemetryTest, SetMessageIntervalCanDisableBatteryStatusWithoutAffectingScaledPressure) +{ + initMavlinkTestState(); + + mavlink_message_t setMsg; + mavlink_msg_command_long_pack( + 42, 200, &setMsg, + 1, testTargetComponent, + MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_BATTERY_STATUS, + -1.0f, 0, 0, 0, 0, 0); + + pushRxMessage(&setMsg); + handleMAVLinkTelemetry(1000); + + const uint16_t messageIds[] = { + MAVLINK_MSG_ID_BATTERY_STATUS, + MAVLINK_MSG_ID_SCALED_PRESSURE + }; + const int32_t expectedIntervals[] = { + -1, + 1000000 + }; + + for (size_t i = 0; i < ARRAYLEN(messageIds); i++) { + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)messageIds[i], + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, messageIds[i]); + EXPECT_EQ(interval.interval_us, expectedIntervals[i]); + } +} + +TEST(MavlinkTelemetryTest, SetMessageIntervalResetRevertsToCurrentGroupInterval) +{ + initMavlinkTestState(); + + mavlink_message_t streamMsg; + mavlink_msg_request_data_stream_pack( + 42, 200, &streamMsg, + 1, testTargetComponent, + MAV_DATA_STREAM_POSITION, 10, 1); + + pushRxMessage(&streamMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t setMsg; + mavlink_msg_command_long_pack( + 42, 200, &setMsg, + 1, testTargetComponent, + MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_GLOBAL_POSITION_INT, + 250000.0f, 0, 0, 0, 0, 0); + + pushRxMessage(&setMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t clearMsg; + mavlink_msg_command_long_pack( + 42, 200, &clearMsg, + 1, testTargetComponent, + MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_GLOBAL_POSITION_INT, + 0.0f, 0, 0, 0, 0, 0); + + pushRxMessage(&clearMsg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_GLOBAL_POSITION_INT, + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT); + EXPECT_EQ(interval.interval_us, 100000); +} + +TEST(MavlinkTelemetryTest, HeartbeatIntervalIsIndependentFromExtra2Stream) +{ + initMavlinkTestState(); + + mavlink_message_t stopExtra2Msg; + mavlink_msg_request_data_stream_pack( + 42, 200, &stopExtra2Msg, + 1, testTargetComponent, + MAV_DATA_STREAM_EXTRA2, 0, 0); + + pushRxMessage(&stopExtra2Msg); + handleMAVLinkTelemetry(1000); + + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_HEARTBEAT, + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(popTxMessage(&intervalMsg)); + ASSERT_EQ(intervalMsg.msgid, MAVLINK_MSG_ID_MESSAGE_INTERVAL); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_HEARTBEAT); + EXPECT_EQ(interval.interval_us, 1000000); +} + +TEST(MavlinkTelemetryTest, RequestProtocolVersionUsesConfiguredVersion) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_command_long_pack( + 42, 200, &msg, + 1, testTargetComponent, + MAV_CMD_REQUEST_PROTOCOL_VERSION, + 0, + 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t versionMsg; + ASSERT_TRUE(popTxMessage(&versionMsg)); + ASSERT_EQ(versionMsg.msgid, MAVLINK_MSG_ID_PROTOCOL_VERSION); + + mavlink_protocol_version_t version; + mavlink_msg_protocol_version_decode(&versionMsg, &version); + + EXPECT_EQ(version.version, 200); + EXPECT_EQ(version.min_version, 200); + EXPECT_EQ(version.max_version, 200); +} + +TEST(MavlinkTelemetryTest, SystemTimeSupportsRequestPeriodicOutputAndIntervalQuery) +{ + initMavlinkTestState(); + fakeMillis = 1234; + + mavlink_message_t requestMsg; + mavlink_msg_command_long_pack( + 42, 200, &requestMsg, + 1, testTargetComponent, + MAV_CMD_REQUEST_MESSAGE, + 0, + (float)MAVLINK_MSG_ID_SYSTEM_TIME, + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&requestMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t systemTimeMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_SYSTEM_TIME, &systemTimeMsg)); + + mavlink_system_time_t systemTime; + mavlink_msg_system_time_decode(&systemTimeMsg, &systemTime); + + EXPECT_EQ(systemTime.time_unix_usec, 0U); + EXPECT_EQ(systemTime.time_boot_ms, 1234U); + + serialTxLen = 0; + handleMAVLinkTelemetry(1000000); + + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_SYSTEM_TIME, &systemTimeMsg)); + mavlink_msg_system_time_decode(&systemTimeMsg, &systemTime); + EXPECT_EQ(systemTime.time_boot_ms, 1234U); + + serialTxLen = 0; + + mavlink_message_t getMsg; + mavlink_msg_command_long_pack( + 42, 200, &getMsg, + 1, testTargetComponent, + MAV_CMD_GET_MESSAGE_INTERVAL, + 0, + (float)MAVLINK_MSG_ID_SYSTEM_TIME, + 0, 0, 0, 0, 0, 0); + + pushRxMessage(&getMsg); + handleMAVLinkTelemetry(1000); + + mavlink_message_t intervalMsg; + ASSERT_TRUE(findTxMessageById(MAVLINK_MSG_ID_MESSAGE_INTERVAL, &intervalMsg)); + + mavlink_message_interval_t interval; + mavlink_msg_message_interval_decode(&intervalMsg, &interval); + + EXPECT_EQ(interval.message_id, MAVLINK_MSG_ID_SYSTEM_TIME); + EXPECT_EQ(interval.interval_us, 1000000); +} + +TEST(MavlinkTelemetryTest, RequestAutopilotCapabilitiesReportsLocalNedCapabilityAndPackedVersion) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_command_long_pack( + 42, 200, &msg, + 1, testTargetComponent, + MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, + 0, + 1, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + mavlink_message_t outMsg; + bool sawAutopilotVersion = false; + + for (size_t i = 0; i < serialTxLen; i++) { + if (mavlink_parse_char(0, serialTxBuffer[i], &outMsg, &status) == MAVLINK_FRAMING_OK) { + if (outMsg.msgid == MAVLINK_MSG_ID_AUTOPILOT_VERSION) { + mavlink_autopilot_version_t version; + mavlink_msg_autopilot_version_decode(&outMsg, &version); + EXPECT_NE((version.capabilities & MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED), 0U); + EXPECT_EQ(version.flight_sw_version, + ((uint32_t)ARDUPILOT_VERSION_MAJOR << 24) | + ((uint32_t)ARDUPILOT_VERSION_MINOR << 16) | + ((uint32_t)ARDUPILOT_VERSION_PATCH << 8)); + EXPECT_EQ(version.middleware_sw_version, 0U); + EXPECT_EQ(version.os_sw_version, 0U); + sawAutopilotVersion = true; + } + } + } + + EXPECT_TRUE(sawAutopilotVersion); +} + +TEST(MavlinkTelemetryTest, HeartbeatGuidedFlagRequiresValidGcsInPoshold) +{ + initMavlinkTestState(); + testFlightMode = FLM_POSITION_HOLD; + gcsValid = false; + + mavlinkSendHeartbeat(); + + mavlink_message_t msg; + ASSERT_TRUE(popTxMessage(&msg)); + ASSERT_EQ(msg.msgid, MAVLINK_MSG_ID_HEARTBEAT); + + mavlink_heartbeat_t heartbeat; + mavlink_msg_heartbeat_decode(&msg, &heartbeat); + EXPECT_EQ((heartbeat.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED), 0); + + serialTxLen = 0; + gcsValid = true; + + mavlinkSendHeartbeat(); + + ASSERT_TRUE(popTxMessage(&msg)); + ASSERT_EQ(msg.msgid, MAVLINK_MSG_ID_HEARTBEAT); + + mavlink_msg_heartbeat_decode(&msg, &heartbeat); + EXPECT_NE((heartbeat.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED), 0); +} + +TEST(MavlinkTelemetryTest, PositionReportsPositiveDownVelocity) +{ + initMavlinkTestState(); + testSensorsMask = SENSOR_GPS; + gpsSol.fixType = GPS_FIX_3D; + gpsSol.llh.lat = 375000000; + gpsSol.llh.lon = -1222500000; + gpsSol.llh.alt = 12345; + estimatedVelocity[Z] = 321.0f; + + mavlinkSendPosition(1000); + + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + mavlink_message_t msg; + bool sawGlobalPosition = false; + + for (size_t i = 0; i < serialTxLen; i++) { + if (mavlink_parse_char(0, serialTxBuffer[i], &msg, &status) == MAVLINK_FRAMING_OK) { + if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) { + mavlink_global_position_int_t position; + mavlink_msg_global_position_int_decode(&msg, &position); + EXPECT_EQ(position.vz, -321); + sawGlobalPosition = true; + } + } + } + + EXPECT_TRUE(sawGlobalPosition); +} + +TEST(MavlinkTelemetryTest, BatteryStatusDoesNotSendExtendedSysState) +{ + initMavlinkTestState(); + + mavlinkSendBatteryTemperatureStatusText(); + + mavlink_status_t status; + memset(&status, 0, sizeof(status)); + mavlink_message_t msg; + bool sawExtSysState = false; + + for (size_t i = 0; i < serialTxLen; i++) { + if (mavlink_parse_char(0, serialTxBuffer[i], &msg, &status) == MAVLINK_FRAMING_OK) { + if (msg.msgid == MAVLINK_MSG_ID_EXTENDED_SYS_STATE) { + sawExtSysState = true; + break; + } + } + } + + EXPECT_FALSE(sawExtSysState); +} + +TEST(MavlinkTelemetryTest, RadioStatusUpdatesRxLinkStats) +{ + initMavlinkTestState(); + rxConfigMutable()->receiverType = RX_TYPE_SERIAL; + rxConfigMutable()->serialrx_provider = SERIALRX_MAVLINK; + telemetryConfigMutable()->mavlink[0].radio_type = MAVLINK_RADIO_ELRS; + + mavlink_message_t msg; + mavlink_msg_radio_status_pack( + 42, 200, &msg, + 200, 150, 255, 7, 3, 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(rxLinkStatistics.uplinkRSSI, -150); + EXPECT_EQ(rxLinkStatistics.uplinkSNR, 7); + EXPECT_EQ(rxLinkStatistics.uplinkLQ, scaleRange(200, 0, 255, 0, 100)); +} + +TEST(MavlinkTelemetryTest, RcChannelsOverrideIsForwarded) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_rc_channels_override_pack( + 42, 200, &msg, + 1, testTargetComponent, + 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mavlinkRxHandleCalls, 1); +} + +TEST(MavlinkTelemetryTest, RcChannelsOverrideIgnoresTargetSystemMismatch) +{ + initMavlinkTestState(); + + mavlink_message_t msg; + mavlink_msg_rc_channels_override_pack( + 42, 200, &msg, + 99, testTargetComponent, + 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + + pushRxMessage(&msg); + handleMAVLinkTelemetry(1000); + + EXPECT_EQ(mavlinkRxHandleCalls, 1); +} + +extern "C" { + +int32_t debug[DEBUG32_VALUE_COUNT]; + +uint32_t armingFlags; +uint32_t stateFlags; +bool cliMode; + +attitudeEulerAngles_t attitude; +gyro_t gyro; +gpsSolutionData_t gpsSol; +gpsLocation_t GPS_home; +navSystemStatus_t NAV_Status; +navigationPosControl_t posControl; +rxRuntimeConfig_t rxRuntimeConfig; +rxLinkStatistics_t rxLinkStatistics; +uint16_t averageSystemLoadPercent; + +timeUs_t micros(void) +{ + return 0; +} + +uint32_t millis(void) +{ + return fakeMillis; +} + +serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) +{ + UNUSED(function); + testPortConfig.functionMask = FUNCTION_TELEMETRY_MAVLINK; + testPortConfig.identifier = SERIAL_PORT_USART1; + testPortConfig.telemetry_baudrateIndex = BAUD_115200; + return &testPortConfig; +} + +serialPortConfig_t *findNextSerialPortConfig(serialPortFunction_e function) +{ + UNUSED(function); + return NULL; +} + +portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function) +{ + UNUSED(portConfig); + UNUSED(function); + return PORTSHARING_NOT_SHARED; +} + +serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function, + serialReceiveCallbackPtr rxCallback, void *rxCallbackData, + uint32_t baudRate, portMode_t mode, portOptions_t options) +{ + UNUSED(identifier); + UNUSED(function); + UNUSED(rxCallback); + UNUSED(rxCallbackData); + UNUSED(baudRate); + UNUSED(mode); + UNUSED(options); + return &testSerialPort; +} + +void closeSerialPort(serialPort_t *serialPort) +{ + UNUSED(serialPort); +} + +uint32_t serialRxBytesWaiting(const serialPort_t *instance) +{ + UNUSED(instance); + return (uint32_t)(serialRxLen - serialRxPos); +} + +uint32_t serialTxBytesFree(const serialPort_t *instance) +{ + UNUSED(instance); + return 1024; +} + +uint8_t serialRead(serialPort_t *instance) +{ + UNUSED(instance); + return serialRxBuffer[serialRxPos++]; +} + +void serialWrite(serialPort_t *instance, uint8_t ch) +{ + UNUSED(instance); + serialTxBuffer[serialTxLen++] = ch; +} + +void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) +{ + UNUSED(instance); + memcpy(&serialTxBuffer[serialTxLen], data, (size_t)count); + serialTxLen += (size_t)count; +} + +void serialBeginWrite(serialPort_t *instance) +{ + UNUSED(instance); +} + +void serialEndWrite(serialPort_t *instance) +{ + UNUSED(instance); +} + +void serialSetMode(serialPort_t *instance, portMode_t mode) +{ + UNUSED(instance); + UNUSED(mode); +} + +bool telemetryDetermineEnabledState(portSharing_e portSharing) +{ + UNUSED(portSharing); + return true; +} + +bool serialIsConnected(const serialPort_t *instance) +{ + UNUSED(instance); + return true; +} + +bool isSerialTransmitBufferEmpty(const serialPort_t *instance) +{ + UNUSED(instance); + return true; +} + +void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort) +{ + waitForSerialPortToFinishTransmittingCalls++; + lastPostProcessPort = serialPort; +} + +void cliEnter(serialPort_t *serialPort) +{ + UNUSED(serialPort); +} + +bool sensors(uint32_t mask) +{ + return (testSensorsMask & mask) != 0; +} + +bool isAmperageConfigured(void) +{ + return false; +} + +bool isBlackboxDeviceFull(void) +{ + return false; +} + +bool feature(uint32_t mask) +{ + UNUSED(mask); + return false; +} + +int16_t getAmperage(void) +{ + return 0; +} + +int32_t getMAhDrawn(void) +{ + return 0; +} + +int32_t getMWhDrawn(void) +{ + return 0; +} + +uint8_t getBatteryCellCount(void) +{ + return 0; +} + +batteryState_e getBatteryState(void) +{ + return BATTERY_OK; +} + +bool isEstimatedWindSpeedValid(void) +{ + return false; +} + +float getEstimatedHorizontalWindSpeed(uint16_t *angle) +{ + if (angle) { + *angle = 0; + } + return 0; +} + +uint16_t getBatteryAverageCellVoltage(void) +{ + return 0; +} + +uint16_t getBatteryVoltage(void) +{ + return 0; +} + +int16_t getThrottlePercent(bool scaled) +{ + UNUSED(scaled); + return 0; +} + +bool osdUsingScaledThrottle(void) +{ + return false; +} + +float getEstimatedActualPosition(int axis) +{ + return estimatedPosition[axis]; +} + +float getEstimatedActualVelocity(int axis) +{ + return estimatedVelocity[axis]; +} + +float getAirspeedEstimate(void) +{ + return 0.0f; +} + +bool pitotIsHealthy(void) +{ + return false; +} + +bool rxIsReceivingSignal(void) +{ + return false; +} + +bool rxAreFlightChannelsValid(void) +{ + return false; +} + +uint16_t getRSSI(void) +{ + return 0; +} + +int16_t rxGetChannelValue(unsigned channel) +{ + UNUSED(channel); + return 1500; +} + +hardwareSensorStatus_e getHwGyroStatus(void) { return HW_SENSOR_NONE; } +hardwareSensorStatus_e getHwAccelerometerStatus(void) { return HW_SENSOR_NONE; } +hardwareSensorStatus_e getHwCompassStatus(void) { return HW_SENSOR_NONE; } +hardwareSensorStatus_e getHwBarometerStatus(void) { return HW_SENSOR_NONE; } +hardwareSensorStatus_e getHwGPSStatus(void) { return HW_SENSOR_NONE; } +hardwareSensorStatus_e getHwRangefinderStatus(void) { return HW_SENSOR_NONE; } +hardwareSensorStatus_e getHwPitotmeterStatus(void) { return HW_SENSOR_NONE; } +hardwareSensorStatus_e getHwOpticalFlowStatus(void) { return HW_SENSOR_NONE; } + +bool getBaroTemperature(int16_t *temperature) +{ + *temperature = 0; + return false; +} + +bool getIMUTemperature(int16_t *temperature) +{ + *temperature = 0; + return false; +} + +bool areSensorsCalibrating(void) +{ + return false; +} + +bool failsafeIsActive(void) +{ + return false; +} + +failsafePhase_e failsafePhase(void) +{ + return FAILSAFE_IDLE; +} + +int isGCSValid(void) +{ + return gcsValid; +} + +bool navigationSetAltitudeTargetWithDatum(geoAltitudeDatumFlag_e datumFlag, int32_t targetAltitudeCm) +{ + altitudeTargetSetCalls++; + lastAltitudeTargetDatum = datumFlag; + lastAltitudeTargetCm = targetAltitudeCm; + return altitudeTargetSetResult; +} + +bool navigationConsumeWaypointReached(uint16_t *seq) +{ + if (!posControl.wpReachedNotificationPending) { + return false; + } + + *seq = posControl.wpReachedSeq; + posControl.wpReachedNotificationPending = false; + return true; +} + +navigationFSMStateFlags_t navGetCurrentStateFlags(void) +{ + return (navigationFSMStateFlags_t)0; +} + +void updateHeadingHoldTarget(int16_t heading) +{ + UNUSED(heading); +} + +void setWaypoint(uint8_t wpNumber, const navWaypoint_t *wp) +{ + UNUSED(wpNumber); + lastWaypoint = *wp; + setWaypointCalls++; +} + +int getWaypointCount(void) +{ + return waypointCount; +} + +void getWaypoint(uint8_t wpNumber, navWaypoint_t *wp) +{ + if (wpNumber == 0 || wpNumber > ARRAYLEN(waypointStore)) { + memset(wp, 0, sizeof(*wp)); + return; + } + *wp = waypointStore[wpNumber - 1]; +} + +bool isWaypointListValid(void) +{ + return true; +} + +void resetWaypointList(void) +{ + resetWaypointCalls++; + waypointCount = 0; + memset(waypointStore, 0, sizeof(waypointStore)); +} + +flightModeForTelemetry_e getFlightModeForTelemetry(void) +{ + return testFlightMode; +} + +bool isModeActivationConditionPresent(boxId_e modeId) +{ + UNUSED(modeId); + return false; +} + +textAttributes_t osdGetSystemMessage(char *message, size_t length, bool remove) +{ + UNUSED(length); + UNUSED(remove); + message[0] = 0x20; + message[1] = '\0'; + return 0; +} + +void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg) +{ + UNUSED(msg); + mavlinkRxHandleCalls++; +} + +static void testMspRebootPostProcess(serialPort_t *serialPort) +{ + mspRebootPostProcessCount++; + lastPostProcessPort = serialPort; +} + +mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) +{ + mspCommandCallCount++; + reply->cmd = cmd->cmd; + reply->flags = 0; + reply->result = 0; + + switch (cmd->cmd) { + case MSP_SET_PASSTHROUGH: + mspPassthroughDispatchCount++; + if (mspPostProcessFn) { + *mspPostProcessFn = testMspRebootPostProcess; + } + return MSP_RESULT_ACK; + case MSP_REBOOT: + if (mspPostProcessFn) { + *mspPostProcessFn = testMspRebootPostProcess; + } + return MSP_RESULT_ACK; + case testLargeReplyMspCommand: + for (uint16_t i = 0; i < 300; i++) { + sbufWriteU8(&reply->buf, (uint8_t)i); + } + return MSP_RESULT_ACK; + default: + return MSP_RESULT_ACK; + } +} + +adsbVehicleValues_t* getVehicleForFill(void) +{ + return NULL; +} + +void adsbNewVehicle(adsbVehicleValues_t *vehicle) +{ + UNUSED(vehicle); +} + +bool adsbHeartbeat(void) +{ + return false; +} + +uint8_t calculateBatteryPercentage(void) +{ + return 0; +} + +} diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 4f921e9612e..12b2671167b 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -79,6 +79,7 @@ extern SysTick_Type *SysTick; #define WS2811_DMA_TC_FLAG 1 #define WS2811_DMA_HANDLER_IDENTIFER 0 +#include "target/common.h" #include "target.h" #define FAST_CODE