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

AP_GPS: SBF if RAW_DATA is enabled check that the GPS is succeeding at logging

This commit is contained in:
Michael du Breuil 2017-04-30 00:31:26 -07:00 committed by Francisco Ferreira
parent 708b483d77
commit 12f83ecf75
2 changed files with 44 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -21,6 +21,7 @@
#include "AP_GPS.h"
#include "AP_GPS_SBF.h"
#include <DataFlash/DataFlash.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
@ -198,7 +199,7 @@ AP_GPS_SBF::process_message(void)
log_ExtEventPVTGeodetic(sbf_msg.data.msg4007u);
}
// PVTGeodetic
if (blockid == 4007) {
else if (blockid == 4007) {
const msg4007 &temp = sbf_msg.data.msg4007u;
// Update time state
@ -281,14 +282,33 @@ AP_GPS_SBF::process_message(void)
return true;
}
// DOP
if (blockid == 4001) {
else if (blockid == 4001) {
const msg4001 &temp = sbf_msg.data.msg4001u;
last_hdop = temp.HDOP;
state.hdop = last_hdop;
}
// ReceiverStatus
else if (blockid == 4014) {
const msg4014 &temp = sbf_msg.data.msg4014u;
RxState = temp.RxState;
}
return false;
}
void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
{
if (gps._raw_data) {
if (!(RxState & SBF_DISK_MOUNTED)){
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS %d: SBF disk is not mounted", state.instance + 1);
}
else if (RxState & SBF_DISK_FULL) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS %d: SBF disk is full", state.instance + 1);
}
else if (!(RxState & SBF_DISK_ACTIVITY)) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS %d: SBF is not currently logging", state.instance + 1);
}
}
}

23
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -23,6 +23,9 @@
#include "GPS_Backend.h"
#define SBF_SETUP_MSG "\nsso, Stream1, COM1, PVTGeodetic+DOP+ExtEventPVTGeodetic, msec100\n"
#define SBF_DISK_ACTIVITY (1 << 7)
#define SBF_DISK_FULL (1 << 8)
#define SBF_DISK_MOUNTED (1 << 9)
class AP_GPS_SBF : public AP_GPS_Backend
{
@ -36,6 +39,10 @@ public:
const char *name() const override { return "SBF"; }
bool is_configured (void) override { return (!gps._raw_data || (RxState & SBF_DISK_ACTIVITY)); }
void broadcast_configuration_failure_reason(void) const override;
private:
bool parse(uint8_t temp);
@ -47,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, msec100\n",
"sso, Stream1, COM1, PVTGeodetic+DOP+ExtEventPVTGeodetic+ReceiverStatus, msec100\n",
"srd, Moderate, UAV\n",
"sem, PVT, 5\n",
"spm, Rover, StandAlone+SBAS+DGPS+RTK\n",
@ -57,6 +64,7 @@ private:
uint32_t crc_error_counter = 0;
uint32_t last_injected_data_ms = 0;
bool validcommand = false;
uint32_t RxState;
struct PACKED msg4007
{
@ -106,9 +114,22 @@ private:
float VPL;
};
struct PACKED msg4014 // ReceiverStatus (v2)
{
uint32_t TOW;
uint16_t WNc;
uint8_t CPULoad;
uint8_t ExtError;
uint32_t UpTime;
uint32_t RxState;
uint32_t RxError;
// remaining data is AGCData, which we don't have a use for, don't extract the data
};
union PACKED msgbuffer {
msg4007 msg4007u;
msg4001 msg4001u;
msg4014 msg4014u;
uint8_t bytes[128];
};