Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: correct use of simulated GPS data 

Co-authored-by: Andrew Tridgell <andrew@tridgell.net>

uses the elements out of the simulated GPS data "d", rather than going to the parameters in teh simulated GPS "backends".
This commit is contained in:
Peter Barker 2024-08-24 16:35:01 +10:00 committed by Andrew Tridgell
parent b89256bec4
commit 46db6c12c1
9 changed files with 32 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/SITL/SIM_GPS.cpp
View File

@ -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);

6
libraries/SITL/SIM_GPS_MSP.cpp
View File

@ -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;

6
libraries/SITL/SIM_GPS_NMEA.cpp
View File

@ -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);

2
libraries/SITL/SIM_GPS_NOVA.cpp
View File

@ -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;

2
libraries/SITL/SIM_GPS_SBF.cpp
View File

@ -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;

8
libraries/SITL/SIM_GPS_SBP.cpp
View File

@ -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);

8
libraries/SITL/SIM_GPS_SBP2.cpp
View File

@ -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);

2
libraries/SITL/SIM_GPS_Trimble.cpp
View File

@ -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

16
libraries/SITL/SIM_GPS_UBLOX.cpp
View File

@ -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);