Skip to content

Commit fba6de1

Browse files
error414error414
committed
* fix ADSB cardinal for broken mag direction
* apply good suggestion from qodo-merge-pro
1 parent de322c4 commit fba6de1

4 files changed

Lines changed: 74 additions & 32 deletions

File tree

‎docs/ADSB.md‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,13 @@ is an air traffic surveillance technology that enables aircraft to be accurately
77

88
OSD can be configured to shows the closest aircraft.
99

10-
## OSD element
10+
## OSD ADSB Info element
11+
* "-" no ADSB device detected
12+
* "H" IMU heading is not valid
13+
* "G" no GPS fix or less than 4 stats
14+
* "[Number]" count of ADSB aircrafts
15+
16+
## OSD ADSB Warning element
1117
OSD can be configured to simple view (one line) or to extended view (two lines) by \
1218
`set osd_adsb_warning_style=EXTENDED`
1319

‎src/main/io/adsb.c‎

Lines changed: 31 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -98,6 +98,26 @@ adsbVehicle_t *findVehicleClosest(void) {
9898
* @return
9999
*/
100100
adsbVehicle_t *findVehicleClosestLimit(int32_t maxVerticalDistance) {
101+
102+
////////////////////////////////////////////////////////////
103+
//debug vehicle
104+
/*adsbVehicleValues_t* vehicle = getVehicleForFill();
105+
if(vehicle != NULL){
106+
char name[9] = "DUMMY ";
107+
vehicle->icao = 666;
108+
vehicle->gps.lat = 492383514;
109+
vehicle->gps.lon = 165148681;
110+
vehicle->alt = 100000;
111+
vehicle->heading = 66;
112+
vehicle->flags = ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS;
113+
vehicle->altitudeType = 0;
114+
memcpy(&(vehicle->callsign), name, sizeof(vehicle->callsign));
115+
vehicle->emitterType = 6;
116+
vehicle->tslc = 0;
117+
adsbNewVehicle(vehicle);
118+
}*/
119+
////////////////////////////////////////////////////////////
120+
101121
adsbVehicle_t *adsbLocal = NULL;
102122
for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) {
103123
if(adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid){
@@ -178,7 +198,7 @@ void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) {
178198
}
179199

180200
// non GPS mode, GPS is not fix, just find free space in list or by icao and save vehicle without calculated values
181-
if (!enviromentOkForCalculatingDistaceBearing()) {
201+
if (!isEnvironmentOkForCalculatingADSBDistanceBearing()) {
182202
if(vehicle == NULL){
183203
vehicle = findFreeSpaceInList();
184204
}
@@ -264,8 +284,16 @@ void adsbTtlClean(timeUs_t currentTimeUs) {
264284
}
265285
};
266286

267-
bool enviromentOkForCalculatingDistaceBearing(void){
268-
return (STATE(GPS_FIX) && gpsSol.numSat > 4);
287+
bool isEnvironmentOkForCalculatingADSBDistanceBearing(void){
288+
return
289+
(gpsSol.numSat > 4 &&
290+
(
291+
STATE(GPS_FIX)
292+
#ifdef USE_GPS_FIX_ESTIMATION
293+
|| STATE(GPS_ESTIMATED_FIX)
294+
#endif
295+
)
296+
);
269297
}
270298

271299
#endif

‎src/main/io/adsb.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -64,5 +64,5 @@ uint8_t getActiveVehiclesCount(void);
6464
void adsbTtlClean(timeUs_t currentTimeUs);
6565
adsbVehicleStatus_t* getAdsbStatus(void);
6666
adsbVehicleValues_t* getVehicleForFill(void);
67-
bool enviromentOkForCalculatingDistaceBearing(void);
67+
bool isEnvironmentOkForCalculatingADSBDistanceBearing(void);
6868
void recalculateVehicle(adsbVehicle_t* vehicle);

‎src/main/io/osd.c‎

Lines changed: 35 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -316,6 +316,14 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals,
316316
}
317317
}
318318

319+
/**
320+
* return flight direction on degrees
321+
*/
322+
static int16_t osdGetFlightDirection(void)
323+
{
324+
return STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
325+
}
326+
319327
/**
320328
* Converts distance into a string based on the current unit system.
321329
* @param dist Distance in centimeters
@@ -1979,8 +1987,7 @@ static bool osdDrawSingleElement(uint8_t item)
19791987
if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
19801988
panHomeDirOffset = osdGetPanServoOffset();
19811989
}
1982-
int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
1983-
int homeDirection = GPS_directionToHome - flightDirection + panHomeDirOffset;
1990+
int homeDirection = GPS_directionToHome - osdGetFlightDirection() + panHomeDirOffset;
19841991
osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection);
19851992
}
19861993
} else {
@@ -2176,16 +2183,17 @@ static bool osdDrawSingleElement(uint8_t item)
21762183
uint8_t buffIndexFirstLine = 0;
21772184
uint8_t arrowIndexIndex = 0;
21782185
adsbVehicle_t *vehicle = findVehicleClosestLimit(METERS_TO_CENTIMETERS(osdConfig()->adsb_ignore_plane_above_me_limit));
2179-
if(vehicle != NULL)
2180-
{
2186+
if (vehicle != NULL) {
21812187
recalculateVehicle(vehicle);
21822188
}
21832189

21842190
if (
2185-
vehicle != NULL &&
2186-
(vehicle->calculatedVehicleValues.dist > 0 &&
2187-
vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning))
2188-
){
2191+
vehicle != NULL
2192+
&& (vehicle->calculatedVehicleValues.dist > 0
2193+
&& vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning))
2194+
&& isEnvironmentOkForCalculatingADSBDistanceBearing()
2195+
2196+
) {
21892197
adsbLengthForClearFirstLine = 11;
21902198

21912199
buff[buffIndexFirstLine++] = SYM_ADSB;
@@ -2219,21 +2227,20 @@ static bool osdDrawSingleElement(uint8_t item)
22192227
//////////////////////////////////////////////////////
22202228
// ALT diff to ADSB vehicle draw
22212229
int16_t panServoDirOffset = 0;
2222-
if (osdConfig()->pan_servo_pwm2centideg != 0){
2230+
if (osdConfig()->pan_servo_pwm2centideg != 0) {
22232231
panServoDirOffset = osdGetPanServoOffset();
22242232
}
22252233

2226-
if(arrowIndexIndex > 0)
2227-
{
2234+
if (arrowIndexIndex > 0 && isImuHeadingValid()) {
22282235
//[direction to vehicle]
2229-
int directionToPeerError = osdGetHeadingAngle(CENTIDEGREES_TO_DEGREES(vehicle->calculatedVehicleValues.dir)) + panServoDirOffset - (int)DECIDEGREES_TO_DEGREES(osdGetHeading());
2236+
int directionToPeerError = osdGetHeadingAngle(CENTIDEGREES_TO_DEGREES(vehicle->calculatedVehicleValues.dir)) + panServoDirOffset - osdGetFlightDirection();
22302237
osdDrawDirCardinal(osdDisplayPort, elemPosX + arrowIndexIndex, elemPosY, directionToPeerError, elemAttr);
22312238
}
22322239
//////////////////////////////////////////////////////
22332240

22342241
//////////////////////////////////////////////////////
22352242
// Second line, extra info
2236-
if(osdConfig()->adsb_warning_style == OSD_ADSB_WARNING_STYLE_EXTENDED){
2243+
if (osdConfig()->adsb_warning_style == OSD_ADSB_WARNING_STYLE_EXTENDED) {
22372244
// Vehicle type
22382245
tfp_sprintf(buff, "%s", getAdsbEmitterTypeString(vehicle->vehicleValues.emitterType));
22392246

@@ -2245,41 +2252,43 @@ static bool osdDrawSingleElement(uint8_t item)
22452252
displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY + 1, buff, elemAttr);
22462253

22472254
// Vehicle direction
2248-
int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
2249-
osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX + 6, elemPosY + 1), (float)(CENTIDEGREES_TO_DEGREES(vehicle->vehicleValues.heading) - flightDirection + panServoDirOffset));
2255+
if(isImuHeadingValid())
2256+
{
2257+
osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX + 6, elemPosY + 1), (float)(CENTIDEGREES_TO_DEGREES(vehicle->vehicleValues.heading) - osdGetFlightDirection() + panServoDirOffset));
2258+
}
22502259

22512260
adsbLengthForClearSecondLine += 7;
22522261
}
22532262
///////////////////////////////////////////////////
2254-
}
2255-
else
2256-
{
2263+
} else {
22572264
//clear first line
2258-
if(adsbLengthForClearFirstLine > 0){
2265+
if(adsbLengthForClearFirstLine > 0) {
22592266
memset(buff, SYM_BLANK, constrain(adsbLengthForClearFirstLine, 0, 20));
22602267
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
22612268
adsbLengthForClearFirstLine = 0;
22622269
}
22632270

22642271
//clear second line
2265-
if(adsbLengthForClearSecondLine > 0){
2272+
if(adsbLengthForClearSecondLine > 0) {
22662273
memset(buff, SYM_BLANK, constrain(adsbLengthForClearSecondLine, 0, 20));
22672274
displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, buff);
22682275
adsbLengthForClearSecondLine = 0;
22692276
}
22702277
}
2271-
22722278
return true;
22732279
}
22742280
case OSD_ADSB_INFO:
22752281
{
22762282
buff[0] = SYM_ADSB;
2277-
if(getAdsbStatus()->vehiclesMessagesTotal > 0 || getAdsbStatus()->heartbeatMessagesTotal > 0){
2278-
tfp_sprintf(buff + 1, "%1d", getActiveVehiclesCount());
2279-
}else{
2283+
if (getAdsbStatus()->vehiclesMessagesTotal == 0 && getAdsbStatus()->heartbeatMessagesTotal == 0) {
22802284
buff[1] = '-';
2285+
} else if (!isEnvironmentOkForCalculatingADSBDistanceBearing()) {
2286+
buff[1] = 'G';
2287+
} else if (!isImuHeadingValid()) {
2288+
buff[1] = 'H';
2289+
} else {
2290+
tfp_sprintf(buff + 1, "%1d", getActiveVehiclesCount());
22812291
}
2282-
22832292
break;
22842293
}
22852294

@@ -3996,8 +4005,7 @@ static bool osdDrawSingleElement(uint8_t item)
39964005
if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
39974006
panHomeDirOffset = osdGetPanServoOffset();
39984007
}
3999-
int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
4000-
int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset;
4008+
int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - osdGetFlightDirection() + panHomeDirOffset;
40014009
osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction);
40024010
} else {
40034011
if (isGeozoneActive()) {

0 commit comments

Comments
 (0)