Skip to content

Commit 5d61e3f

Browse files
tpwrulesrmackay9
authored andcommitted
AP_GPS: log altitude above ellipsoid instead of undulation
ArduPilot's internal definition of undulation is reversed in sign compared to the accepted convention and this creates confusion: ArduPilot#29199 . Instead of the internal definition, log the altitude above ellipsoid, which ArduPilot already consistently and correctly calculates and broadcasts over e.g. DroneCAN and MAVLink. Switching the quantity logged allows users to accommodate the problem in old logs (i.e. always flip GPA.Und sign to match the accepted convention) and reduces opportunity for future confusion. Future work will clean up or replace the internal representation, but this should be a complete fix for the issue from a user perspective.
1 parent e14ea7d commit 5d61e3f

File tree

2 files changed

+8
-5
lines changed

2 files changed

+8
-5
lines changed

libraries/AP_GPS/AP_GPS.cpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -1908,11 +1908,14 @@ void AP_GPS::Write_GPS(uint8_t i)
19081908

19091909
/* write auxiliary accuracy information as well */
19101910
float hacc = 0, vacc = 0, sacc = 0;
1911+
int32_t alt_ellipsoid = INT32_MIN;
19111912
float undulation = 0;
19121913
horizontal_accuracy(i, hacc);
19131914
vertical_accuracy(i, vacc);
19141915
speed_accuracy(i, sacc);
1915-
get_undulation(i, undulation);
1916+
if (get_undulation(i, undulation)) {
1917+
alt_ellipsoid = loc.alt - (undulation*100);
1918+
}
19161919
struct log_GPA pkt2{
19171920
LOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
19181921
time_us : time_us,
@@ -1925,7 +1928,7 @@ void AP_GPS::Write_GPS(uint8_t i)
19251928
have_vv : (uint8_t)have_vertical_velocity(i),
19261929
sample_ms : last_message_time_ms(i),
19271930
delta_ms : last_message_delta_time_ms(i),
1928-
undulation : undulation,
1931+
alt_ellipsoid : alt_ellipsoid,
19291932
rtcm_fragments_used: rtcm_stats.fragments_used,
19301933
rtcm_fragments_discarded: rtcm_stats.fragments_discarded
19311934
};

libraries/AP_GPS/LogStructure.h

+3-3
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ struct PACKED log_GPS {
6363
// @Field: VV: true if vertical velocity is available
6464
// @Field: SMS: time since system startup this sample was taken
6565
// @Field: Delta: system time delta between the last two reported positions
66-
// @Field: Und: Undulation
66+
// @Field: AEl: altitude above WGS-84 ellipsoid; INT32_MIN (-2147483648) if unknown
6767
// @Field: RTCMFU: RTCM fragments used
6868
// @Field: RTCMFD: RTCM fragments discarded
6969
struct PACKED log_GPA {
@@ -78,7 +78,7 @@ struct PACKED log_GPA {
7878
uint8_t have_vv;
7979
uint32_t sample_ms;
8080
uint16_t delta_ms;
81-
float undulation;
81+
int32_t alt_ellipsoid;
8282
uint16_t rtcm_fragments_used;
8383
uint16_t rtcm_fragments_discarded;
8484
};
@@ -207,7 +207,7 @@ struct PACKED log_GPS_RAWS {
207207
{ LOG_GPS_MSG, sizeof(log_GPS), \
208208
"GPS", "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#-s-S-DUmnhnh-", "F--C-0BGGB000--" , true }, \
209209
{ LOG_GPA_MSG, sizeof(log_GPA), \
210-
"GPA", "QBCCCCfBIHfHH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RTCMFU,RTCMFD", "s#-mmnd-ssm--", "F-BBBB0-CC0--" , true }, \
210+
"GPA", "QBCCCCfBIHeHH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,AEl,RTCMFU,RTCMFD", "s#-mmnd-ssm--", "F-BBBB0-CCB--" , true }, \
211211
{ LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
212212
"UBX1", "QBHBBHI", "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt,config", "s#-----", "F------" , true }, \
213213
{ LOG_GPS_UBX2_MSG, sizeof(log_Ubx2), \

0 commit comments

Comments
 (0)