diff options
Diffstat (limited to 'src/modules/sdlog2')
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 55 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 14 |
2 files changed, 46 insertions, 23 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 81c972112..577cadfbb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -143,8 +143,8 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ -static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ +static const unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ +static const unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; @@ -671,7 +671,7 @@ int write_formats(int fd) int written = 0; /* fill message format packet for each format and write it */ - for (int i = 0; i < log_formats_num; i++) { + for (unsigned i = 0; i < log_formats_num; i++) { log_msg_format.body = log_formats[i]; written += write(fd, &log_msg_format, sizeof(log_msg_format)); } @@ -1114,6 +1114,15 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { + + float snr_mean = 0.0f; + + for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) { + snr_mean += buf_gps_pos.satellite_snr[i]; + } + + snr_mean /= buf_gps_pos.satellites_visible; + log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; @@ -1126,6 +1135,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s; log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; + log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible; + log_msg.body.log_GPS.snr_mean = snr_mean; + log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms; + log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator; LOGBUFFER_WRITE_AND_COUNT(GPS); if (_extended_logging) { @@ -1136,27 +1149,33 @@ int sdlog2_thread_main(int argc, char *argv[]) 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 < gps_msg_max_snr; i++) { - for (unsigned i = 0; i < max_sats_a; i++) { - log_msg.body.log_GS0A.satellite_snr[i] = buf_gps_pos.satellite_snr[i]; + int satindex = buf_gps_pos.satellite_prn[i] - 1; + + /* handles index exceeding and wraps to to arithmetic errors */ + if ((satindex >= 0) && (satindex < (int)log_max_snr)) { + /* map satellites by their ID so that logs from two receivers can be compared */ + log_msg.body.log_GS0A.satellite_snr[satindex] = 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]; + log_msg.msg_type = LOG_GS0B_MSG; + memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B)); + /* fill set B */ + for (unsigned i = 0; i < gps_msg_max_snr; i++) { + + /* get second bank of satellites, thus deduct bank size from index */ + int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr; + + /* handles index exceeding and wraps to to arithmetic errors */ + if ((satindex >= 0) && (satindex < (int)log_max_snr)) { + /* map satellites by their ID so that logs from two receivers can be compared */ + log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } - LOGBUFFER_WRITE_AND_COUNT(GS0B); } + LOGBUFFER_WRITE_AND_COUNT(GS0B); } } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 85ef4da39..f4d88f079 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -139,6 +139,10 @@ struct log_GPS_s { float vel_e; float vel_d; float cog; + uint8_t sats; + uint16_t snr_mean; + uint16_t noise_per_ms; + uint16_t jamming_indicator; }; /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */ @@ -321,25 +325,25 @@ struct log_VICN_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. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- 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. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- 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. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /* --- 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. */ + uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -375,7 +379,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), - LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), + LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), |