ADSB selected the most dangerous vehicle by CPA calculation

.gitignore

@@ -46,6 +46,8 @@ launch.json
 # Assitnow token and files for test script
 tokens.yaml
 *.ubx
+/testing/
+/.idea/
 
 # Local development files
 .semgrepignore

dev/adsb/adsb_uart_sender.py

@@ -0,0 +1,82 @@
+import json
+import time
+import argparse
+import serial
+from pymavlink.dialects.v20 import common as mavlink2
+
+def load_aircraft(json_file):
+    with open(json_file, "r") as f:
+        return json.load(f)
+
+def create_mavlink(serial_port):
+    mav = mavlink2.MAVLink(serial_port)
+    mav.srcSystem = 1
+    mav.srcComponent = mavlink2.MAV_COMP_ID_ADSB
+    return mav
+
+def send_heartbeat(mav):
+    mav.heartbeat_send(
+        mavlink2.MAV_TYPE_ADSB,
+        mavlink2.MAV_AUTOPILOT_INVALID,
+        0,
+        0,
+        0
+    )
+
+def send_adsb(mav, aircraft):
+    for ac in aircraft:
+        icao = int(ac["icao_address"])
+        lat = int(ac["lat"] * 1e7)
+        lon = int(ac["lon"] * 1e7)
+        alt_mm = int(ac["altitude"] * 1000)
+        heading_cdeg = int(ac["heading"] * 100)
+        hor_vel_cms = int(ac["hor_velocity"] * 100)
+        ver_vel_cms = int(ac["ver_velocity"] * 100)
+        callsign = ac["callsign"].encode("ascii").ljust(9, b'\x00')
+
+        msg = mavlink2.MAVLink_adsb_vehicle_message(
+            ICAO_address=icao,
+            lat=lat,
+            lon=lon,
+            altitude_type=0,
+            altitude=alt_mm,
+            heading=heading_cdeg,
+            hor_velocity=hor_vel_cms,
+            ver_velocity=ver_vel_cms,
+            callsign=callsign,
+            emitter_type=0,
+            tslc=1,
+            flags=3,
+            squawk=0
+        )
+
+        mav.send(msg)
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("com_port")
+    parser.add_argument("json_file")
+    parser.add_argument("--baud", default=115200, type=int)
+    parser.add_argument("--rate", default=1.0, type=float)
+    args = parser.parse_args()
+
+    ser = serial.Serial(args.com_port, args.baud)
+    mav = create_mavlink(ser)
+
+    aircraft = load_aircraft(args.json_file)
+
+    period = 1.0 / args.rate
+    last_hb = 0
+
+    while True:
+        now = time.time()
+
+        if now - last_hb >= 1.0:
+            send_heartbeat(mav)
+            last_hb = now
+
+        send_adsb(mav, aircraft)
+        time.sleep(period)
+
+if __name__ == "__main__":
+    main()

dev/adsb/aircraft.json

@@ -0,0 +1,52 @@
+[
+{
+    "icao_address": 1002,
+    "lat": 49.2341564,
+    "lon": 16.5505527,
+    "altitude": 300,
+    "heading": 275,
+    "hor_velocity": 30,
+    "ver_velocity": 0,
+    "callsign": "V550"
+},
+{
+    "icao_address": 1001,
+    "lat": 49.2344299,
+    "lon": 16.5610206,
+    "altitude": 300,
+    "heading": 237,
+    "hor_velocity": 30,
+    "ver_velocity": 0,
+    "callsign": "V250"
+},
+{
+    "icao_address": 1003,
+    "lat": 49.2422463,
+    "lon": 16.5645653,
+    "altitude": 300,
+    "heading": 29,
+    "hor_velocity": 30,
+    "ver_velocity": 0,
+    "callsign": "V1100"
+},
+{
+    "icao_address": 1004,
+    "lat": 49.2377083,
+    "lon": 16.5520834,
+    "altitude": 300,
+    "heading": 110,
+    "hor_velocity": 30,
+    "ver_velocity": 0,
+    "callsign": "V650X3"
+},
+{
+    "icao_address": 1005,
+    "lat": 49.2388158,
+    "lon": 16.5730266,
+    "altitude": 300,
+    "heading": 261,
+    "hor_velocity": 30,
+    "ver_velocity": 0,
+    "callsign": "V1250X4"
+}
+]

dev/adsb/readme.md

@@ -0,0 +1,87 @@
+# ADSB Vehicle MAVLink Simulator
+
+A Python tool for simulating ADS-B aircraft traffic over a serial connection using the MAVLink protocol. Useful for testing ADS-B receivers, ground station software, or flight controller integrations without real aircraft.
+
+---
+
+## Requirements
+```bash
+pip install pymavlink pyserial
+```
+
+---
+
+## Usage
+```bash
+python adsb_sim.py <com_port> <json_file> [--baud BAUD] [--rate RATE]
+```
+
+### Arguments
+
+| Argument | Required | Default | Description |
+|---|---|---|---|
+| `com_port` | ✅ | — | Serial port (e.g. `COM3`, `/dev/ttyUSB0`) |
+| `json_file` | ✅ | — | Path to JSON file with aircraft definitions |
+| `--baud` | ❌ | `115200` | Serial baud rate |
+| `--rate` | ❌ | `1.0` | Update rate in Hz |
+
+### Examples
+```bash
+# Windows
+python adsb_sim.py COM3 aircraft.json
+
+# Linux
+python adsb_sim.py /dev/ttyUSB0 aircraft.json --baud 57600 --rate 2.0
+```
+
+---
+
+## Aircraft JSON Format
+
+Each aircraft is defined as an object in a JSON array:
+```json
+[
+  {
+    "icao_address": 1001,
+    "lat": 49.2344299,
+    "lon": 16.5610206,
+    "altitude": 300,
+    "heading": 237,
+    "hor_velocity": 30,
+    "ver_velocity": 0,
+    "callsign": "V250"
+  }
+]
+```
+
+| Field | Type | Unit | Description |
+|---|---|---|---|
+| `icao_address` | int | — | Unique ICAO identifier |
+| `lat` | float | degrees | Latitude |
+| `lon` | float | degrees | Longitude |
+| `altitude` | float | meters ASL | Altitude above sea level |
+| `heading` | float | degrees (0–360) | Track heading |
+| `hor_velocity` | float | m/s | Horizontal speed |
+| `ver_velocity` | float | m/s | Vertical speed (positive = climb) |
+| `callsign` | string | — | Aircraft callsign (max 8 chars) |
+
+---
+
+## How It Works
+
+The simulator connects to a serial port and continuously transmits two MAVLink message types:
+
+- **HEARTBEAT** — sent once per second to identify the component as an ADS-B device
+- **ADSB_VEHICLE** — sent for each aircraft at the configured rate, containing position, velocity, heading and identification data
+
+All aircraft from the JSON file are broadcast in every update cycle. The positions are static — aircraft do not move between updates.
+
+---
+
+## Notes
+
+- Altitude is transmitted in millimeters internally (`altitude * 1000`) as required by the MAVLink `ADSB_VEHICLE` message spec
+- Heading is transmitted in centidegrees (`heading * 100`)
+- Callsigns are ASCII-encoded and null-padded to 9 bytes
+- `flags` is set to `3` (lat/lon and altitude valid)
+- `tslc` (time since last communication) is hardcoded to `1` second

docs/ADSB.md

@@ -63,3 +63,41 @@ AT+SETTINGS=SAVE
 * in INAV configurator ports TAB set telemetry MAVLINK, and baudrate 115200
 * https://pantsforbirds.com/adsbee-1090/quick-start/
 
+---
+
+---
+
+## Alert and Warning
+The ADS-B warning/alert system supports two operating modes, controlled by the parameter adsb_calculation_use_cpa (ON or OFF).
+
+---
+
+### ADS-B Warning and Alert Messages (CPA Mode OFF)
+The ADS-B warning/alert system supports two operating modes, controlled by the parameter **adsb_calculation_use_cpa** (ON or OFF).
+
+When **adsb_calculation_use_cpa = OFF**, the system evaluates only the **current distance between the aircraft and the UAV**. The aircraft with the **shortest distance** is always selected for monitoring.
+
+- If the aircraft enters the **warning zone** (`adsb_distance_warning`), the corresponding **OSD element is displayed**.
+- If the aircraft enters the **alert zone** (`adsb_distance_alert`), the **OSD element starts blinking**, indicating a higher-priority alert.
+
+This mode therefore provides a simple proximity-based warning determined purely by real-time distance.
+
+---
+
+### ADS-B Warning and Alert Messages (CPA Mode ON)
+
+When **adsb_calculation_use_cpa = ON**, the system evaluates aircraft using the **Closest Point of Approach (CPA)** and predicted trajectories, not only the current distance.
+
+1. **Aircraft already inside the alert zone**  
+   If one or more aircraft are currently inside the **alert zone** (`adsb_distance_alert`), the **closest aircraft** to the UAV is selected and the **OSD element blinks**.
+
+2. **Aircraft in the warning zone, none predicted to enter the alert zone**  
+   If aircraft are present in the **warning zone** (`adsb_distance_warning`), but none of them are predicted to enter the **alert zone** (their CPA distance is greater than `adsb_distance_alert`), the **closest aircraft to the UAV** is selected and the **OSD element remains steady** (no blinking).
+
+3. **Aircraft in the warning zone, one predicted to enter the alert zone**  
+   If at least one aircraft in the **warning zone** is predicted to enter the **alert zone**, that aircraft is selected and the **OSD element blinks**.
+
+4. **Aircraft in the warning zone, multiple predicted to enter the alert zone**  
+   If multiple aircraft are predicted to enter the **alert zone**, the system selects the aircraft that will **reach the alert zone first**, and the **OSD element blinks**.
+
+![ADSB CPA_ON](assets/images/adsb-CPA-on.png)

docs/Settings.md

@@ -4482,6 +4482,16 @@ Optical flow module scale factor
 
 ---
 
+### osd_adsb_calculation_use_cpa
+
+Use CPA (Closest Point of Approach) method for ADS-B traffic distance and collision risk calculation instead of instantaneous distance.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### osd_adsb_distance_alert
 
 Distance inside which ADSB data flashes for proximity warning

src/main/CMakeLists.txt

@@ -358,6 +358,7 @@ main_sources(COMMON_SRC
     flight/adaptive_filter.h
 
     io/adsb.c
+    io/adsb.h
     io/beeper.c
     io/beeper.h
     io/servo_sbus.c

src/main/fc/fc_msp.c

@@ -1040,6 +1040,49 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU8(dst, 0);
         sbufWriteU32(dst, 0);
         sbufWriteU32(dst, 0);
+#endif
+        break;
+    case MSP2_ADSB_LIMITS:
+#ifdef USE_ADSB
+        sbufWriteU16(dst, osdConfig()->adsb_distance_warning);
+        sbufWriteU16(dst, osdConfig()->adsb_distance_alert);
+        sbufWriteU16(dst, osdConfig()->adsb_ignore_plane_above_me_limit);
+#else
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, 0);
+#endif
+        break;
+    case MSP2_ADSB_WARNING_VEHICLE_ICAO:
+#ifdef USE_ADSB
+        if(isEnvironmentOkForCalculatingADSBDistanceBearing()) {
+            adsbVehicle_t *vehicle = NULL;
+            bool isAlert = true;
+            vehicle = findVehicleForAlert(
+                    METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_alert),
+                    METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning),
+                    METERS_TO_CENTIMETERS(osdConfig()->adsb_ignore_plane_above_me_limit)
+            );
+
+            if(vehicle == NULL) {
+                vehicle = findVehicleForWarning(METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning), METERS_TO_CENTIMETERS(osdConfig()->adsb_ignore_plane_above_me_limit));
+                isAlert = false;
+            }
+
+            if(vehicle != NULL) {
+                sbufWriteU32(dst, vehicle->vehicleValues.icao);
+                sbufWriteU8(dst, isAlert ? 1 : 0);;
+            } else {
+                sbufWriteU32(dst, 0);
+                sbufWriteU8(dst, 0);;
+            }
+        } else {
+            sbufWriteU32(dst, 0);
+            sbufWriteU8(dst, 0);
+        }
+#else
+        sbufWriteU32(dst, 0);
+        sbufWriteU8(dst, 0);
 #endif
             break;
     case MSP_DEBUG:

src/main/fc/fc_tasks.c

@@ -187,14 +187,6 @@ void taskUpdateCompass(timeUs_t currentTimeUs)
 }
 #endif
 
-#ifdef USE_ADSB
-void taskAdsb(timeUs_t currentTimeUs)
-{
-    UNUSED(currentTimeUs);
-    adsbTtlClean(currentTimeUs);
-}
-#endif
-
 #ifdef USE_BARO
 void taskUpdateBaro(timeUs_t currentTimeUs)
 {
@@ -554,7 +546,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
         [TASK_ADSB] = {
         .taskName = "ADSB",
         .taskFunc = taskAdsb,
-        .desiredPeriod = TASK_PERIOD_HZ(1),      // ADSB is updated at 1 Hz
+        .desiredPeriod = TASK_PERIOD_MS(500),      // ADSB is updated at 1 Hz
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif

src/main/fc/settings.yaml

@@ -3690,6 +3690,12 @@ groups:
         field: adsb_warning_style
         type: uint8_t
         condition: USE_ADSB
+      - name: osd_adsb_calculation_use_cpa
+        description: "Use CPA (Closest Point of Approach) method for ADS-B traffic distance and collision risk calculation instead of instantaneous distance."
+        default_value: OFF
+        field: adsb_calculation_use_cpa
+        type: bool
+        condition: USE_ADSB
       - name: osd_estimations_wind_compensation
         description: "Use wind estimation for remaining flight time/distance estimation"
         default_value: ON