forked from Archive/PX4-Autopilot
Fixed up SNR logging to include maximum 32 satellites (which is plenty even for future, not yet deployed global positioning systems)
This commit is contained in:
parent
fb801b6fae
commit
9024d76e7c
|
@ -834,8 +834,10 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
struct log_ESTM_s log_ESTM;
|
||||
struct log_PWR_s log_PWR;
|
||||
struct log_VICN_s log_VICN;
|
||||
struct log_GSN0_s log_GSN0;
|
||||
struct log_GSN1_s log_GSN1;
|
||||
struct log_GS0A_s log_GS0A;
|
||||
struct log_GS0B_s log_GS0B;
|
||||
struct log_GS1A_s log_GS1A;
|
||||
struct log_GS1B_s log_GS1B;
|
||||
} body;
|
||||
} log_msg = {
|
||||
LOG_PACKET_HEADER_INIT(0)
|
||||
|
@ -969,7 +971,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
LOGBUFFER_WRITE_AND_COUNT(STAT);
|
||||
}
|
||||
|
||||
/* --- GPS POSITION --- */
|
||||
/* --- GPS POSITION - UNIT #1 --- */
|
||||
if (gps_pos_updated) {
|
||||
log_msg.msg_type = LOG_GPS_MSG;
|
||||
log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
|
||||
|
@ -986,16 +988,33 @@ int sdlog2_thread_main(int argc, char *argv[])
|
|||
LOGBUFFER_WRITE_AND_COUNT(GPS);
|
||||
|
||||
/* log the SNR of each satellite for a detailed view of signal quality */
|
||||
log_msg.msg_type = LOG_GSN0_MSG;
|
||||
/* pick the smaller number so we do not overflow any of the arrays */
|
||||
unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
|
||||
unsigned log_max_snr = sizeof(log_msg.body.log_GSN0.satellite_snr) / sizeof(log_msg.body.log_GSN0.satellite_snr[0]);
|
||||
unsigned sat_max_snr = (gps_msg_max_snr < log_max_snr) ? gps_msg_max_snr : log_max_snr;
|
||||
unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
|
||||
|
||||
for (unsigned i = 0; i < sat_max_snr; i++) {
|
||||
log_msg.body.log_GSN0.satellite_snr[i] = buf_gps_pos.satellite_snr[i];
|
||||
log_msg.msg_type = LOG_GS0A_MSG;
|
||||
memset(log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
|
||||
/* fill set A */
|
||||
unsigned max_sats_a = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr;
|
||||
|
||||
for (unsigned i = 0; i < max_sats_a; i++) {
|
||||
log_msg.body.log_GS0A.satellite_snr[i] = buf_gps_pos.satellite_snr[i];
|
||||
}
|
||||
LOGBUFFER_WRITE_AND_COUNT(GS0A);
|
||||
|
||||
/* do we need a 2nd set? */
|
||||
if (gps_msg_max_snr > log_max_snr) {
|
||||
log_msg.msg_type = LOG_GS0B_MSG;
|
||||
memset(log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
|
||||
/* fill set B - deduct the count we already have taken care of */
|
||||
gps_msg_max_snr -= log_max_snr;
|
||||
unsigned max_sats_b = (log_max_snr > gps_msg_max_snr) ? gps_msg_max_snr : log_max_snr;
|
||||
|
||||
for (unsigned i = 0; i < max_sats_b; i++) {
|
||||
/* count from zero, but obey offset of log_max_snr consumed units */
|
||||
log_msg.body.log_GS0B.satellite_snr[i] = buf_gps_pos.satellite_snr[log_max_snr + i];
|
||||
}
|
||||
LOGBUFFER_WRITE_AND_COUNT(GS0B);
|
||||
}
|
||||
LOGBUFFER_WRITE_AND_COUNT(GSN0);
|
||||
}
|
||||
|
||||
/* --- SENSOR COMBINED --- */
|
||||
|
|
|
@ -318,15 +318,27 @@ struct log_VICN_s {
|
|||
float yaw;
|
||||
};
|
||||
|
||||
/* --- GSN0 - GPS SNR #0 --- */
|
||||
#define LOG_GSN0_MSG 26
|
||||
struct log_GSN0_s {
|
||||
/* --- GS0A - GPS SNR #0, SAT GROUP A --- */
|
||||
#define LOG_GS0A_MSG 26
|
||||
struct log_GS0A_s {
|
||||
uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
|
||||
};
|
||||
|
||||
/* --- GSN1 - GPS SNR #1 --- */
|
||||
#define LOG_GSN1_MSG 27
|
||||
struct log_GSN1_s {
|
||||
/* --- GS0B - GPS SNR #0, SAT GROUP B --- */
|
||||
#define LOG_GS0B_MSG 27
|
||||
struct log_GS0B_s {
|
||||
uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
|
||||
};
|
||||
|
||||
/* --- GS1A - GPS SNR #1, SAT GROUP A --- */
|
||||
#define LOG_GS1A_MSG 28
|
||||
struct log_GS1A_s {
|
||||
uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
|
||||
};
|
||||
|
||||
/* --- GS1B - GPS SNR #1, SAT GROUP B --- */
|
||||
#define LOG_GS1B_MSG 29
|
||||
struct log_GS1B_s {
|
||||
uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
|
||||
};
|
||||
|
||||
|
@ -381,8 +393,10 @@ static const struct log_format_s log_formats[] = {
|
|||
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
|
||||
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
|
||||
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
|
||||
LOG_FORMAT(GSN0, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(GSN1, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
|
||||
/* system-level messages, ID >= 0x80 */
|
||||
/* FMT: don't write format of format message, it's useless */
|
||||
|
|
Loading…
Reference in New Issue