diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-23 13:41:27 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-23 13:41:27 +0200 |
| commit | 98d5ed5e7357659951a067eb8e396be9c2c59a58 (patch) | |
| tree | 1a8fa20cdfbcb69da4fe5a7e53356fb98f759ae6 | |
| parent | b3d6dcb2e5a1f66c42d575f13cbc5a7eef16db27 (diff) | |
| download | px4-firmware-98d5ed5e7357659951a067eb8e396be9c2c59a58.tar.gz px4-firmware-98d5ed5e7357659951a067eb8e396be9c2c59a58.tar.bz2 px4-firmware-98d5ed5e7357659951a067eb8e396be9c2c59a58.zip | |
sdlog2: Fix GPS sat offset math
| -rw-r--r-- | src/modules/sdlog2/sdlog2.c | 17 |
1 files changed, 13 insertions, 4 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 873f04828..b6b039d26 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1069,9 +1069,13 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A)); /* fill set A */ for (unsigned i = 0; i < gps_msg_max_snr; i++) { - if (buf_gps_pos.satellite_prn[i] < log_max_snr) { + + int satindex = buf_gps_pos.satellite_prn[i] - 1; + + /* handles index exceeding and wraps to to arithmetic errors */ + if ((satindex >= 0) && (satindex < log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0A.satellite_snr[buf_gps_pos.satellite_prn[i]] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0A); @@ -1080,9 +1084,14 @@ int sdlog2_thread_main(int argc, char *argv[]) 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++) { - if (buf_gps_pos.satellite_prn[i] < log_max_snr) { + + /* 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 < log_max_snr)) { /* map satellites by their ID so that logs from two receivers can be compared */ - log_msg.body.log_GS0B.satellite_snr[buf_gps_pos.satellite_prn[i]] = buf_gps_pos.satellite_snr[i]; + log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i]; } } LOGBUFFER_WRITE_AND_COUNT(GS0B); |