Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -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 bool
AP_GPS_SBF::process_message(void) AP_GPS_SBF::process_message(void)
{ {
uint16_t blockid = (sbf_msg.blockid & 4095u); uint16_t blockid = (sbf_msg.blockid & 8191u);
Debug("BlockID %d", blockid); Debug("BlockID %d", blockid);
// ExtEventPVTGeodetic switch (blockid) {
if (blockid == 4038) { case ExtEventPVTGeodetic:
log_ExtEventPVTGeodetic(sbf_msg.data.msg4007u); log_ExtEventPVTGeodetic(sbf_msg.data.msg4007u);
} break;
// PVTGeodetic case PVTGeodetic:
else if (blockid == 4007) { {
const msg4007 &temp = sbf_msg.data.msg4007u; const msg4007 &temp = sbf_msg.data.msg4007u;
// Update time state // Update time state
@ -272,24 +272,43 @@ AP_GPS_SBF::process_message(void)
break; 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; 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; state.status = AP_GPS::GPS_OK_FIX_2D;
}
return true; return true;
} }
// DOP case DOP:
else if (blockid == 4001) { {
const msg4001 &temp = sbf_msg.data.msg4001u; const msg4001 &temp = sbf_msg.data.msg4001u;
state.hdop = temp.HDOP; state.hdop = temp.HDOP;
state.vdop = temp.VDOP; state.vdop = temp.VDOP;
break;
} }
// ReceiverStatus case ReceiverStatus:
else if (blockid == 4014) { {
const msg4014 &temp = sbf_msg.data.msg4014u; const msg4014 &temp = sbf_msg.data.msg4014u;
RxState = temp.RxState; 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; return false;

33
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -54,7 +54,7 @@ private:
uint8_t _init_blob_index = 0; uint8_t _init_blob_index = 0;
uint32_t _init_blob_time = 0; uint32_t _init_blob_time = 0;
const char* _initialisation_blob[5] = { 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", "srd, Moderate, UAV\n",
"sem, PVT, 5\n", "sem, PVT, 5\n",
"spm, Rover, StandAlone+SBAS+DGPS+RTK\n", "spm, Rover, StandAlone+SBAS+DGPS+RTK\n",
@ -65,7 +65,15 @@ private:
bool validcommand = false; bool validcommand = false;
uint32_t RxState; 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; uint32_t TOW;
uint16_t WNc; uint16_t WNc;
@ -99,7 +107,7 @@ private:
uint8_t Misc; uint8_t Misc;
}; };
struct PACKED msg4001 struct PACKED msg4001 // DOP
{ {
uint32_t TOW; uint32_t TOW;
uint16_t WNc; 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 // 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 { union PACKED msgbuffer {
msg4007 msg4007u; msg4007 msg4007u;
msg4001 msg4001u; msg4001 msg4001u;
msg4014 msg4014u; msg4014 msg4014u;
msg5908 msg5908u;
uint8_t bytes[128]; uint8_t bytes[128];
}; };