mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-04 23:18:29 -04:00
AP_GPS: Add uBlox accuracy metrics interface and logging
This commit is contained in:
parent
9be7039be1
commit
2293070a5b
@ -135,7 +135,13 @@ public:
|
||||
uint16_t hdop; ///< horizontal dilution of precision in cm
|
||||
uint8_t num_sats; ///< Number of visible satelites
|
||||
Vector3f velocity; ///< 3D velocitiy in m/s, in NED format
|
||||
float speed_accuracy;
|
||||
float horizontal_accuracy;
|
||||
float vertical_accuracy;
|
||||
bool have_vertical_velocity:1; ///< does this GPS give vertical velocity?
|
||||
bool have_speed_accuracy:1;
|
||||
bool have_horizontal_accuracy:1;
|
||||
bool have_vertical_accuracy:1;
|
||||
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
|
||||
};
|
||||
|
||||
@ -180,6 +186,42 @@ public:
|
||||
return location(primary_instance);
|
||||
}
|
||||
|
||||
bool speed_accuracy(uint8_t instance, float &sacc) const {
|
||||
if(_GPS_STATE(instance).have_speed_accuracy) {
|
||||
sacc = _GPS_STATE(instance).speed_accuracy;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool speed_accuracy(float &sacc) const {
|
||||
return speed_accuracy(primary_instance, sacc);
|
||||
}
|
||||
|
||||
bool horizontal_accuracy(uint8_t instance, float &hacc) const {
|
||||
if(_GPS_STATE(instance).have_horizontal_accuracy) {
|
||||
hacc = _GPS_STATE(instance).horizontal_accuracy;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool horizontal_accuracy(float &hacc) const {
|
||||
return horizontal_accuracy(primary_instance, hacc);
|
||||
}
|
||||
|
||||
bool vertical_accuracy(uint8_t instance, float &vacc) const {
|
||||
if(_GPS_STATE(instance).have_vertical_accuracy) {
|
||||
vacc = _GPS_STATE(instance).vertical_accuracy;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool vertical_accuracy(float &vacc) const {
|
||||
return vertical_accuracy(primary_instance, vacc);
|
||||
}
|
||||
|
||||
// 3D velocity in NED format
|
||||
const Vector3f &velocity(uint8_t instance) const {
|
||||
return _GPS_STATE(instance).velocity;
|
||||
|
@ -287,6 +287,21 @@ void AP_GPS_UBLOX::log_mon_hw2(void)
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
void AP_GPS_UBLOX::log_accuracy(void) {
|
||||
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
|
||||
return;
|
||||
}
|
||||
struct log_Ubx3 pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_UBX3_MSG),
|
||||
timestamp : hal.scheduler->millis(),
|
||||
instance : state.instance,
|
||||
hAcc : state.horizontal_accuracy,
|
||||
vAcc : state.vertical_accuracy,
|
||||
sAcc : state.speed_accuracy
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
#endif // UBLOX_HW_LOGGING
|
||||
|
||||
void AP_GPS_UBLOX::unexpected_message(void)
|
||||
@ -390,6 +405,10 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
state.location.lat = -353632610L;
|
||||
state.location.alt = 58400;
|
||||
#endif
|
||||
state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
|
||||
state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
|
||||
state.have_horizontal_accuracy = true;
|
||||
state.have_vertical_accuracy = true;
|
||||
break;
|
||||
case MSG_STATUS:
|
||||
Debug("MSG_STATUS fix_status=%u fix_type=%u",
|
||||
@ -462,6 +481,8 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
state.velocity.x = _buffer.velned.ned_north * 0.01f;
|
||||
state.velocity.y = _buffer.velned.ned_east * 0.01f;
|
||||
state.velocity.z = _buffer.velned.ned_down * 0.01f;
|
||||
state.have_speed_accuracy = true;
|
||||
state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
|
||||
_new_speed = true;
|
||||
break;
|
||||
default:
|
||||
@ -498,6 +519,11 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
|
||||
_fix_count = 0;
|
||||
}
|
||||
|
||||
#if UBLOX_HW_LOGGING
|
||||
log_accuracy();
|
||||
#endif //UBLOX_HW_LOGGING
|
||||
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -289,6 +289,7 @@ private:
|
||||
void write_logging_headers(void);
|
||||
void log_mon_hw(void);
|
||||
void log_mon_hw2(void);
|
||||
void log_accuracy(void);
|
||||
};
|
||||
|
||||
#endif // __AP_GPS_UBLOX_H__
|
||||
|
@ -23,6 +23,9 @@ AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::
|
||||
gps(_gps),
|
||||
state(_state)
|
||||
{
|
||||
state.have_speed_accuracy = false;
|
||||
state.have_horizontal_accuracy = false;
|
||||
state.have_vertical_accuracy = false;
|
||||
}
|
||||
|
||||
int32_t AP_GPS_Backend::swap_int32(int32_t v) const
|
||||
|
Loading…
Reference in New Issue
Block a user