SITL: correct use of simulated GPS data

Co-authored-by: Andrew Tridgell <[email protected]>

uses the elements out of the simulated GPS data "d", rather than going to the parameters in teh simulated GPS "backends".

libraries/SITL/SIM_GPS.cpp

@@ -356,6 +356,7 @@ void GPS::update()
 
     last_write_update_ms = now_ms;
 
+    d.num_sats = _sitl->gps_numsats[idx];
     d.latitude = latitude;
     d.longitude = longitude;
     d.yaw_deg = _sitl->state.yawDeg;
@@ -370,8 +371,14 @@ void GPS::update()
     d.speedN = speedN + (velErrorNED.x * rand_float());
     d.speedE = speedE + (velErrorNED.y * rand_float());
     d.speedD = speedD + (velErrorNED.z * rand_float());
+
     d.have_lock = have_lock;
 
+    // fill in accuracies
+    d.horizontal_acc = _sitl->gps_accuracy[idx];
+    d.vertical_acc = _sitl->gps_accuracy[idx];
+    d.speed_acc = _sitl->gps_vel_err[instance].get().xy().length();
+
     if (_sitl->gps_drift_alt[idx] > 0) {
         // add slow altitude drift controlled by a slow sine wave
         d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f);

libraries/SITL/SIM_GPS_MSP.cpp

@@ -63,9 +63,9 @@ void GPS_MSP::publish(const GPS_Data *d)
     msp_gps.gps_week = t.week;
     msp_gps.ms_tow = t.ms;
     msp_gps.fix_type = d->have_lock?3:0;
-    msp_gps.satellites_in_view = d->have_lock ? _sitl->gps_numsats[instance] : 3;
-    msp_gps.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100;
-    msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100;
+    msp_gps.satellites_in_view = d->have_lock ? d->num_sats : 3;
+    msp_gps.horizontal_pos_accuracy = d->horizontal_acc*100;
+    msp_gps.vertical_pos_accuracy = d->vertical_acc*100;
     msp_gps.horizontal_vel_accuracy = 30;
     msp_gps.hdop = 100;
     msp_gps.longitude = d->longitude * 1.0e7;

libraries/SITL/SIM_GPS_NMEA.cpp

@@ -74,7 +74,7 @@ void GPS_NMEA::publish(const GPS_Data *d)
                      lat_string,
                      lng_string,
                      d->have_lock?1:0,
-                     d->have_lock?_sitl->gps_numsats[instance]:3,
+                     d->have_lock?d->num_sats:3,
                      1.2,
                      d->altitude);
 
@@ -118,8 +118,8 @@ void GPS_NMEA::publish(const GPS_Data *d)
                     d->roll_deg,
                     d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed,
                     3, // fixed rtk yaw solution,
-                    d->have_lock?_sitl->gps_numsats[instance]:3,
-                    d->have_lock?_sitl->gps_numsats[instance]:3,
+                    d->have_lock?d->num_sats:3,
+                    d->have_lock?d->num_sats:3,
                     d->speedE * 3.6,
                     d->speedN * 3.6,
                     -d->speedD * 3.6);

libraries/SITL/SIM_GPS_NOVA.cpp

@@ -129,7 +129,7 @@ void GPS_NOVA::publish(const GPS_Data *d)
     bestpos.lat = d->latitude;
     bestpos.lng = d->longitude;
     bestpos.hgt = d->altitude;
-    bestpos.svsused = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    bestpos.svsused = d->have_lock ? d->num_sats : 3;
     bestpos.latsdev=0.2;
     bestpos.lngsdev=0.2;
     bestpos.hgtsdev=0.2;

libraries/SITL/SIM_GPS_SBF.cpp

@@ -134,7 +134,7 @@ void GPS_SBF::publish_PVTGeodetic(const GPS_Data *d)
     pvtGeod_buf.rxclkdrift = DNU_FLOAT;
     pvtGeod_buf.timesystem = DNU_UINT8; 
     pvtGeod_buf.datum = DNU_UINT8; 
-    pvtGeod_buf.nrsv = _sitl->gps_numsats[instance];
+    pvtGeod_buf.nrsv = d->num_sats;
     pvtGeod_buf.wacorrinfo = 0; //default value
     pvtGeod_buf.referenceid = DNU_UINT16; 
     pvtGeod_buf.meancorrage = DNU_UINT16; 

libraries/SITL/SIM_GPS_SBP.cpp

@@ -77,9 +77,9 @@ void GPS_SBP::publish(const GPS_Data *d)
     pos.lon = d->longitude;
     pos.lat= d->latitude;
     pos.height = d->altitude;
-    pos.h_accuracy = _sitl->gps_accuracy[instance]*1000;
-    pos.v_accuracy = _sitl->gps_accuracy[instance]*1000;
-    pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    pos.h_accuracy = d->horizontal_acc*1000;
+    pos.v_accuracy = d->vertical_acc*1000;
+    pos.n_sats = d->have_lock ? d->num_sats : 3;
 
     // Send single point position solution
     pos.flags = 0;
@@ -94,7 +94,7 @@ void GPS_SBP::publish(const GPS_Data *d)
     velned.d = 1e3 * d->speedD;
     velned.h_accuracy = 5e3;
     velned.v_accuracy = 5e3;
-    velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    velned.n_sats = d->have_lock ? d->num_sats : 3;
     velned.flags = 0;
     sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
 

libraries/SITL/SIM_GPS_SBP2.cpp

@@ -77,9 +77,9 @@ void GPS_SBP2::publish(const GPS_Data *d)
     pos.lon = d->longitude;
     pos.lat= d->latitude;
     pos.height = d->altitude;
-    pos.h_accuracy = _sitl->gps_accuracy[instance]*1000;
-    pos.v_accuracy = _sitl->gps_accuracy[instance]*1000;
-    pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    pos.h_accuracy = d->horizontal_acc*1000;
+    pos.v_accuracy = d->vertical_acc*1000;
+    pos.n_sats = d->have_lock ? d->num_sats : 3;
 
     // Send single point position solution
     pos.flags = 1;
@@ -94,7 +94,7 @@ void GPS_SBP2::publish(const GPS_Data *d)
     velned.d = 1e3 * d->speedD;
     velned.h_accuracy = 1e3 * 0.5;
     velned.v_accuracy = 1e3 * 0.5;
-    velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    velned.n_sats = d->have_lock ? d->num_sats : 3;
     velned.flags = 1;
     sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
 

libraries/SITL/SIM_GPS_Trimble.cpp

@@ -93,7 +93,7 @@ void GPS_Trimble::publish(const GPS_Data *d)
                 GSOF_POS_TIME_LEN,
                 htobe32(gps_tow.ms),
                 htobe16(gps_tow.week),
-                d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3),
+                d->have_lock ? d->num_sats : uint8_t(3),
                 pos_flags_1,
                 pos_flags_2,
                 bootcount

libraries/SITL/SIM_GPS_UBLOX.cpp

@@ -195,8 +195,8 @@ void GPS_UBlox::publish(const GPS_Data *d)
     pos.latitude  = d->latitude * 1.0e7;
     pos.altitude_ellipsoid = d->altitude * 1000.0f;
     pos.altitude_msl = d->altitude * 1000.0f;
-    pos.horizontal_accuracy = _sitl->gps_accuracy[instance]*1000;
-    pos.vertical_accuracy = _sitl->gps_accuracy[instance]*1000;
+    pos.horizontal_accuracy = d->horizontal_acc*1000;
+    pos.vertical_accuracy = d->vertical_acc*1000;
 
     status.time = gps_tow.ms;
     status.fix_type = d->have_lock?3:0;
@@ -216,13 +216,13 @@ void GPS_UBlox::publish(const GPS_Data *d)
     if (velned.heading_2d < 0.0f) {
         velned.heading_2d += 360.0f * 100000.0f;
     }
-    velned.speed_accuracy = _sitl->gps_vel_err[instance].get().xy().length() * 100;  // m/s -> cm/s
+    velned.speed_accuracy = d->speed_acc * 100;  // m/s -> cm/s
     velned.heading_accuracy = 4;
 
     memset(&sol, 0, sizeof(sol));
     sol.fix_type = d->have_lock?3:0;
     sol.fix_status = 221;
-    sol.satellites = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    sol.satellites = d->have_lock ? d->num_sats : 3;
     sol.time = gps_tow.ms;
     sol.week = gps_tow.week;
 
@@ -248,13 +248,13 @@ void GPS_UBlox::publish(const GPS_Data *d)
     pvt.fix_type = d->have_lock? 0x3 : 0;
     pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok
     pvt.flags2 =0;
-    pvt.num_sv = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    pvt.num_sv = d->have_lock ? d->num_sats : 3;
     pvt.lon = d->longitude * 1.0e7;
     pvt.lat  = d->latitude * 1.0e7;
     pvt.height = d->altitude * 1000.0f;
     pvt.h_msl = d->altitude * 1000.0f;
-    pvt.h_acc = _sitl->gps_accuracy[instance] * 1000;
-    pvt.v_acc = _sitl->gps_accuracy[instance] * 1000;
+    pvt.h_acc = d->horizontal_acc * 1000;
+    pvt.v_acc = d->vertical_acc * 1000;
     pvt.velN = 1000.0f * d->speedN;
     pvt.velE = 1000.0f * d->speedE;
     pvt.velD = 1000.0f * d->speedD;
@@ -309,7 +309,7 @@ void GPS_UBlox::publish(const GPS_Data *d)
         for (uint8_t i = 0; i < SV_COUNT; i++) {
             svinfo.sv[i].chn = i;
             svinfo.sv[i].svid = i;
-            svinfo.sv[i].flags = (i < _sitl->gps_numsats[instance]) ? 0x7 : 0x6; // sv used, diff correction data, orbit information
+            svinfo.sv[i].flags = (i < d->num_sats) ? 0x7 : 0x6; // sv used, diff correction data, orbit information
             svinfo.sv[i].quality = 7; // code and carrier lock and time synchronized
             svinfo.sv[i].cno = MAX(20, 30 - i);
             svinfo.sv[i].elev = MAX(30, 90 - i);