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

AP_GPS: Add support for logging ublox RXM-RAWX messages 

Can only record 32 satellites raw info before memory corruption occurs. (And raised the limit on normal RXM-RAW messages to 22)
This commit is contained in:
Michael du Breuil 2015-06-29 15:04:35 -07:00 committed by Andrew Tridgell
parent d665c85c20
commit a382ce2b0c
2 changed files with 74 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -123,9 +123,12 @@ AP_GPS_UBLOX::send_next_rate_update(void)
case 7:
_configure_message_rate(CLASS_RXM, MSG_RXM_RAW, gps._raw_data);
break;
case 8:
_configure_message_rate(CLASS_RXM, MSG_RXM_RAWX, gps._raw_data);
break;
#endif
#if UBLOX_VERSION_AUTODETECTION
case 8:
case 9:
_request_version();
break;
#endif
@ -348,6 +351,45 @@ void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
}
void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
uint64_t now = hal.scheduler->micros64();
struct log_GPS_RAWH header = {
LOG_PACKET_HEADER_INIT(LOG_GPS_RAWH_MSG),
time_us : now,
rcvTow : raw.rcvTow,
week : raw.week,
leapS : raw.leapS,
numMeas : raw.numMeas,
recStat : raw.recStat
};
gps._DataFlash->WriteBlock(&header, sizeof(header));
for (uint8_t i=0; i<raw.numMeas; i++) {
struct log_GPS_RAWS pkt = {
LOG_PACKET_HEADER_INIT(LOG_GPS_RAWS_MSG),
time_us : now,
prMes : raw.svinfo[i].prMes,
cpMes : raw.svinfo[i].cpMes,
doMes : raw.svinfo[i].doMes,
gnssId : raw.svinfo[i].gnssId,
svId : raw.svinfo[i].svId,
freqId : raw.svinfo[i].freqId,
locktime : raw.svinfo[i].locktime,
cno : raw.svinfo[i].cno,
prStdev : raw.svinfo[i].prStdev,
cpStdev : raw.svinfo[i].cpStdev,
doStdev : raw.svinfo[i].doStdev,
trkStat : raw.svinfo[i].trkStat
};
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
}
#endif // UBLOX_RXM_RAW_LOGGING
void AP_GPS_UBLOX::unexpected_message(void)
@ -436,6 +478,9 @@ AP_GPS_UBLOX::_parse_gps(void)
if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
log_rxm_raw(_buffer.rxm_raw);
return false;
} else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
log_rxm_rawx(_buffer.rxm_rawx);
return false;
}
#endif // UBLOX_RXM_RAW_LOGGING

30
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -42,7 +42,8 @@
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define UBLOX_RXM_RAW_LOGGING 1
#define UBLOX_MAX_RXM_RAW_SATS 16
#define UBLOX_MAX_RXM_RAW_SATS 22
#define UBLOX_MAX_RXM_RAWX_SATS 32
#else
#define UBLOX_RXM_RAW_LOGGING 0
#endif
@ -224,6 +225,28 @@ private:
uint8_t lli;
} svinfo[UBLOX_MAX_RXM_RAW_SATS];
};
struct PACKED ubx_rxm_rawx {
double rcvTow;
uint16_t week;
int8_t leapS;
uint8_t numMeas;
uint8_t recStat;
uint8_t reserved1[3];
struct ubx_rxm_rawx_sv {
double prMes;
double cpMes;
float doMes;
uint8_t gnssId;
uint8_t svId;
uint8_t freqId;
uint16_t locktime;
uint8_t cno;
uint8_t prStdev;
uint8_t cpStdev;
uint8_t doStdev;
uint8_t trkStat;
} svinfo[UBLOX_MAX_RXM_RAWX_SATS];
};
#endif
// Receive buffer
union PACKED {
@ -239,6 +262,7 @@ private:
ubx_nav_svinfo_header svinfo_header;
#if UBLOX_RXM_RAW_LOGGING
ubx_rxm_raw rxm_raw;
ubx_rxm_rawx rxm_rawx;
#endif
uint8_t bytes[];
} _buffer;
@ -265,7 +289,8 @@ private:
MSG_MON_HW = 0x09,
MSG_MON_HW2 = 0x0B,
MSG_NAV_SVINFO = 0x30,
MSG_RXM_RAW = 0x10
MSG_RXM_RAW = 0x10,
MSG_RXM_RAWX = 0x15
};
enum ubs_nav_fix_type {
FIX_NONE = 0,
@ -338,6 +363,7 @@ private:
void log_mon_hw2(void);
void log_accuracy(void);
void log_rxm_raw(const struct ubx_rxm_raw &raw);
void log_rxm_rawx(const struct ubx_rxm_rawx &raw);
};
#endif // __AP_GPS_UBLOX_H__