Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: implement the GPS_DRV_OPTION for ellisoid height in mode drivers 

this allows a lot more drivers to use the GPS_DRV_OPTION but to use
ellisoid height. Particularly useful for DroneCAN GPS modules

using ellisoid height instead of AMSL is useful in some specialised
application
This commit is contained in:
Andrew Tridgell 2023-09-19 13:15:09 +10:00
parent 4bc82e4b3e
commit 36dabc0985
10 changed files with 35 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_GPS/AP_GPS.cpp
View File

@ -314,7 +314,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: _DRV_OPTIONS
// @DisplayName: driver options
// @Description: Additional backend specific options
// @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL for uBlox driver
// @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL
// @User: Advanced
AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),

2
libraries/AP_GPS/AP_GPS_ERB.cpp
View File

@ -150,9 +150,9 @@ AP_GPS_ERB::_parse_gps(void)
_last_pos_time = _buffer.pos.time;
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;
set_alt_amsl_cm(state, _buffer.pos.altitude_msl * 100);
state.status = next_fix;
_new_position = true;
state.horizontal_accuracy = _buffer.pos.horizontal_accuracy * 1.0e-3f;

8
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -329,7 +329,7 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_GGA:
_last_GGA_ms = now;
if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) {
state.location.alt = _new_altitude;
set_alt_amsl_cm(state, _new_altitude);
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
}
@ -426,7 +426,7 @@ bool AP_GPS_NMEA::_term_complete()
}
state.location.lat = _ksxt.fields[2]*1.0e7;
state.location.lng = _ksxt.fields[1]*1.0e7;
state.location.alt = _ksxt.fields[3]*1.0e2;
set_alt_amsl_cm(state, _ksxt.fields[3]*1.0e2);
_last_KSXT_pos_ms = now;
if (_ksxt.fields[9] >= 1) {
// we have 3D fix
@ -458,8 +458,9 @@ bool AP_GPS_NMEA::_term_complete()
_last_3D_velocity_ms = now;
state.location.lat = ag.lat*1.0e7;
state.location.lng = ag.lng*1.0e7;
state.location.alt = ag.alt*1.0e2;
state.undulation = -ag.undulation;
state.have_undulation = true;
set_alt_amsl_cm(state, ag.alt*1.0e2);
state.velocity = ag.vel_NED;
velocity_to_speed_course(state);
state.speed_accuracy = ag.vel_stddev.length();
@ -469,7 +470,6 @@ bool AP_GPS_NMEA::_term_complete()
state.have_speed_accuracy = true;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
state.have_undulation = true;
check_new_itow(ag.itow, _sentence_length);
break;
}

2
libraries/AP_GPS/AP_GPS_NOVA.cpp
View File

@ -205,9 +205,9 @@ 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;
set_alt_amsl_cm(state, bestposu.hgt * 100);
state.num_sats = bestposu.svsused;

2
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -415,9 +415,9 @@ AP_GPS_SBF::process_message(void)
if (temp.Latitude > -200000) {
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;
set_alt_amsl_cm(state, ((float)temp.Height - temp.Undulation) * 1e2f);
}
if (temp.NrSV != 255) {

7
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -179,9 +179,12 @@ 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);
const int32_t alt_amsl = swap_int32(_buffer.nav.altitude_msl);
const int32_t alt_ellipsoid = swap_int32(_buffer.nav.altitude_ellipsoid);
state.have_undulation = true;
state.undulation = (state.location.alt - swap_int32(_buffer.nav.altitude_ellipsoid))*0.01;
state.undulation = (alt_amsl - alt_ellipsoid)*0.01;
set_alt_amsl_cm(state, alt_amsl);
state.location.alt = swap_int32(_buffer.nav.altitude_msl);
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;

3
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -436,10 +436,11 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb)
Location loc = { };
loc.lat = cb.msg->latitude_deg_1e8 / 10;
loc.lng = cb.msg->longitude_deg_1e8 / 10;
loc.alt = cb.msg->height_msl_mm / 10;
const int32_t alt_amsl_cm = cb.msg->height_msl_mm / 10;
interim_state.have_undulation = true;
interim_state.undulation = (cb.msg->height_msl_mm - cb.msg->height_ellipsoid_mm) * 0.001;
interim_state.location = loc;
set_alt_amsl_cm(interim_state, alt_amsl_cm);
handle_velocity(cb.msg->ned_velocity[0], cb.msg->ned_velocity[1], cb.msg->ned_velocity[2]);

12
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1368,13 +1368,9 @@ AP_GPS_UBLOX::_parse_gps(void)
_last_pos_time = _buffer.posllh.itow;
state.location.lng = _buffer.posllh.longitude;
state.location.lat = _buffer.posllh.latitude;
if (option_set(AP_GPS::HeightEllipsoid)) {
state.location.alt = _buffer.posllh.altitude_ellipsoid / 10;
} else {
state.location.alt = _buffer.posllh.altitude_msl / 10;
}
state.have_undulation = true;
state.undulation = (_buffer.posllh.altitude_msl - _buffer.posllh.altitude_ellipsoid) * 0.001;
set_alt_amsl_cm(state, _buffer.posllh.altitude_msl / 10);
state.status = next_fix;
_new_position = true;
@ -1532,13 +1528,9 @@ AP_GPS_UBLOX::_parse_gps(void)
_last_pos_time = _buffer.pvt.itow;
state.location.lng = _buffer.pvt.lon;
state.location.lat = _buffer.pvt.lat;
if (option_set(AP_GPS::HeightEllipsoid)) {
state.location.alt = _buffer.pvt.h_ellipsoid / 10;
} else {
state.location.alt = _buffer.pvt.h_msl / 10;
}
state.have_undulation = true;
state.undulation = (_buffer.pvt.h_msl - _buffer.pvt.h_ellipsoid) * 0.001;
set_alt_amsl_cm(state, _buffer.pvt.h_msl / 10);
switch (_buffer.pvt.fix_type)
{
case 0:

15
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -468,6 +468,21 @@ good_yaw:
}
#endif // GPS_MOVING_BASELINE
/*
set altitude in location structure, honouring the driver option for
MSL vs ellipsoid height
*/
void AP_GPS_Backend::set_alt_amsl_cm(AP_GPS::GPS_State &_state, int32_t alt_amsl_cm)
{
if (option_set(AP_GPS::HeightEllipsoid) && _state.have_undulation) {
// user has asked ArduPilot to use ellipsoid height in the
// canonical height for mission and navigation
_state.location.alt = alt_amsl_cm - _state.undulation*100;
} else {
_state.location.alt = alt_amsl_cm;
}
}
#if AP_GPS_DEBUG_LOGGING_ENABLED
/*

3
libraries/AP_GPS/GPS_Backend.h
View File

@ -157,6 +157,9 @@ protected:
void log_data(const uint8_t *data, uint16_t length);
#endif
// set alt in location, honouring GPS driver option for ellipsoid height
void set_alt_amsl_cm(AP_GPS::GPS_State &_state, int32_t alt_amsl_cm);
private:
// itow from previous message
uint64_t _pseudo_itow;