aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_GPS: added detailed hardware status logging for uBlox

Browse Source 
this gives us noise, jamming and RF information from the uBlox
protocol. This will hopefully allow a more detailed investigation of
antenna choices and RF interference
This commit is contained in:
Andrew Tridgell 2014-03-24 12:02:37 +11:00
parent 7ff293ca38
commit 5630bb1ef6
2 changed files with 190 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -21,6 +21,7 @@
#include <stdint.h>
#include <AP_HAL.h>
#include <DataFlash.h>
#define UBLOX_DEBUGGING 0
#define UBLOX_FAKE_3DLOCK 0
@ -37,13 +38,23 @@ extern const AP_HAL::HAL& hal;
extern const AP_HAL::HAL& hal;
/*
only do detailed hardware logging on boards likely to have more log
storage space
*/
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define UBLOX_HW_LOGGING 1
#else
#define UBLOX_HW_LOGGING 0
#endif
const prog_char AP_GPS_UBLOX::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY;
const uint8_t AP_GPS_UBLOX::_ublox_set_binary_size = sizeof(AP_GPS_UBLOX::_ublox_set_binary);
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_UBLOX::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
AP_GPS_UBLOX::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash)
{
_port = s;
@ -54,10 +65,12 @@ AP_GPS_UBLOX::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
_port->flush();
_nav_setting = nav_setting;
_DataFlash = DataFlash;
// configure the GPS for the messages we want
_configure_gps();
_nav_setting = nav_setting;
_step = 0;
_new_position = false;
_new_speed = false;
@ -79,27 +92,36 @@ AP_GPS_UBLOX::send_next_rate_update(void)
//hal.console->printf_P(PSTR("next_rate: %u\n"), (unsigned)rate_update_step);
switch (rate_update_step) {
switch (rate_update_step++) {
case 0:
_configure_navigation_rate(200);
break;
case 1:
_configure_message_rate(CLASS_NAV, MSG_POSLLH, 1);
_configure_message_rate(CLASS_NAV, MSG_POSLLH, 1); // 28+8 bytes
break;
case 2:
_configure_message_rate(CLASS_NAV, MSG_STATUS, 1);
_configure_message_rate(CLASS_NAV, MSG_STATUS, 1); // 16+8 bytes
break;
case 3:
_configure_message_rate(CLASS_NAV, MSG_SOL, 1);
_configure_message_rate(CLASS_NAV, MSG_SOL, 1); // 52+8 bytes
break;
case 4:
_configure_message_rate(CLASS_NAV, MSG_VELNED, 1);
_configure_message_rate(CLASS_NAV, MSG_VELNED, 1); // 36+8 bytes
break;
}
rate_update_step++;
if (rate_update_step > 4) {
#if UBLOX_HW_LOGGING
case 5:
// gather MON_HW at 0.5Hz
_configure_message_rate(CLASS_MON, MSG_MON_HW, 2); // 64+8 bytes
break;
case 6:
// gather MON_HW2 at 0.5Hz
_configure_message_rate(CLASS_MON, MSG_MON_HW2, 2); // 24+8 bytes
break;
#endif
default:
need_rate_update = false;
rate_update_step = 0;
break;
}
}
@ -231,6 +253,97 @@ AP_GPS_UBLOX::read(void)
}
// Private Methods /////////////////////////////////////////////////////////////
#if UBLOX_HW_LOGGING
#define LOG_MSG_UBX1 200
#define LOG_MSG_UBX2 201
struct PACKED log_Ubx1 {
LOG_PACKET_HEADER;
uint32_t timestamp;
uint8_t instance;
uint16_t noisePerMS;
uint8_t jamInd;
uint8_t aPower;
};
struct PACKED log_Ubx2 {
LOG_PACKET_HEADER;
uint32_t timestamp;
uint8_t instance;
int8_t ofsI;
uint8_t magI;
int8_t ofsQ;
uint8_t magQ;
};
static const struct LogStructure ubx_log_structures[] PROGMEM = {
{ LOG_MSG_UBX1, sizeof(log_Ubx1),
"UBX1", "IBHBB", "TimeMS,Instance,noisePerMS,jamInd,aPower" },
{ LOG_MSG_UBX2, sizeof(log_Ubx2),
"UBX2", "IBbBbB", "TimeMS,Instance,ofsI,magI,ofsQ,magQ" }
};
void AP_GPS_UBLOX::write_logging_headers(void)
{
if (!logging_started) {
logging_started = true;
_DataFlash->AddLogFormats(ubx_log_structures, 2);
}
}
void AP_GPS_UBLOX::log_mon_hw(void)
{
if (_DataFlash == NULL || !_DataFlash->logging_started()) {
return;
}
// log mon_hw message
write_logging_headers();
struct log_Ubx1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_UBX1),
timestamp : hal.scheduler->millis(),
instance : (uint8_t)(_secondary_gps?1:0),
noisePerMS : _buffer.mon_hw.noisePerMS,
jamInd : _buffer.mon_hw.jamInd,
aPower : _buffer.mon_hw.aPower
};
_DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
void AP_GPS_UBLOX::log_mon_hw2(void)
{
if (_DataFlash == NULL || !_DataFlash->logging_started()) {
return;
}
// log mon_hw message
write_logging_headers();
struct log_Ubx2 pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_UBX2),
timestamp : hal.scheduler->millis(),
instance : (uint8_t)(_secondary_gps?1:0),
ofsI : _buffer.mon_hw2.ofsI,
magI : _buffer.mon_hw2.magI,
ofsQ : _buffer.mon_hw2.ofsQ,
magQ : _buffer.mon_hw2.magQ,
};
_DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
#endif // UBLOX_HW_LOGGING
void AP_GPS_UBLOX::unexpected_message(void)
{
Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
if (++_disable_counter == 0) {
// disable future sends of this message id, but
// only do this every 256 messages, as some
// message types can't be disabled and we don't
// want to get into an ack war
Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
_configure_message_rate(_class, _msg_id, 0);
}
}
bool
AP_GPS_UBLOX::_parse_gps(void)
@ -257,18 +370,23 @@ AP_GPS_UBLOX::_parse_gps(void)
return false;
}
if (_class != CLASS_NAV) {
Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
if (++_disable_counter == 0) {
// disable future sends of this message id, but
// only do this every 256 messages, as some
// message types can't be disabled and we don't
// want to get into an ack war
Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
_configure_message_rate(_class, _msg_id, 0);
#if UBLOX_HW_LOGGING
if (_class == CLASS_MON) {
if (_msg_id == MSG_MON_HW) {
log_mon_hw();
} else if (_msg_id == MSG_MON_HW2) {
log_mon_hw2();
} else {
unexpected_message();
}
return false;
}
#endif // UBLOX_HW_LOGGING
if (_class != CLASS_NAV) {
unexpected_message();
return false;
}
switch (_msg_id) {
case MSG_POSLLH:
@ -348,7 +466,6 @@ AP_GPS_UBLOX::_parse_gps(void)
break;
case MSG_VELNED:
Debug("MSG_VELNED");
speed_3d_cm = _buffer.velned.speed_3d; // cm/s
ground_speed_cm = _buffer.velned.speed_2d; // cm/s
ground_course_cd = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
_have_raw_velocity = true;

62
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -50,14 +50,15 @@ public:
_payload_length(0),
_payload_counter(0),
_fix_count(0),
need_rate_update(false),
_disable_counter(0),
next_fix(GPS::FIX_NONE),
need_rate_update(false),
rate_update_step(0)
rate_update_step(0),
_last_hw_status(0)
{}
// Methods
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting);
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
virtual bool read();
static bool _detect(uint8_t );
@ -152,6 +153,37 @@ private:
uint32_t speed_accuracy;
uint32_t heading_accuracy;
};
struct PACKED ubx_mon_hw {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[25];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
};
struct PACKED ubx_mon_hw2 {
int8_t ofsI;
uint8_t magI;
int8_t ofsQ;
uint8_t magQ;
uint8_t cfgSource;
uint8_t reserved0[3];
uint32_t lowLevCfg;
uint32_t reserved1[2];
uint32_t postStatus;
uint32_t reserved2;
};
// Receive buffer
union PACKED {
ubx_nav_posllh posllh;
@ -159,6 +191,8 @@ private:
ubx_nav_solution solution;
ubx_nav_velned velned;
ubx_cfg_nav_settings nav_settings;
ubx_mon_hw mon_hw;
ubx_mon_hw2 mon_hw2;
uint8_t bytes[];
} _buffer;
@ -168,6 +202,7 @@ private:
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
CLASS_MON = 0x0A,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_POSLLH = 0x2,
@ -177,7 +212,9 @@ private:
MSG_CFG_PRT = 0x00,
MSG_CFG_RATE = 0x08,
MSG_CFG_SET_RATE = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24
MSG_CFG_NAV_SETTINGS = 0x24,
MSG_MON_HW = 0x09,
MSG_MON_HW2 = 0x0B
};
enum ubs_nav_fix_type {
FIX_NONE = 0,
@ -204,14 +241,16 @@ private:
// 8 bit count of fix messages processed, used for periodic
// processing
uint8_t _fix_count;
uint8_t _class;
// do we have new position information?
bool _new_position;
bool _new_position:1;
// do we have new speed information?
bool _new_speed;
bool _new_speed:1;
bool need_rate_update:1;
// have we written the logging headers to DataFlash?
bool logging_started:1;
uint8_t _disable_counter;
@ -221,9 +260,9 @@ private:
// used to update fix between status and position packets
Fix_Status next_fix;
bool need_rate_update;
uint8_t rate_update_step;
uint32_t _last_5hz_time;
uint32_t _last_hw_status;
void _configure_navigation_rate(uint16_t rate_ms);
void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
@ -232,6 +271,11 @@ private:
void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
void send_next_rate_update(void);
void unexpected_message(void);
void write_logging_headers(void);
void log_mon_hw(void);
void log_mon_hw2(void);
};
#endif // __AP_GPS_UBLOX_H__