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: SBF report velocity accuracy

Browse Source
This commit is contained in:
Michael du Breuil 2017-05-24 14:39:47 -07:00 committed by Francisco Ferreira
parent 2ddf7f99b5
commit 3da3ad05bf
2 changed files with 61 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -190,16 +190,16 @@ AP_GPS_SBF::log_ExtEventPVTGeodetic(const msg4007 &temp)
bool
AP_GPS_SBF::process_message(void)
{
uint16_t blockid = (sbf_msg.blockid & 4095u);
uint16_t blockid = (sbf_msg.blockid & 8191u);
Debug("BlockID %d", blockid);
// ExtEventPVTGeodetic
if (blockid == 4038) {
switch (blockid) {
case ExtEventPVTGeodetic:
log_ExtEventPVTGeodetic(sbf_msg.data.msg4007u);
}
// PVTGeodetic
else if (blockid == 4007) {
break;
case PVTGeodetic:
{
const msg4007 &temp = sbf_msg.data.msg4007u;
// Update time state
@ -272,24 +272,43 @@ AP_GPS_SBF::process_message(void)
break;
}
if ((temp.Mode & 64) > 0) // gps is in base mode
if ((temp.Mode & 64) > 0) { // gps is in base mode
state.status = AP_GPS::NO_FIX;
if ((temp.Mode & 128) > 0) // gps only has 2d fix
} else if ((temp.Mode & 128) > 0) { // gps only has 2d fix
state.status = AP_GPS::GPS_OK_FIX_2D;
}
return true;
}
// DOP
else if (blockid == 4001) {
case DOP:
{
const msg4001 &temp = sbf_msg.data.msg4001u;
state.hdop = temp.HDOP;
state.vdop = temp.VDOP;
break;
}
// ReceiverStatus
else if (blockid == 4014) {
case ReceiverStatus:
{
const msg4014 &temp = sbf_msg.data.msg4014u;
RxState = temp.RxState;
break;
}
case VelCovGeodetic:
{
const msg5908 &temp = sbf_msg.data.msg5908u;
// select the maximum variance, as the EKF will apply it to all the columnds in it's estimate
// FIXME: Support returning the covariance matric to the EKF
float max_variance_squared = MAX(temp.Cov_VnVn, MAX(temp.Cov_VeVe, temp.Cov_VuVu));
if (is_positive(max_variance_squared)) {
state.have_speed_accuracy = true;
state.speed_accuracy = sqrt(max_variance_squared);
} else {
state.have_speed_accuracy = false;
}
break;
}
}
return false;

33
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -54,7 +54,7 @@ private:
uint8_t _init_blob_index = 0;
uint32_t _init_blob_time = 0;
const char* _initialisation_blob[5] = {
"sso, Stream1, COM1, PVTGeodetic+DOP+ExtEventPVTGeodetic+ReceiverStatus, msec100\n",
"sso, Stream1, COM1, PVTGeodetic+DOP+ExtEventPVTGeodetic+ReceiverStatus+VelCovGeodetic, msec100\n",
"srd, Moderate, UAV\n",
"sem, PVT, 5\n",
"spm, Rover, StandAlone+SBAS+DGPS+RTK\n",
@ -65,7 +65,15 @@ private:
bool validcommand = false;
uint32_t RxState;
struct PACKED msg4007
enum sbf_ids {
DOP = 4001,
PVTGeodetic = 4007,
ReceiverStatus = 4014,
ExtEventPVTGeodetic = 4038,
VelCovGeodetic = 5908
};
struct PACKED msg4007 // PVTGeodetic
{
uint32_t TOW;
uint16_t WNc;
@ -99,7 +107,7 @@ private:
uint8_t Misc;
};
struct PACKED msg4001
struct PACKED msg4001 // DOP
{
uint32_t TOW;
uint16_t WNc;
@ -125,10 +133,29 @@ private:
// remaining data is AGCData, which we don't have a use for, don't extract the data
};
struct PACKED msg5908 // VelCovGeodetic
{
uint32_t TOW;
uint16_t WNc;
uint8_t Mode;
uint8_t Error;
float Cov_VnVn;
float Cov_VeVe;
float Cov_VuVu;
float Cov_DtDt;
float Cov_VnVe;
float Cov_VnVu;
float Cov_VnDt;
float Cov_VeVu;
float Cov_VeDt;
float Cov_VuDt;
};
union PACKED msgbuffer {
msg4007 msg4007u;
msg4001 msg4001u;
msg4014 msg4014u;
msg5908 msg5908u;
uint8_t bytes[128];
};