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

AP_GPS: ability to log GPS detection state to dataflash

This commit is contained in:
Peter Barker 2016-08-01 21:58:23 +10:00 committed by Francisco Ferreira
parent 94a5e94ec9
commit 2354a0ec5a
17 changed files with 83 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AP_GPS/AP_GPS.cpp
View File

@ -396,7 +396,7 @@ void AP_GPS::detect_instance(uint8_t instance)
switch (_type[instance]) {
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
case GPS_TYPE_QURT:
_broadcast_gps_type("QURTGPS", instance, -1); // baud rate isn't valid
dstate->detect_started_ms = 0; // specified, not detected
new_gps = new AP_GPS_QURT(*this, state[instance], _port[instance]);
goto found_gps;
break;
@ -405,7 +405,7 @@ void AP_GPS::detect_instance(uint8_t instance)
// user has to explicitly set the MAV type, do not use AUTO
// do not try to detect the MAV type, assume it's there
case GPS_TYPE_MAV:
_broadcast_gps_type("MAV", instance, -1);
dstate->detect_started_ms = 0; // specified, not detected
new_gps = new AP_GPS_MAV(*this, state[instance], nullptr);
goto found_gps;
break;
@ -414,8 +414,8 @@ void AP_GPS::detect_instance(uint8_t instance)
// user has to explicitly set the UAVCAN type, do not use AUTO
// do not try to detect the UAVCAN type, assume it's there
case GPS_TYPE_UAVCAN:
dstate->detect_started_ms = 0; // specified, not detected
if (AP_BoardConfig::get_can_enable() >= 1) {
_broadcast_gps_type("UAVCAN", instance, -1);
new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr);
// register new listener at first empty node
@ -451,17 +451,14 @@ void AP_GPS::detect_instance(uint8_t instance)
switch (_type[instance]) {
// by default the sbf/trimble gps outputs no data on its port, until configured.
case GPS_TYPE_SBF:
_broadcast_gps_type("SBF", instance, -1); // baud rate isn't valid
new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]);
break;
case GPS_TYPE_GSOF:
_broadcast_gps_type("GSOF", instance, -1); // baud rate isn't valid
new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]);
break;
case GPS_TYPE_NOVA:
_broadcast_gps_type("NOVA", instance, -1); // baud rate isn't valid
new_gps = new AP_GPS_NOVA(*this, state[instance], _port[instance]);
break;
@ -511,7 +508,6 @@ void AP_GPS::detect_instance(uint8_t instance)
((!_auto_config && _baudrates[dstate->current_baud] >= 38400) ||
_baudrates[dstate->current_baud] == 115200) &&
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
_broadcast_gps_type("u-blox", instance, dstate->current_baud);
new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]);
}
#if !HAL_MINIMIZE_FEATURES
@ -519,41 +515,34 @@ void AP_GPS::detect_instance(uint8_t instance)
// and are surprisingly large
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) &&
AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) {
_broadcast_gps_type("MTK19", instance, dstate->current_baud);
new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]);
} else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) &&
AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) {
_broadcast_gps_type("MTK", instance, dstate->current_baud);
new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]);
}
#endif
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) {
_broadcast_gps_type("SBP", instance, dstate->current_baud);
new_gps = new AP_GPS_SBP2(*this, state[instance], _port[instance]);
}
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
_broadcast_gps_type("SBP", instance, dstate->current_baud);
new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]);
}
#if !HAL_MINIMIZE_FEATURES
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&
AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
_broadcast_gps_type("SIRF", instance, dstate->current_baud);
new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]);
}
#endif
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) &&
AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) {
_broadcast_gps_type("ERB", instance, dstate->current_baud);
new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]);
} else if (now - dstate->detect_started_ms > (ARRAY_SIZE(_baudrates) * GPS_BAUD_TIME_MS)) {
// prevent false detection of NMEA mode in
// a MTK or UBLOX which has booted in NMEA mode
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) &&
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
_broadcast_gps_type("NMEA", instance, dstate->current_baud);
new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
}
}
@ -561,6 +550,7 @@ void AP_GPS::detect_instance(uint8_t instance)
found_gps:
if (new_gps != nullptr) {
_broadcast_gps_type(instance);
state[instance].status = NO_FIX;
drivers[instance] = new_gps;
timing[instance].last_message_time_ms = now;
@ -958,25 +948,38 @@ bool AP_GPS::blend_health_check() const
return (_blend_health_counter < 50);
}
void AP_GPS::_broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index)
void AP_GPS::_detection_message(char *buffer, uint8_t buflen, uint8_t instance)
{
char buffer[64];
if (baud_index >= 0) {
hal.util->snprintf(buffer, sizeof(buffer),
"GPS %d: detected as %s at %d baud",
instance + 1,
type,
_baudrates[baud_index]);
} else {
hal.util->snprintf(buffer, sizeof(buffer),
"GPS %d: detected as %s",
instance + 1,
type);
AP_GPS_Backend *driver = drivers[instance];
if (driver == nullptr) {
// we really shouldn't have been called
buffer[0] = '\0';
return;
}
const struct detect_state dstate = detect_state[instance];
if (dstate.detect_started_ms > 0) {
hal.util->snprintf(buffer, buflen,
"GPS %d: detected as %s at %d baud",
instance + 1,
driver->name(),
_baudrates[dstate.current_baud]);
} else {
hal.util->snprintf(buffer, buflen,
"GPS %d: specified as %s",
instance + 1,
driver->name());
}
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, buffer);
}
void AP_GPS::_broadcast_gps_type(uint8_t instance)
{
char buffer[64];
_detection_message(buffer, sizeof(buffer), instance);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, buffer);
}
/*
re-assemble GPS_RTCM_DATA message
*/
@ -1060,6 +1063,22 @@ void AP_GPS::inject_data_all(const uint8_t *data, uint16_t len)
}
}
void AP_GPS::Write_DataFlash_Log_Startup_messages()
{
if (_DataFlash == nullptr) {
return;
}
for (uint8_t instance=0; instance<num_instances; instance++) {
char buffer[64];
_detection_message(buffer, sizeof(buffer), instance);
if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
continue;
}
_DataFlash->Log_Write_Message(buffer);
}
}
/*
return the expected lag (in seconds) in the position and velocity readings from the gps
*/

6
libraries/AP_GPS/AP_GPS.h
View File

@ -364,6 +364,8 @@ public:
static const struct AP_Param::GroupInfo var_info[];
void Write_DataFlash_Log_Startup_messages();
protected:
// dataflash for logging, if available
@ -441,7 +443,9 @@ private:
void detect_instance(uint8_t instance);
void update_instance(uint8_t instance);
void _broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index);
void _detection_message(char *buffer, uint8_t buflen, uint8_t instance);
void _broadcast_gps_type(uint8_t instance);
/*
buffer for re-assembling RTCM data for GPS injection.

2
libraries/AP_GPS/AP_GPS_ERB.h
View File

@ -36,6 +36,8 @@ public:
static bool _detect(struct ERB_detect_state &state, uint8_t data);
const char *name() const override { return "ERB"; }
private:
struct PACKED erb_header {
uint8_t preamble1;

2
libraries/AP_GPS/AP_GPS_GSOF.h
View File

@ -34,6 +34,8 @@ public:
// Methods
bool read();
const char *name() const override { return "GSOF"; }
private:
bool parse(uint8_t temp);

2
libraries/AP_GPS/AP_GPS_MAV.h
View File

@ -35,6 +35,8 @@ public:
void handle_msg(const mavlink_message_t *msg);
const char *name() const override { return "MAV"; }
private:
bool _new_data;
};

2
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -36,6 +36,8 @@ public:
static bool _detect(struct MTK_detect_state &state, uint8_t data);
static void send_init_blob(uint8_t instance, AP_GPS &gps);
const char *name() const override { return "MTK"; }
private:
struct PACKED diyd_mtk_msg {
int32_t latitude;

2
libraries/AP_GPS/AP_GPS_MTK19.h
View File

@ -36,6 +36,8 @@ public:
static bool _detect(struct MTK19_detect_state &state, uint8_t data);
const char *name() const override { return "MTK19"; }
private:
struct PACKED diyd_mtk_msg {
int32_t latitude;

2
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -62,6 +62,8 @@ public:
static bool _detect(struct NMEA_detect_state &state, uint8_t data);
const char *name() const override { return "NMEA"; }
private:
/// Coding for the GPS sentences that the parser handles
enum _sentence_types { //there are some more than 10 fields in some sentences , thus we have to increase these value.

2
libraries/AP_GPS/AP_GPS_NOVA.h
View File

@ -34,6 +34,8 @@ public:
void inject_data(const uint8_t *data, uint16_t len) override;
const char *name() const override { return "NOVA"; }
private:
bool parse(uint8_t temp);

2
libraries/AP_GPS/AP_GPS_QURT.h
View File

@ -29,6 +29,8 @@ public:
bool read() override;
const char *name() const override { return "QURTGPS"; }
private:
bool initialised = false;
uint32_t last_tow;

2
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -34,6 +34,8 @@ public:
// Methods
bool read();
const char *name() const override { return "SBF"; }
private:
bool parse(uint8_t temp);

2
libraries/AP_GPS/AP_GPS_SBP.h
View File

@ -38,6 +38,8 @@ public:
static bool _detect(struct SBP_detect_state &state, uint8_t data);
const char *name() const override { return "SBP"; }
private:
// ************************************************************************

2
libraries/AP_GPS/AP_GPS_SBP2.h
View File

@ -38,6 +38,8 @@ public:
static bool _detect(struct SBP2_detect_state &state, uint8_t data);
const char *name() const override { return "SBP2"; }
private:
// ************************************************************************

2
libraries/AP_GPS/AP_GPS_SIRF.h
View File

@ -35,6 +35,8 @@ public:
static bool _detect(struct SIRF_detect_state &state, uint8_t data);
const char *name() const override { return "SIRF"; }
private:
struct PACKED sirf_geonav {
uint16_t fix_invalid;

3
libraries/AP_GPS/AP_GPS_UAVCAN.h
View File

@ -33,6 +33,9 @@ public:
// This method is called from UAVCAN thread
void handle_gnss_msg(const AP_GPS::GPS_State &msg) override;
const char *name() const override { return "UAVCAN"; }
private:
bool _new_data;

2
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -118,6 +118,8 @@ public:
// return velocity lag
float get_lag(void) const override;
const char *name() const override { return "u-blox"; }
private:
// u-blox UBX protocol essentials
struct PACKED ubx_header {

2
libraries/AP_GPS/GPS_Backend.h
View File

@ -56,6 +56,8 @@ public:
// driver specific lag
virtual float get_lag(void) const { return 0.2f; }
virtual const char *name() const = 0;
protected:
AP_HAL::UARTDriver *port; ///< UART we are attached to
AP_GPS &gps; ///< access to frontend (for parameters)