From 46db6c12c13330f1b4390a77015e98c2122038f7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 24 Aug 2024 16:35:01 +1000 Subject: [PATCH] SITL: correct use of simulated GPS data Co-authored-by: Andrew Tridgell 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 | 7 +++++++ libraries/SITL/SIM_GPS_MSP.cpp | 6 +++--- libraries/SITL/SIM_GPS_NMEA.cpp | 6 +++--- libraries/SITL/SIM_GPS_NOVA.cpp | 2 +- libraries/SITL/SIM_GPS_SBF.cpp | 2 +- libraries/SITL/SIM_GPS_SBP.cpp | 8 ++++---- libraries/SITL/SIM_GPS_SBP2.cpp | 8 ++++---- libraries/SITL/SIM_GPS_Trimble.cpp | 2 +- libraries/SITL/SIM_GPS_UBLOX.cpp | 16 ++++++++-------- 9 files changed, 32 insertions(+), 25 deletions(-) diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index a6e2564cf9..35fe90d90a 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/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); diff --git a/libraries/SITL/SIM_GPS_MSP.cpp b/libraries/SITL/SIM_GPS_MSP.cpp index 65d8838b7d..66bde9c22c 100644 --- a/libraries/SITL/SIM_GPS_MSP.cpp +++ b/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; diff --git a/libraries/SITL/SIM_GPS_NMEA.cpp b/libraries/SITL/SIM_GPS_NMEA.cpp index 30c705382e..23d2a91ad4 100644 --- a/libraries/SITL/SIM_GPS_NMEA.cpp +++ b/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); diff --git a/libraries/SITL/SIM_GPS_NOVA.cpp b/libraries/SITL/SIM_GPS_NOVA.cpp index 08f793f4d2..808d6b19cc 100644 --- a/libraries/SITL/SIM_GPS_NOVA.cpp +++ b/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; diff --git a/libraries/SITL/SIM_GPS_SBF.cpp b/libraries/SITL/SIM_GPS_SBF.cpp index 13ccf1ab6f..5df5501756 100644 --- a/libraries/SITL/SIM_GPS_SBF.cpp +++ b/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; diff --git a/libraries/SITL/SIM_GPS_SBP.cpp b/libraries/SITL/SIM_GPS_SBP.cpp index 9f4c8078e8..a3b11f2e87 100644 --- a/libraries/SITL/SIM_GPS_SBP.cpp +++ b/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); diff --git a/libraries/SITL/SIM_GPS_SBP2.cpp b/libraries/SITL/SIM_GPS_SBP2.cpp index 3acd391e06..f7b33361f3 100644 --- a/libraries/SITL/SIM_GPS_SBP2.cpp +++ b/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); diff --git a/libraries/SITL/SIM_GPS_Trimble.cpp b/libraries/SITL/SIM_GPS_Trimble.cpp index 9bb3edd541..4e5883ef45 100644 --- a/libraries/SITL/SIM_GPS_Trimble.cpp +++ b/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 diff --git a/libraries/SITL/SIM_GPS_UBLOX.cpp b/libraries/SITL/SIM_GPS_UBLOX.cpp index 507d767df9..ce77a44d28 100644 --- a/libraries/SITL/SIM_GPS_UBLOX.cpp +++ b/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);