Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: added get_undulation support

Browse Source
This commit is contained in:
Joshua Henderson 2022-07-28 04:42:01 -04:00 committed by Randy Mackay
parent f69b1ca566
commit 0c04ce105e
9 changed files with 67 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_GPS/AP_GPS.cpp
View File

@ -1319,6 +1319,11 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
float hacc = 0.0f;
float vacc = 0.0f;
float sacc = 0.0f;
float undulation = 0.0;
int32_t height_elipsoid_mm = 0;
if (get_undulation(0, undulation)) {
height_elipsoid_mm = loc.alt*10 - undulation*1000;
}
horizontal_accuracy(0, hacc);
vertical_accuracy(0, vacc);
speed_accuracy(0, sacc);
@ -1334,7 +1339,7 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
ground_speed(0)*100, // cm/s
ground_course(0)*100, // 1/100 degrees,
num_sats(0),
0, // TODO: Elipsoid height in mm
height_elipsoid_mm, // Elipsoid height in mm
hacc * 1000, // one-sigma standard deviation in mm
vacc * 1000, // one-sigma standard deviation in mm
sacc * 1000, // one-sigma standard deviation in mm/s
@ -1354,6 +1359,11 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
float hacc = 0.0f;
float vacc = 0.0f;
float sacc = 0.0f;
float undulation = 0.0;
float height_elipsoid_mm = 0;
if (get_undulation(0, undulation)) {
height_elipsoid_mm = loc.alt*10 - undulation*1000;
}
horizontal_accuracy(1, hacc);
vertical_accuracy(1, vacc);
speed_accuracy(1, sacc);
@ -1372,7 +1382,7 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
state[1].rtk_num_sats,
state[1].rtk_age_ms,
gps_yaw_cdeg(1),
0, // TODO: Elipsoid height in mm
height_elipsoid_mm, // Elipsoid height in mm
hacc * 1000, // one-sigma standard deviation in mm
vacc * 1000, // one-sigma standard deviation in mm
sacc * 1000, // one-sigma standard deviation in mm/s
@ -1808,6 +1818,16 @@ void AP_GPS::calc_blended_state(void)
timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = 0;
timing[GPS_BLENDED_INSTANCE].last_message_time_ms = 0;
if (state[0].have_undulation) {
state[GPS_BLENDED_INSTANCE].have_undulation = true;
state[GPS_BLENDED_INSTANCE].undulation = state[0].undulation;
} else if (state[1].have_undulation) {
state[GPS_BLENDED_INSTANCE].have_undulation = true;
state[GPS_BLENDED_INSTANCE].undulation = state[1].undulation;
} else {
state[GPS_BLENDED_INSTANCE].have_undulation = false;
}
// combine the states into a blended solution
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
// use the highest status
@ -2053,6 +2073,17 @@ bool AP_GPS::get_error_codes(uint8_t instance, uint32_t &error_codes) const
return drivers[instance]->get_error_codes(error_codes);
}
// get the difference between WGS84 and AMSL. A positive value means
// the AMSL height is higher than WGS84 ellipsoid height
bool AP_GPS::get_undulation(uint8_t instance, float &undulation) const
{
if (!state[instance].have_undulation) {
return false;
}
undulation = state[instance].undulation;
return true;
}
// Logging support:
// Write an GPS packet
void AP_GPS::Write_GPS(uint8_t i)
@ -2086,9 +2117,11 @@ void AP_GPS::Write_GPS(uint8_t i)
/* write auxiliary accuracy information as well */
float hacc = 0, vacc = 0, sacc = 0;
float undulation = 0;
horizontal_accuracy(i, hacc);
vertical_accuracy(i, vacc);
speed_accuracy(i, sacc);
get_undulation(i, undulation);
struct log_GPA pkt2{
LOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
time_us : time_us,
@ -2100,7 +2133,8 @@ void AP_GPS::Write_GPS(uint8_t i)
yaw_accuracy : yaw_accuracy_deg,
have_vv : (uint8_t)have_vertical_velocity(i),
sample_ms : last_message_time_ms(i),
delta_ms : last_message_delta_time_ms(i)
delta_ms : last_message_delta_time_ms(i),
undulation : undulation,
};
AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
}

12
libraries/AP_GPS/AP_GPS.h
View File

@ -193,6 +193,8 @@ public:
bool have_vertical_accuracy; ///< does GPS give vertical position accuracy? Set to true only once available.
bool have_gps_yaw; ///< does GPS give yaw? Set to true only once available.
bool have_gps_yaw_accuracy; ///< does the GPS give a heading accuracy estimate? Set to true only once available
float undulation; //<height that WGS84 is above AMSL at the current location
bool have_undulation; ///<do we have a value for the undulation
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
uint64_t last_corrected_gps_time_us;///< the system time we got the last corrected GPS timestamp, microseconds
bool corrected_timestamp_updated; ///< true if the corrected timestamp has been updated
@ -293,6 +295,16 @@ public:
return location(primary_instance);
}
// get the difference between WGS84 and AMSL. A positive value means
// the AMSL height is higher than WGS84 ellipsoid height
bool get_undulation(uint8_t instance, float &undulation) const;
// get the difference between WGS84 and AMSL. A positive value means
// the AMSL height is higher than WGS84 ellipsoid height
bool get_undulation(float &undulation) const {
return get_undulation(primary_instance, undulation);
}
// report speed accuracy
bool speed_accuracy(uint8_t instance, float &sacc) const;
bool speed_accuracy(float &sacc) const {

2
libraries/AP_GPS/AP_GPS_ERB.cpp
View File

@ -146,6 +146,8 @@ AP_GPS_ERB::_parse_gps(void)
state.location.lng = (int32_t)(_buffer.pos.longitude * (double)1e7);
state.location.lat = (int32_t)(_buffer.pos.latitude * (double)1e7);
state.location.alt = (int32_t)(_buffer.pos.altitude_msl * 100);
state.have_undulation = true;
state.undulation = _buffer.pos.altitude_msl - _buffer.pos.altitude_ellipsoid;
state.status = next_fix;
_new_position = true;
state.horizontal_accuracy = _buffer.pos.horizontal_accuracy * 1.0e-3f;

2
libraries/AP_GPS/AP_GPS_NOVA.cpp
View File

@ -205,6 +205,8 @@ AP_GPS_NOVA::process_message(void)
state.location.lat = (int32_t) (bestposu.lat * (double)1e7);
state.location.lng = (int32_t) (bestposu.lng * (double)1e7);
state.location.alt = (int32_t) (bestposu.hgt * 100);
state.have_undulation = true;
state.undulation = bestposu.undulation;
state.num_sats = bestposu.svsused;

2
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -416,6 +416,8 @@ AP_GPS_SBF::process_message(void)
state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * (double)1e7);
state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * (double)1e7);
state.location.alt = (int32_t)(((float)temp.Height - temp.Undulation) * 1e2f);
state.have_undulation = true;
state.undulation = temp.Undulation;
}
if (temp.NrSV != 255) {

2
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -180,6 +180,8 @@ AP_GPS_SIRF::_parse_gps(void)
state.location.lat = swap_int32(_buffer.nav.latitude);
state.location.lng = swap_int32(_buffer.nav.longitude);
state.location.alt = swap_int32(_buffer.nav.altitude_msl);
state.have_undulation = true;
state.undulation = (state.location.alt - swap_int32(_buffer.nav.altitude_ellipsoid))*0.01;
state.ground_speed = swap_int32(_buffer.nav.ground_speed)*0.01f;
state.ground_course = wrap_360(swap_int16(_buffer.nav.ground_course)*0.01f);
state.num_sats = _buffer.nav.satellites;

7
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1216,6 +1216,9 @@ AP_GPS_UBLOX::_parse_gps(void)
state.location.lng = _buffer.posllh.longitude;
state.location.lat = _buffer.posllh.latitude;
state.location.alt = _buffer.posllh.altitude_msl / 10;
state.have_undulation = true;
state.undulation = (_buffer.posllh.altitude_msl - _buffer.posllh.altitude_ellipsoid) * 0.001;
state.status = next_fix;
_new_position = true;
state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
@ -1369,7 +1372,9 @@ AP_GPS_UBLOX::_parse_gps(void)
state.location.lng = _buffer.pvt.lon;
state.location.lat = _buffer.pvt.lat;
state.location.alt = _buffer.pvt.h_msl / 10;
switch (_buffer.pvt.fix_type)
state.have_undulation = true;
state.undulation = (_buffer.pvt.h_msl - _buffer.pvt.h_ellipsoid) * 0.001;
switch (_buffer.pvt.fix_type)
{
case 0:
state.status = AP_GPS::NO_FIX;

2
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -341,7 +341,7 @@ private:
uint8_t flags2;
uint8_t num_sv;
int32_t lon, lat;
int32_t height, h_msl;
int32_t h_ellipsoid, h_msl;
uint32_t h_acc, v_acc;
int32_t velN, velE, velD, gspeed;
int32_t head_mot;

4
libraries/AP_GPS/LogStructure.h
View File

@ -62,6 +62,7 @@ struct PACKED log_GPS {
// @Field: VV: true if vertical velocity is available
// @Field: SMS: time since system startup this sample was taken
// @Field: Delta: system time delta between the last two reported positions
// @Field: Und: Undulation
struct PACKED log_GPA {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -74,6 +75,7 @@ struct PACKED log_GPA {
uint8_t have_vv;
uint32_t sample_ms;
uint16_t delta_ms;
float undulation;
};
/*
@ -200,7 +202,7 @@ struct PACKED log_GPS_RAWS {
{ LOG_GPS_MSG, sizeof(log_GPS), \
"GPS", "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#---SmDUmnhnh-", "F----0BGGB000--" , true }, \
{ LOG_GPA_MSG, sizeof(log_GPA), \
"GPA", "QBCCCCfBIH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta", "s#mmmnd-ss", "F-BBBB0-CC" , true }, \
"GPA", "QBCCCCfBIHf", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und", "s#mmmnd-ssm", "F-BBBB0-CC0" , true }, \
{ LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
"UBX1", "QBHBBHI", "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt,config", "s#-----", "F------" , true }, \
{ LOG_GPS_UBX2_MSG, sizeof(log_Ubx2), \