src/main/io/adsb.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18
  19 #include <string.h>
  20
  21
  22 #include "adsb.h"
  23
  24 #include "navigation/navigation.h"
  25 #include "navigation/navigation_private.h"
  26
  27 #include "common/maths.h"
  28 #pragma GCC diagnostic push
  29 #pragma GCC diagnostic ignored "-Wunused-function"
  30 #include "common/mavlink.h"
  31 #pragma GCC diagnostic pop
  32
  33
  34 #include "math.h"
  35
  36
  37 #ifdef USE_ADSB
  38
  39 adsbVehicle_t adsbVehiclesList[MAX_ADSB_VEHICLES];
  40 adsbVehicleStatus_t adsbVehiclesStatus;
  41
  42 adsbVehicleValues_t vehicleValues;
  43
  44
  45 adsbVehicleValues_t* getVehicleForFill(void){
  46     return &vehicleValues;
  47 }
  48
  49 // use bsearch function
  50 adsbVehicle_t *findVehicleByIcao(uint32_t avicao) {
  51     for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
  52         if (avicao == adsbVehiclesList[i].vehicleValues.icao) {
  53             return &adsbVehiclesList[i];
  54         }
  55     }
  56     return NULL;
  57 }
  58
  59 adsbVehicle_t *findVehicleFarthest(void) {
  60     adsbVehicle_t *adsbLocal = NULL;
  61     for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
  62         if (adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist < adsbVehiclesList[i].calculatedVehicleValues.dist)) {
  63             adsbLocal = &adsbVehiclesList[i];
  64         }
  65     }
  66     return adsbLocal;
  67 }
  68
  69 uint8_t getActiveVehiclesCount(void) {
  70     uint8_t total = 0;
  71     for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
  72         if (adsbVehiclesList[i].ttl > 0) {
  73             total++;
  74         }
  75     }
  76     return total;
  77 }
  78
  79 adsbVehicle_t *findVehicleClosest(void) {
  80     adsbVehicle_t *adsbLocal = NULL;
  81     for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
  82         if (adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid && (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist > adsbVehiclesList[i].calculatedVehicleValues.dist)) {
  83             adsbLocal = &adsbVehiclesList[i];
  84         }
  85     }
  86     return adsbLocal;
  87 }
  88
  89 adsbVehicle_t *findFreeSpaceInList(void) {
  90     //find expired first
  91     for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
  92         if (adsbVehiclesList[i].ttl == 0) {
  93             return &adsbVehiclesList[i];
  94         }
  95     }
  96
  97     return NULL;
  98 }
  99
 100 adsbVehicle_t *findVehicleNotCalculated(void) {
 101     //find expired first
 102     for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
 103         if (adsbVehiclesList[i].calculatedVehicleValues.valid == false) {
 104             return &adsbVehiclesList[i];
 105         }
 106     }
 107
 108     return NULL;
 109 }
 110
 111 adsbVehicle_t* findVehicle(uint8_t index)
 112 {
 113     if (index < MAX_ADSB_VEHICLES){
 114         return &adsbVehiclesList[index];
 115     }
 116
 117     return NULL;
 118 }
 119
 120 adsbVehicleStatus_t* getAdsbStatus(void){
 121     return &adsbVehiclesStatus;
 122 }
 123
 124 void gpsDistanceCmBearing(int32_t currentLat1, int32_t currentLon1, int32_t destinationLat2, int32_t destinationLon2, uint32_t *dist, int32_t *bearing) {
 125     float GPS_scaleLonDown = cos_approx((fabsf((float) gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f);
 126     const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees
 127     const float dLon = (float) (destinationLon2 - currentLon1) * GPS_scaleLonDown;
 128
 129     *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
 130     *bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon));      // Convert the output radians to 100xdeg
 131     *bearing = wrap_36000(*bearing);
 132 };
 133
 134 void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) {
 135
 136     // no valid lat lon or altitude
 137     if((vehicleValuesLocal->flags & (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS)) != (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS)){
 138         return;
 139     }
 140
 141     adsbVehiclesStatus.vehiclesMessagesTotal++;
 142     adsbVehicle_t *vehicle = NULL;
 143
 144     vehicle = findVehicleByIcao(vehicleValuesLocal->icao);
 145     if(vehicle != NULL && vehicleValuesLocal->tslc > ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST){
 146         vehicle->ttl = 0;
 147         return;
 148     }
 149
 150     // non GPS mode, GPS is not fix, just find free space in list or by icao and save vehicle without calculated values
 151     if (!enviromentOkForCalculatingDistaceBearing()) {
 152
 153         if(vehicle == NULL){
 154             vehicle = findFreeSpaceInList();
 155         }
 156
 157         if (vehicle != NULL) {
 158             memcpy(&(vehicle->vehicleValues), vehicleValuesLocal, sizeof(vehicle->vehicleValues));
 159             vehicle->ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST;
 160             vehicle->calculatedVehicleValues.valid = false;
 161             return;
 162         }
 163     } else {
 164         // GPS mode, GPS is fixed and has enough sats
 165
 166
 167         if(vehicle == NULL){
 168             vehicle = findFreeSpaceInList();
 169         }
 170
 171         if(vehicle == NULL){
 172             vehicle = findVehicleNotCalculated();
 173         }
 174
 175         if(vehicle == NULL){
 176             vehicle = findVehicleFarthest();
 177         }
 178
 179         if (vehicle != NULL) {
 180             memcpy(&(vehicle->vehicleValues), vehicleValuesLocal, sizeof(vehicle->vehicleValues));
 181             recalculateVehicle(vehicle);
 182             vehicle->ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST;
 183             return;
 184         }
 185     }
 186 };
 187
 188 void recalculateVehicle(adsbVehicle_t* vehicle){
 189     gpsDistanceCmBearing(gpsSol.llh.lat, gpsSol.llh.lon, vehicle->vehicleValues.lat, vehicle->vehicleValues.lon, &(vehicle->calculatedVehicleValues.dist), &(vehicle->calculatedVehicleValues.dir));
 190
 191     if (vehicle != NULL && vehicle->calculatedVehicleValues.dist > ADSB_LIMIT_CM) {
 192         vehicle->ttl = 0;
 193         return;
 194     }
 195
 196     vehicle->calculatedVehicleValues.verticalDistance = vehicle->vehicleValues.alt - (int32_t)getEstimatedActualPosition(Z) - GPS_home.alt;
 197     vehicle->calculatedVehicleValues.valid = true;
 198 }
 199
 200 void adsbTtlClean(timeUs_t currentTimeUs) {
 201
 202     static timeUs_t adsbTtlLastCleanServiced = 0;
 203     timeDelta_t adsbTtlSinceLastCleanServiced = cmpTimeUs(currentTimeUs, adsbTtlLastCleanServiced);
 204
 205
 206     if (adsbTtlSinceLastCleanServiced > 1000000) // 1s
 207     {
 208         for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
 209             if (adsbVehiclesList[i].ttl > 0) {
 210                 adsbVehiclesList[i].ttl--;
 211             }
 212         }
 213
 214         adsbTtlLastCleanServiced = currentTimeUs;
 215     }
 216 };
 217
 218 bool enviromentOkForCalculatingDistaceBearing(void){
 219     return (STATE(GPS_FIX) && gpsSol.numSat > 4);
 220 }
 221
 222 #endif
 223