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

AP_GPS: added logging of RXM_RAW messages for UBlox 

this will allow for post-flight RTK processing of DF logs
This commit is contained in:
Micheal Knight 2015-05-04 18:18:34 +10:00 committed by Andrew Tridgell
parent f8e09da50e
commit ee85d37ee2
4 changed files with 78 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_GPS/AP_GPS.cpp
View File

@ -98,6 +98,14 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
AP_GROUPINFO("SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, 0xFF00),
#endif
#if GPS_RTK_AVAILABLE
// @Param: RXM_RAW
// @DisplayName: Raw data logging
// @Description: Enable logging of RXM raw data from uBlox which includes carrier phase and pseudo range information. This allows for post processing of dataflash logs for more precise positioning. Note that this requires a raw capable uBlox such as the 6P or 6T.
// @Values: 0:Disabled,1:log at 1MHz,5:log at 5MHz.
AP_GROUPINFO("RAW_DATA", 9, AP_GPS, _raw_data, 0),
#endif
AP_GROUPEND
};

1
libraries/AP_GPS/AP_GPS.h
View File

@ -333,6 +333,7 @@ public:
#endif
AP_Int8 _sbas_mode;
AP_Int8 _min_elevation;
AP_Int8 _raw_data;
// handle sending of initialisation strings to the GPS
void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);

45
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -47,8 +47,10 @@ extern const AP_HAL::HAL& hal;
*/
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define UBLOX_HW_LOGGING 1
#define UBLOX_RXM_RAW_LOGGING 1
#else
#define UBLOX_HW_LOGGING 0
#define UBLOX_RXM_RAW_LOGGING 0
#endif
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
@ -121,6 +123,11 @@ AP_GPS_UBLOX::send_next_rate_update(void)
case 7:
_request_version();
break;
#endif
#if UBLOX_RXM_RAW_LOGGING
case 8:
_configure_message_rate(CLASS_RXM, MSG_RXM_RAW, gps._raw_data); // 24*16+8 bytes
break;
#endif
default:
need_rate_update = false;
@ -316,6 +323,33 @@ void AP_GPS_UBLOX::log_accuracy(void) {
}
#endif // UBLOX_HW_LOGGING
#if UBLOX_RXM_RAW_LOGGING
void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
uint32_t now = hal.scheduler->millis();
for (uint8_t i=0; i<raw.numSV; i++) {
struct log_GPS_RAW pkt = {
LOG_PACKET_HEADER_INIT(LOG_GPS_RAW_MSG),
timestamp : now,
iTOW : raw.iTOW,
week : raw.week,
numSV : raw.numSV,
sv : raw.svinfo[i].sv,
cpMes : raw.svinfo[i].cpMes,
prMes : raw.svinfo[i].prMes,
doMes : raw.svinfo[i].doMes,
mesQI : raw.svinfo[i].mesQI,
cno : raw.svinfo[i].cno,
lli : raw.svinfo[i].lli
};
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
}
#endif // UBLOX_RXM_RAW_LOGGING
void AP_GPS_UBLOX::unexpected_message(void)
{
Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
@ -398,6 +432,13 @@ AP_GPS_UBLOX::_parse_gps(void)
}
#endif // UBLOX_HW_LOGGING
#if UBLOX_RXM_RAW_LOGGING
if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
log_rxm_raw(_buffer.rxm_raw);
return false;
}
#endif // UBLOX_RXM_RAW_LOGGING
if (_class != CLASS_NAV) {
unexpected_message();
return false;
@ -573,7 +614,7 @@ AP_GPS_UBLOX::_parse_gps(void)
* update checksum for a set of bytes
*/
void
AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b)
AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b)
{
while (len--) {
ck_a += *data;
@ -587,7 +628,7 @@ AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_
* send a ublox message
*/
void
AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size)
{
struct ubx_header header;
uint8_t ck_a=0, ck_b=0;

32
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -38,6 +38,7 @@
* would mean we would never detect it.
*/
#define UBLOX_SET_BINARY "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0003,0001,38400,0*26\r\n"
#define UBLOX_MAX_RXM_RAW_SATS 16
class AP_GPS_UBLOX : public AP_GPS_Backend
{
@ -200,6 +201,21 @@ private:
uint8_t globalFlags;
uint16_t reserved;
};
struct PACKED ubx_rxm_raw {
int32_t iTOW;
int16_t week;
uint8_t numSV;
uint8_t reserved1;
struct ubx_rxm_raw_sv {
double cpMes;
double prMes;
float doMes;
uint8_t sv;
int8_t mesQI;
int8_t cno;
uint8_t lli;
} svinfo[UBLOX_MAX_RXM_RAW_SATS];
};
// Receive buffer
union PACKED {
ubx_nav_posllh posllh;
@ -212,6 +228,7 @@ private:
ubx_mon_hw2 mon_hw2;
ubx_cfg_sbas sbas;
ubx_nav_svinfo_header svinfo_header;
ubx_rxm_raw rxm_raw;
uint8_t bytes[];
} _buffer;
@ -222,6 +239,7 @@ private:
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
CLASS_MON = 0x0A,
CLASS_RXM = 0x02,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_POSLLH = 0x2,
@ -235,7 +253,8 @@ private:
MSG_CFG_SBAS = 0x16,
MSG_MON_HW = 0x09,
MSG_MON_HW2 = 0x0B,
MSG_NAV_SVINFO = 0x30
MSG_NAV_SVINFO = 0x30,
MSG_RXM_RAW = 0x10
};
enum ubs_nav_fix_type {
FIX_NONE = 0,
@ -266,9 +285,9 @@ private:
uint16_t _payload_length;
uint16_t _payload_counter;
// 8 bit count of fix messages processed, used for periodic
// processing
uint8_t _fix_count;
// 8 bit count of fix messages processed, used for periodic
// processing
uint8_t _fix_count;
uint8_t _class;
uint32_t _last_vel_time;
@ -296,8 +315,8 @@ private:
void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void _configure_gps(void);
void _configure_sbas(bool enable);
void _update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b);
void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
void _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b);
void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size);
void send_next_rate_update(void);
void _request_version(void);
@ -306,6 +325,7 @@ private:
void log_mon_hw(void);
void log_mon_hw2(void);
void log_accuracy(void);
void log_rxm_raw(const struct ubx_rxm_raw &raw);
};
#endif // __AP_GPS_UBLOX_H__