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:10:40 +10:00 committed by Peter Barker
parent a26ebd3600
commit 89bd6ab218
10 changed files with 34 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

@ -334,7 +334,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),

3
libraries/AP_GPS/AP_GPS_DroneCAN.cpp
View File

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

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

@ -208,9 +208,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) {

6
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -183,9 +183,11 @@ AP_GPS_SIRF::_parse_gps(void)
}
state.location.lat = int32_t(be32toh(_buffer.nav.latitude));
state.location.lng = int32_t(be32toh(_buffer.nav.longitude));
state.location.alt = int32_t(be32toh(_buffer.nav.altitude_msl));
const int32_t alt_amsl = int32_t(be32toh(_buffer.nav.altitude_msl));
const int32_t alt_ellipsoid = int32_t(be32toh(_buffer.nav.altitude_ellipsoid));
state.undulation = (alt_amsl - alt_ellipsoid)*0.01;
state.have_undulation = true;
state.undulation = (state.location.alt - int32_t(be32toh(_buffer.nav.altitude_ellipsoid)))*0.01;
set_alt_amsl_cm(state, alt_amsl);
state.ground_speed = int32_t(be32toh(_buffer.nav.ground_speed))*0.01f;
state.ground_course = wrap_360(int16_t(be16toh(_buffer.nav.ground_course)*0.01f));
state.num_sats = _buffer.nav.satellites;

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

@ -437,6 +437,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

@ -156,6 +156,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;