11/*
2- * This file is part of Cleanflight .
2+ * This file is part of INAV Project .
33 *
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.
4+ * This Source Code Form is subject to the terms of the Mozilla Public
5+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
6+ * You can obtain one at http://mozilla.org/MPL/2.0/.
87 *
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.
8+ * Alternatively, the contents of this file may be used under the terms
9+ * of the GNU General Public License Version 3, as described below:
10+ *
11+ * This file is free software: you may copy, redistribute and/or modify
12+ * it under the terms of the GNU General Public License as published by the
13+ * Free Software Foundation, either version 3 of the License, or (at your
14+ * option) any later version.
15+ *
16+ * This file is distributed in the hope that it will be useful, but
17+ * WITHOUT ANY WARRANTY; without even the implied warranty of
18+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
19+ * Public License for more details.
1320 *
1421 * You should have received a copy of the GNU General Public License
15- * along with Cleanflight. If not, see < http://www.gnu.org/licenses/> .
22+ * along with this program. If not, see http://www.gnu.org/licenses/.
1623 */
1724
1825
@@ -86,6 +93,26 @@ adsbVehicle_t *findVehicleClosest(void) {
8693 return adsbLocal ;
8794}
8895
96+ /**
97+ * find the closest vehicle, apply filter max verticalDistance
98+ * @return
99+ */
100+ adsbVehicle_t * findVehicleClosestLimit (int32_t maxVerticalDistance ) {
101+ adsbVehicle_t * adsbLocal = NULL ;
102+ for (uint8_t i = 0 ; i < MAX_ADSB_VEHICLES ; i ++ ) {
103+ if (adsbVehiclesList [i ].ttl > 0 && adsbVehiclesList [i ].calculatedVehicleValues .valid ){
104+ if (adsbVehiclesList [i ].calculatedVehicleValues .verticalDistance > 0 && maxVerticalDistance > 0 && adsbVehiclesList [i ].calculatedVehicleValues .verticalDistance > maxVerticalDistance ){
105+ continue ;
106+ }
107+
108+ if (adsbLocal == NULL || adsbLocal -> calculatedVehicleValues .dist > adsbVehiclesList [i ].calculatedVehicleValues .dist ) {
109+ adsbLocal = & adsbVehiclesList [i ];
110+ }
111+ }
112+ }
113+ return adsbLocal ;
114+ }
115+
89116adsbVehicle_t * findFreeSpaceInList (void ) {
90117 //find expired first
91118 for (uint8_t i = 0 ; i < MAX_ADSB_VEHICLES ; i ++ ) {
@@ -121,23 +148,18 @@ adsbVehicleStatus_t* getAdsbStatus(void){
121148 return & adsbVehiclesStatus ;
122149}
123150
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-
134151bool adsbHeartbeat (void ){
135152 adsbVehiclesStatus .heartbeatMessagesTotal ++ ;
136153 return true;
137154}
138155
139156void adsbNewVehicle (adsbVehicleValues_t * vehicleValuesLocal ) {
140157
158+ if (vehicleValuesLocal -> icao == 0 )
159+ {
160+ return ;
161+ }
162+
141163 // no valid lat lon or altitude
142164 if ((vehicleValuesLocal -> flags & (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS )) != (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS )){
143165 return ;
@@ -148,28 +170,27 @@ void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) {
148170 adsbVehicle_t * vehicle = NULL ;
149171
150172 vehicle = findVehicleByIcao (vehicleValuesLocal -> icao );
151- if (vehicle != NULL && vehicleValuesLocal -> tslc > ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST ){
152- vehicle -> ttl = 0 ;
173+ if (vehicleValuesLocal -> tslc > ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST ){
174+ if (vehicle != NULL ){
175+ vehicle -> ttl = 0 ;
176+ }
153177 return ;
154178 }
155179
156180 // non GPS mode, GPS is not fix, just find free space in list or by icao and save vehicle without calculated values
157181 if (!enviromentOkForCalculatingDistaceBearing ()) {
158-
159182 if (vehicle == NULL ){
160183 vehicle = findFreeSpaceInList ();
161184 }
162185
163186 if (vehicle != NULL ) {
164187 memcpy (& (vehicle -> vehicleValues ), vehicleValuesLocal , sizeof (vehicle -> vehicleValues ));
165- vehicle -> ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST ;
188+ vehicle -> ttl = MAX ( 0 , ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST - vehicleValuesLocal -> tslc ) ;
166189 vehicle -> calculatedVehicleValues .valid = false;
167190 return ;
168191 }
169192 } else {
170193 // GPS mode, GPS is fixed and has enough sats
171-
172-
173194 if (vehicle == NULL ){
174195 vehicle = findFreeSpaceInList ();
175196 }
@@ -180,21 +201,39 @@ void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) {
180201
181202 if (vehicle == NULL ){
182203 vehicle = findVehicleFarthest ();
204+ if (vehicle != NULL )
205+ {
206+ //calculate distance to new vehicle, we need to compare if new vehicle is closer than the farthest plane
207+ fpVector3_t vehicleVector ;
208+ geoConvertGeodeticToLocal (& vehicleVector , & posControl .gpsOrigin , & vehicleValuesLocal -> gps , GEO_ALT_RELATIVE );
209+ if (calculateDistanceToDestination (& vehicleVector ) > vehicle -> calculatedVehicleValues .dist ){
210+ //saved vehicle in list is closer, no need to update vehicle in list
211+ vehicle = NULL ;
212+ }
213+ }
183214 }
184215
185216 if (vehicle != NULL ) {
186217 memcpy (& (vehicle -> vehicleValues ), vehicleValuesLocal , sizeof (vehicle -> vehicleValues ));
187218 recalculateVehicle (vehicle );
188- vehicle -> ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST ;
219+ vehicle -> ttl = MAX ( 0 , ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST - vehicleValuesLocal -> tslc ) ;
189220 return ;
190221 }
191222 }
192223};
193224
194225void recalculateVehicle (adsbVehicle_t * vehicle ){
195- gpsDistanceCmBearing (gpsSol .llh .lat , gpsSol .llh .lon , vehicle -> vehicleValues .lat , vehicle -> vehicleValues .lon , & (vehicle -> calculatedVehicleValues .dist ), & (vehicle -> calculatedVehicleValues .dir ));
226+ if (vehicle -> ttl == 0 ){
227+ return ;
228+ }
229+
230+ fpVector3_t vehicleVector ;
231+ geoConvertGeodeticToLocal (& vehicleVector , & posControl .gpsOrigin , & vehicle -> vehicleValues .gps , GEO_ALT_RELATIVE );
196232
197- if (vehicle != NULL && vehicle -> calculatedVehicleValues .dist > ADSB_LIMIT_CM ) {
233+ vehicle -> calculatedVehicleValues .dist = calculateDistanceToDestination (& vehicleVector );
234+ vehicle -> calculatedVehicleValues .dir = calculateBearingToDestination (& vehicleVector );
235+
236+ if (vehicle -> calculatedVehicleValues .dist > ADSB_LIMIT_CM ) {
198237 vehicle -> ttl = 0 ;
199238 return ;
200239 }
@@ -215,6 +254,10 @@ void adsbTtlClean(timeUs_t currentTimeUs) {
215254 if (adsbVehiclesList [i ].ttl > 0 ) {
216255 adsbVehiclesList [i ].ttl -- ;
217256 }
257+
258+ if (adsbVehiclesList [i ].ttl > 0 ) {
259+ recalculateVehicle (& adsbVehiclesList [i ]);
260+ }
218261 }
219262
220263 adsbTtlLastCleanServiced = currentTimeUs ;
0 commit comments