From ac604cac545debbbdffccc415b1fa34f100412df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 8 Jul 2020 09:53:01 +1000 Subject: [PATCH] HAL_SITL: update for GPS param changes --- libraries/AP_HAL_SITL/SITL_State.cpp | 3 +- libraries/AP_HAL_SITL/sitl_gps.cpp | 79 +++++++++++++--------------- 2 files changed, 37 insertions(+), 45 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 8ca3834b906..09fa4739d51 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -181,8 +181,7 @@ void SITL_State::_fdm_input_step(void) _update_gps(_sitl->state.latitude, _sitl->state.longitude, _sitl->state.altitude, _sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD, - _sitl->state.yawDeg, - !_sitl->gps_disable); + _sitl->state.yawDeg, true); _update_airspeed(_sitl->state.airspeed); _update_rangefinder(_sitl->state.range); diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 0f1aacd2ffb..72190d57684 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -87,13 +87,13 @@ int SITL_State::gps_pipe(uint8_t idx) */ void SITL_State::_gps_write(const uint8_t *p, uint16_t size, uint8_t instance) { - if (instance == 1 && !_sitl->gps2_enable) { + if (instance == 1 && _sitl->gps_disable[instance]) { return; } while (size--) { - if (_sitl->gps_byteloss > 0.0f) { + if (_sitl->gps_byteloss[instance] > 0.0f) { float r = ((((unsigned)random()) % 1000000)) / 1.0e4; - if (r < _sitl->gps_byteloss) { + if (r < _sitl->gps_byteloss[instance]) { // lose the byte p++; continue; @@ -357,7 +357,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance) 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:3; + sol.satellites = d->have_lock?_sitl->gps_numsats[instance]:3; sol.time = time_week_ms; sol.week = time_week; @@ -383,7 +383,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance) 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:3; + pvt.num_sv = d->have_lock?_sitl->gps_numsats[instance]:3; pvt.lon = d->longitude * 1.0e7; pvt.lat = d->latitude * 1.0e7; pvt.height = d->altitude * 1000.0f; @@ -438,7 +438,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance) 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) ? 0x7 : 0x6; // sv used, diff correction data, orbit information + svinfo.sv[i].flags = (i < _sitl->gps_numsats[instance]) ? 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); @@ -497,7 +497,7 @@ void SITL_State::_update_gps_mtk(const struct gps_data *d, uint8_t instance) if (p.ground_course < 0.0f) { p.ground_course += 360.0f * 1000000.0f; } - p.satellites = d->have_lock?_sitl->gps_numsats:3; + p.satellites = d->have_lock?_sitl->gps_numsats[instance]:3; p.fix_type = d->have_lock?3:1; // the spec is not very clear, but the time field seems to be @@ -552,7 +552,7 @@ void SITL_State::_update_gps_mtk16(const struct gps_data *d, uint8_t instance) if (p.ground_course < 0.0f) { p.ground_course += 360.0f * 100.0f; } - p.satellites = d->have_lock?_sitl->gps_numsats:3; + p.satellites = d->have_lock?_sitl->gps_numsats[instance]:3; p.fix_type = d->have_lock?3:1; // the spec is not very clear, but the time field seems to be @@ -610,7 +610,7 @@ void SITL_State::_update_gps_mtk19(const struct gps_data *d, uint8_t instance) if (p.ground_course < 0.0f) { p.ground_course += 360.0f * 100.0f; } - p.satellites = d->have_lock?_sitl->gps_numsats:3; + p.satellites = d->have_lock?_sitl->gps_numsats[instance]:3; p.fix_type = d->have_lock?3:1; // the spec is not very clear, but the time field seems to be @@ -710,7 +710,7 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance) lat_string, lng_string, d->have_lock?1:0, - d->have_lock?_sitl->gps_numsats:3, + d->have_lock?_sitl->gps_numsats[instance]:3, 2.0, d->altitude); float speed_knots = norm(d->speedN, d->speedE) * M_PER_SEC_TO_KNOTS; @@ -840,7 +840,7 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d, uint8_t instance) pos.height = d->altitude; pos.h_accuracy = 5e3; pos.v_accuracy = 10e3; - pos.n_sats = _sitl->gps_numsats; + pos.n_sats = _sitl->gps_numsats[instance]; // Send single point position solution pos.flags = 0; @@ -855,7 +855,7 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d, uint8_t instance) velned.d = 1e3 * d->speedD; velned.h_accuracy = 5e3; velned.v_accuracy = 5e3; - velned.n_sats = _sitl->gps_numsats; + velned.n_sats = _sitl->gps_numsats[instance]; velned.flags = 0; _sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned, instance); @@ -957,7 +957,7 @@ void SITL_State::_update_gps_sbp2(const struct gps_data *d, uint8_t instance) pos.height = d->altitude; pos.h_accuracy = 5e3; pos.v_accuracy = 10e3; - pos.n_sats = _sitl->gps_numsats; + pos.n_sats = _sitl->gps_numsats[instance]; // Send single point position solution pos.flags = 1; @@ -972,7 +972,7 @@ void SITL_State::_update_gps_sbp2(const struct gps_data *d, uint8_t instance) velned.d = 1e3 * d->speedD; velned.h_accuracy = 5e3; velned.v_accuracy = 5e3; - velned.n_sats = _sitl->gps_numsats; + velned.n_sats = _sitl->gps_numsats[instance]; velned.flags = 1; _sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned, instance); @@ -1120,7 +1120,7 @@ void SITL_State::_update_gps_nova(const struct gps_data *d, uint8_t instance) bestpos.lat = d->latitude; bestpos.lng = d->longitude; bestpos.hgt = d->altitude; - bestpos.svsused = _sitl->gps_numsats; + bestpos.svsused = _sitl->gps_numsats[instance]; bestpos.latsdev=0.2; bestpos.lngsdev=0.2; bestpos.hgtsdev=0.2; @@ -1205,40 +1205,33 @@ void SITL_State::_update_gps_file(uint8_t instance) */ void SITL_State::_update_gps(double latitude, double longitude, float altitude, double speedN, double speedE, double speedD, - double yaw, bool have_lock) + double yaw, bool _have_lock) { char c; - // simulate delayed lock times - if (AP_HAL::millis() < _sitl->gps_lock_time*1000UL) { - have_lock = false; + //Capture current position as basestation location for + if (!_gps_has_basestation_position && + _have_lock && + AP_HAL::millis() >= _sitl->gps_lock_time[0]*1000UL) { + _gps_basestation_data.latitude = latitude; + _gps_basestation_data.longitude = longitude; + _gps_basestation_data.altitude = altitude; + _gps_basestation_data.speedN = speedN; + _gps_basestation_data.speedE = speedE; + _gps_basestation_data.speedD = speedD; + _gps_basestation_data.have_lock = _have_lock; + _gps_has_basestation_position = true; } - altitude += _sitl->gps_alt_offset; - - //Capture current position as basestation location for - if (!_gps_has_basestation_position) { - if (have_lock) { - _gps_basestation_data.latitude = latitude; - _gps_basestation_data.longitude = longitude; - _gps_basestation_data.altitude = altitude; - _gps_basestation_data.speedN = speedN; - _gps_basestation_data.speedE = speedE; - _gps_basestation_data.speedD = speedD; - _gps_basestation_data.have_lock = have_lock; - _gps_has_basestation_position = true; - } - } for (uint8_t idx=0; idx<2; idx++) { struct gps_data d; - if (idx == 1 && !_sitl->gps2_enable) { - continue; - } + // simulate delayed lock times + bool have_lock = (_have_lock && !_sitl->gps_disable[idx] && AP_HAL::millis() >= _sitl->gps_lock_time[idx]*1000UL); // run at configured GPS rate (default 5Hz) - if ((AP_HAL::millis() - gps_state[idx].last_update) < (uint32_t)(1000/_sitl->gps_hertz)) { + if ((AP_HAL::millis() - gps_state[idx].last_update) < (uint32_t)(1000/_sitl->gps_hertz[idx])) { continue; } @@ -1254,7 +1247,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, d.yaw = yaw; // add an altitude error controlled by a slow sine wave - d.altitude = altitude + _sitl->gps_noise * sinf(AP_HAL::millis() * 0.0005f); + d.altitude = altitude + _sitl->gps_noise[idx] * sinf(AP_HAL::millis() * 0.0005f) + _sitl->gps_alt_offset[idx]; // Add offet to c.g. velocity to get velocity at antenna d.speedN = speedN; @@ -1262,9 +1255,9 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, d.speedD = speedD; d.have_lock = have_lock; - if (_sitl->gps_drift_alt > 0) { + if (_sitl->gps_drift_alt[idx] > 0) { // slow altitude drift - d.altitude += _sitl->gps_drift_alt*sinf(AP_HAL::millis()*0.001f*0.02f); + d.altitude += _sitl->gps_drift_alt[idx]*sinf(AP_HAL::millis()*0.001f*0.02f); } // correct the latitude, longitude, hiehgt and NED velocity for the offset between @@ -1309,9 +1302,9 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, d = _gps_data[idx][next_index]; - if (_sitl->gps_delay != delay) { + if (_sitl->gps_delay[idx] != delay) { // cope with updates to the delay control - delay = _sitl->gps_delay; + delay = _sitl->gps_delay[idx]; for (uint8_t i=0; i