Skip to content

Commit 0121d2c

Browse files
sensei-hackersensei-hacker
authored
Merge pull request #11076 from error414/ADSB-fix-cardinal-for-broken-mag
fix ADSB cardinal for broken mag direction
2 parents 8bbdc84 + 91144d6 commit 0121d2c

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
@@ -2061,8 +2069,7 @@ static bool osdDrawSingleElement(uint8_t item)
20612069
if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
20622070
panHomeDirOffset = osdGetPanServoOffset();
20632071
}
2064-
int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
2065-
int homeDirection = GPS_directionToHome - flightDirection + panHomeDirOffset;
2072+
int homeDirection = GPS_directionToHome - osdGetFlightDirection() + panHomeDirOffset;
20662073
osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection);
20672074
}
20682075
} else {
@@ -2258,16 +2265,17 @@ static bool osdDrawSingleElement(uint8_t item)
22582265
uint8_t buffIndexFirstLine = 0;
22592266
uint8_t arrowIndexIndex = 0;
22602267
adsbVehicle_t *vehicle = findVehicleClosestLimit(METERS_TO_CENTIMETERS(osdConfig()->adsb_ignore_plane_above_me_limit));
2261-
if(vehicle != NULL)
2262-
{
2268+
if (vehicle != NULL) {
22632269
recalculateVehicle(vehicle);
22642270
}
22652271

22662272
if (
2267-
vehicle != NULL &&
2268-
(vehicle->calculatedVehicleValues.dist > 0 &&
2269-
vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning))
2270-
){
2273+
vehicle != NULL
2274+
&& (vehicle->calculatedVehicleValues.dist > 0
2275+
&& vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning))
2276+
&& isEnvironmentOkForCalculatingADSBDistanceBearing()
2277+
2278+
) {
22712279
adsbLengthForClearFirstLine = 11;
22722280

22732281
buff[buffIndexFirstLine++] = SYM_ADSB;
@@ -2301,21 +2309,20 @@ static bool osdDrawSingleElement(uint8_t item)
23012309
//////////////////////////////////////////////////////
23022310
// ALT diff to ADSB vehicle draw
23032311
int16_t panServoDirOffset = 0;
2304-
if (osdConfig()->pan_servo_pwm2centideg != 0){
2312+
if (osdConfig()->pan_servo_pwm2centideg != 0) {
23052313
panServoDirOffset = osdGetPanServoOffset();
23062314
}
23072315

2308-
if(arrowIndexIndex > 0)
2309-
{
2316+
if (arrowIndexIndex > 0 && isImuHeadingValid()) {
23102317
//[direction to vehicle]
2311-
int directionToPeerError = osdGetHeadingAngle(CENTIDEGREES_TO_DEGREES(vehicle->calculatedVehicleValues.dir)) + panServoDirOffset - (int)DECIDEGREES_TO_DEGREES(osdGetHeading());
2318+
int directionToPeerError = osdGetHeadingAngle(CENTIDEGREES_TO_DEGREES(vehicle->calculatedVehicleValues.dir)) + panServoDirOffset - osdGetFlightDirection();
23122319
osdDrawDirCardinal(osdDisplayPort, elemPosX + arrowIndexIndex, elemPosY, directionToPeerError, elemAttr);
23132320
}
23142321
//////////////////////////////////////////////////////
23152322

23162323
//////////////////////////////////////////////////////
23172324
// Second line, extra info
2318-
if(osdConfig()->adsb_warning_style == OSD_ADSB_WARNING_STYLE_EXTENDED){
2325+
if (osdConfig()->adsb_warning_style == OSD_ADSB_WARNING_STYLE_EXTENDED) {
23192326
// Vehicle type
23202327
tfp_sprintf(buff, "%s", getAdsbEmitterTypeString(vehicle->vehicleValues.emitterType));
23212328

@@ -2327,41 +2334,43 @@ static bool osdDrawSingleElement(uint8_t item)
23272334
displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY + 1, buff, elemAttr);
23282335

23292336
// Vehicle direction
2330-
int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
2331-
osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX + 6, elemPosY + 1), (float)(CENTIDEGREES_TO_DEGREES(vehicle->vehicleValues.heading) - flightDirection + panServoDirOffset));
2337+
if(isImuHeadingValid())
2338+
{
2339+
osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX + 6, elemPosY + 1), (float)(CENTIDEGREES_TO_DEGREES(vehicle->vehicleValues.heading) - osdGetFlightDirection() + panServoDirOffset));
2340+
}
23322341

23332342
adsbLengthForClearSecondLine += 7;
23342343
}
23352344
///////////////////////////////////////////////////
2336-
}
2337-
else
2338-
{
2345+
} else {
23392346
//clear first line
2340-
if(adsbLengthForClearFirstLine > 0){
2347+
if(adsbLengthForClearFirstLine > 0) {
23412348
memset(buff, SYM_BLANK, constrain(adsbLengthForClearFirstLine, 0, 20));
23422349
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
23432350
adsbLengthForClearFirstLine = 0;
23442351
}
23452352

23462353
//clear second line
2347-
if(adsbLengthForClearSecondLine > 0){
2354+
if(adsbLengthForClearSecondLine > 0) {
23482355
memset(buff, SYM_BLANK, constrain(adsbLengthForClearSecondLine, 0, 20));
23492356
displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, buff);
23502357
adsbLengthForClearSecondLine = 0;
23512358
}
23522359
}
2353-
23542360
return true;
23552361
}
23562362
case OSD_ADSB_INFO:
23572363
{
23582364
buff[0] = SYM_ADSB;
2359-
if(getAdsbStatus()->vehiclesMessagesTotal > 0 || getAdsbStatus()->heartbeatMessagesTotal > 0){
2360-
tfp_sprintf(buff + 1, "%1d", getActiveVehiclesCount());
2361-
}else{
2365+
if (getAdsbStatus()->vehiclesMessagesTotal == 0 && getAdsbStatus()->heartbeatMessagesTotal == 0) {
23622366
buff[1] = '-';
2367+
} else if (!isEnvironmentOkForCalculatingADSBDistanceBearing()) {
2368+
buff[1] = 'G';
2369+
} else if (!isImuHeadingValid()) {
2370+
buff[1] = 'H';
2371+
} else {
2372+
tfp_sprintf(buff + 1, "%1d", getActiveVehiclesCount());
23632373
}
2364-
23652374
break;
23662375
}
23672376

@@ -4081,8 +4090,7 @@ static bool osdDrawSingleElement(uint8_t item)
40814090
if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
40824091
panHomeDirOffset = osdGetPanServoOffset();
40834092
}
4084-
int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading());
4085-
int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset;
4093+
int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - osdGetFlightDirection() + panHomeDirOffset;
40864094
osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction);
40874095
} else {
40884096
if (isGeozoneActive()) {

0 commit comments

Comments
 (0)