diff --git a/.gitignore b/.gitignore index 6d7138e6bd5..19ba4affd2e 100644 --- a/.gitignore +++ b/.gitignore @@ -46,6 +46,8 @@ launch.json # Assitnow token and files for test script tokens.yaml *.ubx +/testing/ +/.idea/ # Local development files .semgrepignore diff --git a/dev/adsb/adsb_uart_sender.py b/dev/adsb/adsb_uart_sender.py new file mode 100644 index 00000000000..9769ee396b7 --- /dev/null +++ b/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() diff --git a/dev/adsb/aircraft.json b/dev/adsb/aircraft.json new file mode 100644 index 00000000000..3ecac40691b --- /dev/null +++ b/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" +} +] diff --git a/dev/adsb/readme.md b/dev/adsb/readme.md new file mode 100644 index 00000000000..2f7c7de6248 --- /dev/null +++ b/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 [--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 \ No newline at end of file diff --git a/docs/ADSB.md b/docs/ADSB.md index 2e5bef69dfe..c1438471ce4 100644 --- a/docs/ADSB.md +++ b/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) \ No newline at end of file diff --git a/docs/Settings.md b/docs/Settings.md index 4e577bbedd5..72762392366 100644 --- a/docs/Settings.md +++ b/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 diff --git a/docs/assets/images/adsb-CPA-on.png b/docs/assets/images/adsb-CPA-on.png new file mode 100644 index 00000000000..af5e8b27903 Binary files /dev/null and b/docs/assets/images/adsb-CPA-on.png differ diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 101c7223372..3689f94c9fb 100755 --- a/src/main/CMakeLists.txt +++ b/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 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 45379d1caa4..e6397f5aecf 100644 --- a/src/main/fc/fc_msp.c +++ b/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: diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 4ca125d5c6a..6fd71db5f59 100755 --- a/src/main/fc/fc_tasks.c +++ b/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 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1e1932531e5..bdc32b2963d 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/io/adsb.c b/src/main/io/adsb.c index 34ed1183e00..2bf5b3ca36f 100644 --- a/src/main/io/adsb.c +++ b/src/main/io/adsb.c @@ -21,144 +21,247 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see http://www.gnu.org/licenses/. */ - - #include - - #include "adsb.h" +#ifdef USE_ADSB + #include "navigation/navigation.h" #include "navigation/navigation_private.h" #include "common/maths.h" +#include "math.h" + #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" #include "common/mavlink.h" #pragma GCC diagnostic pop +#include "io/osd.h" +adsbVehicle_t adsbVehiclesDictionary[MAX_ADSB_VEHICLES]; +adsbVehicleStatus_t adsbVehiclesStatus; -#include "math.h" +//buffer for fill data from mavlink +adsbVehicleValues_t vehicleValues; +adsbVehicleValues_t* getVehicleForFill(void){ + return &vehicleValues; +} -#ifdef USE_ADSB +static void calculateCPA(fpVector3_t vehicleVector, float ground_speed, float heading_deg, float* t_cpa, float* d_min) { + float pos_x = vehicleVector.x * 0.01f; + float pos_y = vehicleVector.y * 0.01f; -adsbVehicle_t adsbVehiclesList[MAX_ADSB_VEHICLES]; -adsbVehicleStatus_t adsbVehiclesStatus; + float heading_rad = heading_deg * (float)M_PI / 180.0f; -adsbVehicleValues_t vehicleValues; + float v_x = ground_speed * cos_approx(heading_rad); + float v_y = ground_speed * sin_approx(heading_rad); + float v_sq = v_x * v_x + v_y * v_y; + if (v_sq < 1e-6f) { + *t_cpa = 0.0f; + *d_min = sqrtf(pos_x * pos_x + pos_y * pos_y); + return; + } -adsbVehicleValues_t* getVehicleForFill(void){ - return &vehicleValues; + float dot = pos_x * v_x + pos_y * v_y; + + float t = -dot / v_sq; + if (t < 0.0f) { + t = 0.0f; + } + + *t_cpa = t; + + float closest_x = pos_x + v_x * t; + float closest_y = pos_y + v_y * t; + + *d_min = sqrtf(closest_x * closest_x + closest_y * closest_y); +} + + +static void calculateMeetPoint(adsbVehicle_t *vehicle) { + + fpVector3_t pos; + gpsOrigin_t uavPosition = { + .alt = gpsSol.llh.alt, + .lat = gpsSol.llh.lat, + .lon = gpsSol.llh.lon, + .scale = posControl.gpsOrigin.scale, + .valid = posControl.gpsOrigin.valid, + }; + geoConvertGeodeticToLocal(&pos, &uavPosition, &vehicle->vehicleValues.gps, GEO_ALT_RELATIVE); + + float t_cpa; + float d_min; + + calculateCPA( + pos, + ((float)vehicle->vehicleValues.horVelocity) * 0.01f, // cm/s -> m/s (bez integer dělení) + CENTIDEGREES_TO_DEGREES(vehicle->vehicleValues.heading), + &t_cpa, + &d_min + ); + + vehicle->calculatedVehicleValues.meetPointDistance = (int)d_min; + vehicle->calculatedVehicleValues.meetPointTime = (int)t_cpa; +} + +static void recalculateVehicle(adsbVehicle_t* vehicle) { + if(vehicle->ttl == 0){ + return; + } + + fpVector3_t vehicleVector; + geoConvertGeodeticToLocal(&vehicleVector, &posControl.gpsOrigin, &vehicle->vehicleValues.gps, GEO_ALT_RELATIVE); + + vehicle->calculatedVehicleValues.dist = calculateDistanceToDestination(&vehicleVector); + vehicle->calculatedVehicleValues.dir = calculateBearingToDestination(&vehicleVector); + + if (vehicle->calculatedVehicleValues.dist > ADSB_LIMIT_CM) { + vehicle->ttl = 0; + vehicle->calculatedVehicleValues.valid = false; + return; + } + + if(osdConfig()->adsb_calculation_use_cpa){ + calculateMeetPoint(vehicle); + } + + vehicle->calculatedVehicleValues.verticalDistance = vehicle->vehicleValues.alt - (int32_t)getEstimatedActualPosition(Z) - GPS_home.alt; + vehicle->calculatedVehicleValues.valid = true; } // use bsearch function -adsbVehicle_t *findVehicleByIcao(uint32_t avicao) { +static adsbVehicle_t *findVehicleByIcao(uint32_t avicao) { for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { - if (avicao == adsbVehiclesList[i].vehicleValues.icao) { - return &adsbVehiclesList[i]; + if (avicao == adsbVehiclesDictionary[i].vehicleValues.icao) { + return &adsbVehiclesDictionary[i]; } } return NULL; } -adsbVehicle_t *findVehicleFarthest(void) { +static adsbVehicle_t *findVehicleFarthest(void) { adsbVehicle_t *adsbLocal = NULL; for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { - if (adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist < adsbVehiclesList[i].calculatedVehicleValues.dist)) { - adsbLocal = &adsbVehiclesList[i]; + if (adsbVehiclesDictionary[i].ttl > 0 && adsbVehiclesDictionary[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist < adsbVehiclesDictionary[i].calculatedVehicleValues.dist)) { + adsbLocal = &adsbVehiclesDictionary[i]; } } return adsbLocal; } -uint8_t getActiveVehiclesCount(void) { - uint8_t total = 0; +static adsbVehicle_t *findFreeSpaceInList(void) { + //find expired first for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { - if (adsbVehiclesList[i].ttl > 0) { - total++; + if (adsbVehiclesDictionary[i].ttl == 0) { + return &adsbVehiclesDictionary[i]; } } - return total; + + return NULL; } -adsbVehicle_t *findVehicleClosest(void) { - adsbVehicle_t *adsbLocal = NULL; +static adsbVehicle_t *findVehicleNotCalculated(void) { + //find expired first for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { - if (adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist > adsbVehiclesList[i].calculatedVehicleValues.dist)) { - adsbLocal = &adsbVehiclesList[i]; + if (adsbVehiclesDictionary[i].calculatedVehicleValues.valid == false) { + return &adsbVehiclesDictionary[i]; } } - return adsbLocal; -} -/** - * find the closest vehicle, apply filter max verticalDistance - * @return - */ -adsbVehicle_t *findVehicleClosestLimit(int32_t maxVerticalDistance) { - - //////////////////////////////////////////////////////////// - //debug vehicle - /*adsbVehicleValues_t* vehicle = getVehicleForFill(); - if(vehicle != NULL){ - char name[9] = "DUMMY "; - vehicle->icao = 666; - vehicle->gps.lat = 492383514; - vehicle->gps.lon = 165148681; - vehicle->alt = 100000; - vehicle->heading = 66; - vehicle->flags = ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS; - vehicle->altitudeType = 0; - memcpy(&(vehicle->callsign), name, sizeof(vehicle->callsign)); - vehicle->emitterType = 6; - vehicle->tslc = 0; - adsbNewVehicle(vehicle); - }*/ - //////////////////////////////////////////////////////////// + return NULL; +} +adsbVehicle_t *findVehicleForWarning(uint32_t warningDistanceCm, int32_t maxVerticalDistance) { adsbVehicle_t *adsbLocal = NULL; + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { - if(adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid){ - if(adsbVehiclesList[i].calculatedVehicleValues.verticalDistance > 0 && maxVerticalDistance > 0 && adsbVehiclesList[i].calculatedVehicleValues.verticalDistance > maxVerticalDistance){ - continue; - } - if (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist > adsbVehiclesList[i].calculatedVehicleValues.dist) { - adsbLocal = &adsbVehiclesList[i]; - } + adsbVehicle_t *vehicle = &adsbVehiclesDictionary[i]; + + //only active vehicles + if (vehicle->ttl == 0 || !vehicle->calculatedVehicleValues.valid) { + continue; + } + + //it's too high + if (vehicle->calculatedVehicleValues.verticalDistance > 0 && maxVerticalDistance > 0 && + vehicle->calculatedVehicleValues.verticalDistance > maxVerticalDistance) { + continue; + } + + // allow dist == 0 to be considered valid (closest possible) + if (vehicle->calculatedVehicleValues.dist >= warningDistanceCm) { + continue; + } + + if (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist > vehicle->calculatedVehicleValues.dist) { + adsbLocal = vehicle; } } + return adsbLocal; } -adsbVehicle_t *findFreeSpaceInList(void) { - //find expired first +adsbVehicle_t *findVehicleForAlert(uint32_t alertDistanceCm, uint32_t warningDistanceCm, int32_t maxVerticalDistance) { + adsbVehicle_t *best = NULL; + int32_t bestTimeToAlert = INT32_MAX; + uint32_t bestDist = UINT32_MAX; + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { - if (adsbVehiclesList[i].ttl == 0) { - return &adsbVehiclesList[i]; + adsbVehicle_t *vehicle = &adsbVehiclesDictionary[i]; + int32_t timeToAlert = -1; + + if (vehicle->ttl == 0 || !vehicle->calculatedVehicleValues.valid) { + continue; + } + + if (vehicle->calculatedVehicleValues.verticalDistance > 0 && maxVerticalDistance > 0 && + vehicle->calculatedVehicleValues.verticalDistance > maxVerticalDistance) { + continue; + } + + if (vehicle->calculatedVehicleValues.dist <= alertDistanceCm) { + timeToAlert = 0; + } else if (osdConfig()->adsb_calculation_use_cpa && + vehicle->calculatedVehicleValues.dist <= warningDistanceCm && + vehicle->calculatedVehicleValues.meetPointTime >= 0 && + vehicle->calculatedVehicleValues.meetPointDistance > 0) { + const uint32_t meetPointDistanceCm = ((uint32_t)vehicle->calculatedVehicleValues.meetPointDistance) * 100u; + if (meetPointDistanceCm > alertDistanceCm) { + continue; + } + timeToAlert = vehicle->calculatedVehicleValues.meetPointTime; + } else { + continue; + } + + if (best == NULL || timeToAlert < bestTimeToAlert || (timeToAlert == bestTimeToAlert && vehicle->calculatedVehicleValues.dist < bestDist)) { + best = vehicle; + bestTimeToAlert = timeToAlert; + bestDist = vehicle->calculatedVehicleValues.dist; } } - return NULL; + return best; } -adsbVehicle_t *findVehicleNotCalculated(void) { - //find expired first +uint8_t getActiveVehiclesCount(void) { + uint8_t total = 0; for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { - if (adsbVehiclesList[i].calculatedVehicleValues.valid == false) { - return &adsbVehiclesList[i]; + if (adsbVehiclesDictionary[i].ttl > 0) { + total++; } } - - return NULL; + return total; } adsbVehicle_t* findVehicle(uint8_t index) { if (index < MAX_ADSB_VEHICLES){ - return &adsbVehiclesList[index]; + return &adsbVehiclesDictionary[index]; } return NULL; @@ -240,50 +343,8 @@ void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) { return; } } -}; - -void recalculateVehicle(adsbVehicle_t* vehicle){ - if(vehicle->ttl == 0){ - return; - } - - fpVector3_t vehicleVector; - geoConvertGeodeticToLocal(&vehicleVector, &posControl.gpsOrigin, &vehicle->vehicleValues.gps, GEO_ALT_RELATIVE); - - vehicle->calculatedVehicleValues.dist = calculateDistanceToDestination(&vehicleVector); - vehicle->calculatedVehicleValues.dir = calculateBearingToDestination(&vehicleVector); - - if (vehicle->calculatedVehicleValues.dist > ADSB_LIMIT_CM) { - vehicle->ttl = 0; - return; - } - - vehicle->calculatedVehicleValues.verticalDistance = vehicle->vehicleValues.alt - (int32_t)getEstimatedActualPosition(Z) - GPS_home.alt; - vehicle->calculatedVehicleValues.valid = true; } -void adsbTtlClean(timeUs_t currentTimeUs) { - - static timeUs_t adsbTtlLastCleanServiced = 0; - timeDelta_t adsbTtlSinceLastCleanServiced = cmpTimeUs(currentTimeUs, adsbTtlLastCleanServiced); - - - if (adsbTtlSinceLastCleanServiced > 1000000) // 1s - { - for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { - if (adsbVehiclesList[i].ttl > 0) { - adsbVehiclesList[i].ttl--; - } - - if (adsbVehiclesList[i].ttl > 0) { - recalculateVehicle(&adsbVehiclesList[i]); - } - } - - adsbTtlLastCleanServiced = currentTimeUs; - } -}; - bool isEnvironmentOkForCalculatingADSBDistanceBearing(void){ return (gpsSol.numSat > 4 && @@ -296,5 +357,25 @@ bool isEnvironmentOkForCalculatingADSBDistanceBearing(void){ ); } +void taskAdsb(timeUs_t currentTimeUs){ + static timeUs_t adsbTtlLastCleanServiced = 0; + timeDelta_t adsbTtlSinceLastCleanServiced = cmpTimeUs(currentTimeUs, adsbTtlLastCleanServiced); + + const bool shouldDecrementTtl = (adsbTtlSinceLastCleanServiced > 1000000); // 1s + + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if (adsbVehiclesDictionary[i].ttl > 0) { + if (shouldDecrementTtl) { + adsbVehiclesDictionary[i].ttl--; + } + recalculateVehicle(&adsbVehiclesDictionary[i]); + } + } + + if (shouldDecrementTtl) { + adsbTtlLastCleanServiced = currentTimeUs; + } +} + #endif diff --git a/src/main/io/adsb.h b/src/main/io/adsb.h index bf530e14a5e..501bd207ad7 100644 --- a/src/main/io/adsb.h +++ b/src/main/io/adsb.h @@ -1,20 +1,26 @@ /* - * This file is part of Cleanflight. + * This file is part of INAV Project. * - * 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. + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. * - * 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. + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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. + * + * This file 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 . + * along with this program. If not, see http://www.gnu.org/licenses/. */ - #pragma once #include @@ -30,6 +36,8 @@ typedef struct { int32_t dir; // centidegrees direction to plane, pivot is inav FC uint32_t dist; // horisontal distance to plane, cm, pivot is inav FC int32_t verticalDistance; // vertical distance to plane, cm, pivot is inav FC + int32_t meetPointDistance; // meters, Closest point + int32_t meetPointTime; // seconds, Time of the closest approach } adsbVehicleCalculatedValues_t; typedef struct { @@ -58,11 +66,12 @@ typedef struct { void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal); bool adsbHeartbeat(void); -adsbVehicle_t *findVehicleClosestLimit(int32_t maxVerticalDistance); adsbVehicle_t * findVehicle(uint8_t index); uint8_t getActiveVehiclesCount(void); -void adsbTtlClean(timeUs_t currentTimeUs); adsbVehicleStatus_t* getAdsbStatus(void); adsbVehicleValues_t* getVehicleForFill(void); bool isEnvironmentOkForCalculatingADSBDistanceBearing(void); -void recalculateVehicle(adsbVehicle_t* vehicle); \ No newline at end of file +void taskAdsb(timeUs_t currentTimeUs); + +adsbVehicle_t *findVehicleForWarning(uint32_t warningDistanceCm, int32_t maxVerticalDistance); +adsbVehicle_t *findVehicleForAlert(uint32_t alertDistanceCm, uint32_t warningDistanceCm, int32_t maxVerticalDistance); \ No newline at end of file diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 6ad55632c17..73fdfb75c3a 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -224,7 +224,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3); void osdStartedSaveProcess(void) { @@ -2266,18 +2266,24 @@ static bool osdDrawSingleElement(uint8_t item) uint8_t buffIndexFirstLine = 0; uint8_t arrowIndexIndex = 0; - adsbVehicle_t *vehicle = findVehicleClosestLimit(METERS_TO_CENTIMETERS(osdConfig()->adsb_ignore_plane_above_me_limit)); - if (vehicle != NULL) { - recalculateVehicle(vehicle); - } + bool isAlert = true; + + adsbVehicle_t *vehicle = NULL; + + if(isEnvironmentOkForCalculatingADSBDistanceBearing()){ + 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->calculatedVehicleValues.dist > 0 - && vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning)) - && isEnvironmentOkForCalculatingADSBDistanceBearing() + 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) { adsbLengthForClearFirstLine = 11; buff[buffIndexFirstLine++] = SYM_ADSB; @@ -2301,7 +2307,7 @@ static bool osdDrawSingleElement(uint8_t item) buffIndexFirstLine += osdConfig()->decimals_altitude + 2; ////////////////////////////////////////////////////// - if (vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_alert)) { + if (isAlert) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -4281,6 +4287,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .adsb_distance_alert = SETTING_OSD_ADSB_DISTANCE_ALERT_DEFAULT, .adsb_ignore_plane_above_me_limit = SETTING_OSD_ADSB_IGNORE_PLANE_ABOVE_ME_LIMIT_DEFAULT, .adsb_warning_style = SETTING_OSD_ADSB_WARNING_STYLE_DEFAULT, + .adsb_calculation_use_cpa = SETTING_OSD_ADSB_CALCULATION_USE_CPA_DEFAULT, #endif #if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index bbaa68f862d..89fc5f90d39 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -533,7 +533,8 @@ typedef struct osdConfig_s { uint16_t adsb_distance_warning; // in metres uint16_t adsb_distance_alert; // in metres uint16_t adsb_ignore_plane_above_me_limit; // in metres - osd_adsb_warning_style_e adsb_warning_style; // adsb warning element style, one or two lines + osd_adsb_warning_style_e adsb_warning_style; // adsb warning element style, one or two lines + bool adsb_calculation_use_cpa; // adsb calculation type, the closest or the closest approach #endif uint8_t radar_peers_display_time; // in seconds #ifdef USE_GEOZONE diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 5a3af115f9c..b2a5923b41a 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 // Same version over MSPv1 & MSPv2 - message format didn't change and it backward compatible #define API_VERSION_MAJOR 2 // increment when major changes are made -#define API_VERSION_MINOR 5 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 6 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 0b893916895..54d9263d196 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -111,6 +111,8 @@ #define MSP2_INAV_SELECT_MIXER_PROFILE 0x2080 #define MSP2_ADSB_VEHICLE_LIST 0x2090 +#define MSP2_ADSB_LIMITS 0x2091 +#define MSP2_ADSB_WARNING_VEHICLE_ICAO 0x2092 #define MSP2_INAV_CUSTOM_OSD_ELEMENTS 0x2100 #define MSP2_INAV_CUSTOM_OSD_ELEMENT 0x2101 @@ -126,3 +128,4 @@ #define MSP2_INAV_SET_GVAR 0x2214 #define MSP2_INAV_FULL_LOCAL_POSE 0x2220 +