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

614 lines
18 KiB
C++
Raw Blame History

/*
SITL handling
This simulates a GPS on a serial port
Andrew Tridgell November 2011
*/
#include "SIM_GPS.h"
#if HAL_SIM_GPS_ENABLED
#include <time.h>
#include <sys/time.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_HAL/AP_HAL.h>
#include <SITL/SITL.h>
#include <AP_InternalError/AP_InternalError.h>
#include "SIM_GPS_FILE.h"
#include "SIM_GPS_Trimble.h"
#include "SIM_GPS_MSP.h"
#include "SIM_GPS_NMEA.h"
#include "SIM_GPS_NOVA.h"
#include "SIM_GPS_SBP2.h"
#include "SIM_GPS_SBP.h"
#include "SIM_GPS_UBLOX.h"
#include "SIM_GPS_SBF.h"
#include <GCS_MAVLink/GCS.h>
namespace SITL {
// user settable parameters for GNSS sensors
const AP_Param::GroupInfo SIM::GPSParms::var_info[] = {
// @Param: ENABLE
// @DisplayName: GPS enable
// @Description: Enable simulated GPS
// @Values: 0:Disable, 1:Enable
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, GPSParms, enabled, 0, AP_PARAM_FLAG_ENABLE),
// @Param: LAG_MS
// @DisplayName: GPS Lag
// @Description: GPS lag
// @Units: ms
// @User: Advanced
AP_GROUPINFO("LAG_MS", 2, GPSParms, delay_ms, 100),
// @Param: TYPE
// @DisplayName: GPS type
// @Description: Sets the type of simulation used for GPS
// @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP2, 11:Trimble, 19:MSP
// @User: Advanced
AP_GROUPINFO("TYPE", 3, GPSParms, type, GPS::Type::UBLOX),
// @Param: BYTELOS
// @DisplayName: GPS Byteloss
// @Description: Percent of bytes lost from GPS
// @Units: %
// @User: Advanced
AP_GROUPINFO("BYTELOS", 4, GPSParms, byteloss, 0),
// @Param: NUMSATS
// @DisplayName: GPS Num Satellites
// @Description: Number of satellites GPS has in view
AP_GROUPINFO("NUMSATS", 5, GPSParms, numsats, 10),
// @Param: GLTCH
// @DisplayName: GPS Glitch
// @Description: Glitch offsets of simulated GPS sensor
// @Vector3Parameter: 1
// @User: Advanced
AP_GROUPINFO("GLTCH", 6, GPSParms, glitch, 0),
// @Param: HZ
// @DisplayName: GPS Hz
// @Description: GPS Update rate
// @Units: Hz
AP_GROUPINFO("HZ", 7, GPSParms, hertz, 5),
// @Param: DRFTALT
// @DisplayName: GPS Altitude Drift
// @Description: GPS altitude drift error
// @Units: m
// @User: Advanced
AP_GROUPINFO("DRFTALT", 8, GPSParms, drift_alt, 0),
// @Param: POS
// @DisplayName: GPS Position
// @Description: GPS antenna phase center position relative to the body frame origin
// @Units: m
// @Vector3Parameter: 1
AP_GROUPINFO("POS", 9, GPSParms, pos_offset, 0),
// @Param: NOISE
// @DisplayName: GPS Noise
// @Description: Amplitude of the GPS altitude error
// @Units: m
// @User: Advanced
AP_GROUPINFO("NOISE", 10, GPSParms, noise, 0),
// @Param: LCKTIME
// @DisplayName: GPS Lock Time
// @Description: Delay in seconds before GPS acquires lock
// @Units: s
// @User: Advanced
AP_GROUPINFO("LCKTIME", 11, GPSParms, lock_time, 0),
// @Param: ALT_OFS
// @DisplayName: GPS Altitude Offset
// @Description: GPS Altitude Error
// @Units: m
AP_GROUPINFO("ALT_OFS", 12, GPSParms, alt_offset, 0),
// @Param: HDG
// @DisplayName: GPS Heading
// @Description: Enable GPS output of NMEA heading HDT sentence or UBLOX_RELPOSNED
// @Values: 0:Disabled, 1:Emit HDT, 2:Emit THS, 3:KSXT, 4:Be Moving Baseline Base
// @User: Advanced
AP_GROUPINFO("HDG", 13, GPSParms, hdg_enabled, SIM::GPS_HEADING_NONE),
// @Param: ACC
// @DisplayName: GPS Accuracy
// @Description: GPS Accuracy
// @User: Advanced
AP_GROUPINFO("ACC", 14, GPSParms, accuracy, 0.3),
// @Param: VERR
// @DisplayName: GPS Velocity Error
// @Description: GPS Velocity Error Offsets in NED
// @Vector3Parameter: 1
// @User: Advanced
AP_GROUPINFO("VERR", 15, GPSParms, vel_err, 0),
// @Param: JAM
// @DisplayName: GPS jamming enable
// @Description: Enable simulated GPS jamming
// @User: Advanced
// @Values: 0:Disabled, 1:Enabled
AP_GROUPINFO("JAM", 16, GPSParms, jam, 0),
AP_GROUPEND
};
}
// the number of GPS leap seconds - copied from AP_GPS.h
#define GPS_LEAPSECONDS_MILLIS 18000ULL
extern const AP_HAL::HAL& hal;
using namespace SITL;
// ensure the backend we have allocated matches the one that's configured:
GPS_Backend::GPS_Backend(GPS &_front, uint8_t _instance) :
instance{_instance},
front{_front}
{
_sitl = AP::sitl();
#if HAL_SIM_GPS_ENABLED && AP_SIM_MAX_GPS_SENSORS > 0
// default the first backend to enabled:
if (_instance == 0 && !_sitl->gps[0].enabled.configured()) {
_sitl->gps[0].enabled.set(1);
}
#endif
}
ssize_t GPS_Backend::write_to_autopilot(const char *p, size_t size) const
{
return front.write_to_autopilot(p, size);
}
ssize_t GPS_Backend::read_from_autopilot(char *buffer, size_t size) const
{
return front.read_from_autopilot(buffer, size);
}
GPS::GPS(uint8_t _instance) :
SerialDevice(8192, 2048),
instance{_instance}
{
}
uint32_t GPS::device_baud() const
{
if (backend == nullptr) {
return 0;
}
return backend->device_baud();
}
/*
write some bytes from the simulated GPS
*/
ssize_t GPS::write_to_autopilot(const char *p, size_t size) const
{
// the second GPS instance fails in a different way to the first;
// the first will start sending back 3 satellites, the second just
// stops responding when disabled. This is not necessarily a good
// thing.
if (instance == 1 && !_sitl->gps[instance].enabled) {
return -1;
}
const float byteloss = _sitl->gps[instance].byteloss;
// shortcut if we're not doing byteloss:
if (!is_positive(byteloss)) {
return SerialDevice::write_to_autopilot(p, size);
}
size_t ret = 0;
while (size--) {
float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
if (r < byteloss) {
// lose the byte
p++;
continue;
}
const ssize_t pret = SerialDevice::write_to_autopilot(p, 1);
if (pret == 0) {
// no space?
return ret;
}
if (pret != 1) {
// error has occured?
return pret;
}
ret++;
p++;
}
return ret;
}
/*
get timeval using simulation time
*/
void GPS_Backend::simulation_timeval(struct timeval *tv)
{
uint64_t now = AP_HAL::micros64();
static uint64_t first_usec;
static struct timeval first_tv;
if (first_usec == 0) {
first_usec = now;
first_tv.tv_sec = AP::sitl()->start_time_UTC;
}
*tv = first_tv;
tv->tv_sec += now / 1000000ULL;
uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
tv->tv_sec += new_usec / 1000000ULL;
tv->tv_usec = new_usec % 1000000ULL;
}
/*
simple simulation of jamming
*/
void GPS::simulate_jamming(struct GPS_Data &d)
{
auto &jam = jamming[instance];
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - jam.last_jam_ms > 1000) {
jam.jam_start_ms = now_ms;
jam.latitude = d.latitude;
jam.longitude = d.longitude;
}
jam.last_jam_ms = now_ms;
// how often each of the key state variables change during jamming
const float vz_change_hz = 0.5;
const float vel_change_hz = 0.8;
const float pos_change_hz = 1.1;
const float sats_change_hz = 3;
const float acc_change_hz = 3;
if (now_ms - jam.jam_start_ms < unsigned(1000U+(get_random16()%5000))) {
// total loss of signal for a period at the start is common
d.num_sats = 0;
d.have_lock = false;
} else {
if ((now_ms - jam.last_sats_change_ms)*0.001 > 2*fabsf(rand_float())/sats_change_hz) {
jam.last_sats_change_ms = now_ms;
d.num_sats = 2 + (get_random16() % 15);
if (d.num_sats >= 4) {
if (get_random16() % 2 == 0) {
d.have_lock = false;
} else {
d.have_lock = true;
}
} else {
d.have_lock = false;
}
}
if ((now_ms - jam.last_vz_change_ms)*0.001 > 2*fabsf(rand_float())/vz_change_hz) {
jam.last_vz_change_ms = now_ms;
d.speedD = rand_float() * 400;
}
if ((now_ms - jam.last_vel_change_ms)*0.001 > 2*fabsf(rand_float())/vel_change_hz) {
jam.last_vel_change_ms = now_ms;
d.speedN = rand_float() * 400;
d.speedE = rand_float() * 400;
}
if ((now_ms - jam.last_pos_change_ms)*0.001 > 2*fabsf(rand_float())/pos_change_hz) {
jam.last_pos_change_ms = now_ms;
jam.latitude += rand_float()*200 * LATLON_TO_M_INV * 1e-7;
jam.longitude += rand_float()*200 * LATLON_TO_M_INV * 1e-7;
}
if ((now_ms - jam.last_acc_change_ms)*0.001 > 2*fabsf(rand_float())/acc_change_hz) {
jam.last_acc_change_ms = now_ms;
d.vertical_acc = fabsf(rand_float())*300;
d.horizontal_acc = fabsf(rand_float())*300;
d.speed_acc = fabsf(rand_float())*50;
}
}
d.latitude = constrain_float(jam.latitude, -90, 90);
d.longitude = constrain_float(jam.longitude, -180, 180);
}
/*
return GPS time of week
*/
GPS_Backend::GPS_TOW GPS_Backend::gps_time()
{
GPS_TOW gps_tow;
struct timeval tv;
simulation_timeval(&tv);
const uint32_t epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - (GPS_LEAPSECONDS_MILLIS / 1000ULL);
uint32_t epoch_seconds = tv.tv_sec - epoch;
gps_tow.week = epoch_seconds / AP_SEC_PER_WEEK;
uint32_t t_ms = tv.tv_usec / 1000;
// round time to nearest 200ms
gps_tow.ms = (epoch_seconds % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC + ((t_ms/200) * 200);
return gps_tow;
}
void GPS::check_backend_allocation()
{
const Type configured_type = Type(_sitl->gps[instance].type.get());
if (allocated_type == configured_type) {
return;
}
// mismatch; delete any already-allocated backend:
if (backend != nullptr) {
delete backend;
backend = nullptr;
}
// attempt to allocate backend
switch (configured_type) {
case Type::NONE:
// no GPS attached
break;
#if AP_SIM_GPS_UBLOX_ENABLED
case Type::UBLOX:
backend = NEW_NOTHROW GPS_UBlox(*this, instance);
break;
#endif
#if AP_SIM_GPS_NMEA_ENABLED
case Type::NMEA:
backend = NEW_NOTHROW GPS_NMEA(*this, instance);
break;
#endif
#if AP_SIM_GPS_SBP_ENABLED
case Type::SBP:
backend = NEW_NOTHROW GPS_SBP(*this, instance);
break;
#endif
#if AP_SIM_GPS_SBP2_ENABLED
case Type::SBP2:
backend = NEW_NOTHROW GPS_SBP2(*this, instance);
break;
#endif
#if AP_SIM_GPS_NOVA_ENABLED
case Type::NOVA:
backend = NEW_NOTHROW GPS_NOVA(*this, instance);
break;
#endif
#if AP_SIM_GPS_MSP_ENABLED
case Type::MSP:
backend = NEW_NOTHROW GPS_MSP(*this, instance);
break;
#endif
#if AP_SIM_GPS_SBF_ENABLED
case Type::SBF:
backend = NEW_NOTHROW GPS_SBF(*this, instance);
break;
#endif
#if AP_SIM_GPS_TRIMBLE_ENABLED
case Type::TRIMBLE:
backend = NEW_NOTHROW GPS_Trimble(*this, instance);
break;
#endif
#if AP_SIM_GPS_FILE_ENABLED
case Type::FILE:
backend = NEW_NOTHROW GPS_FILE(*this, instance);
break;
#endif
};
if (configured_type != Type::NONE && backend == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SIM_GPS: No backend for %u", (unsigned)configured_type);
}
allocated_type = configured_type;
}
/*
possibly send a new GPS packet
*/
void GPS::update()
{
if (!init_sitl_pointer()) {
return;
}
check_backend_allocation();
if (backend == nullptr) {
return;
}
double latitude =_sitl->state.latitude;
double longitude = _sitl->state.longitude;
float altitude = _sitl->state.altitude;
const double speedN = _sitl->state.speedN;
const double speedE = _sitl->state.speedE;
const double speedD = _sitl->state.speedD;
const uint32_t now_ms = AP_HAL::millis();
if (now_ms < 20000) {
// apply the init offsets for the first 20s. This allows for
// having the origin a long way from the takeoff location,
// which makes testing long flights easier
latitude += _sitl->gps_init_lat_ofs;
longitude += _sitl->gps_init_lon_ofs;
altitude += _sitl->gps_init_alt_ofs;
}
//Capture current position as basestation location for
if (!_gps_has_basestation_position &&
now_ms >= _sitl->gps[0].lock_time*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_has_basestation_position = true;
}
const auto &params = _sitl->gps[instance];
struct GPS_Data d {};
// simulate delayed lock times
bool have_lock = (params.enabled && now_ms >= params.lock_time*1000UL);
// Only let physics run and GPS write at configured GPS rate (default 5Hz).
if ((now_ms - last_write_update_ms) < (uint32_t)(1000/params.hertz)) {
// Reading runs every iteration.
// Beware- physics don't update every iteration with this approach.
// Currently, none of the drivers rely on quickly updated physics.
backend->update_read();
return;
}
last_write_update_ms = now_ms;
d.num_sats = params.numsats;
d.latitude = latitude;
d.longitude = longitude;
d.yaw_deg = _sitl->state.yawDeg;
d.roll_deg = _sitl->state.rollDeg;
d.pitch_deg = _sitl->state.pitchDeg;
// add an altitude error controlled by a slow sine wave
d.altitude = altitude + params.noise * sinf(now_ms * 0.0005f) + params.alt_offset;
// Add offset to c.g. velocity to get velocity at antenna and add simulated error
Vector3f velErrorNED = params.vel_err;
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 = params.accuracy;
d.vertical_acc = params.accuracy;
d.speed_acc = params.vel_err.get().xy().length();
if (params.drift_alt > 0) {
// add slow altitude drift controlled by a slow sine wave
d.altitude += params.drift_alt*sinf(now_ms*0.001f*0.02f);
}
// correct the latitude, longitude, height and NED velocity for the offset between
// the vehicle c.g. and GPS antenna
Vector3f posRelOffsetBF = params.pos_offset;
if (!posRelOffsetBF.is_zero()) {
// get a rotation matrix following DCM conventions (body to earth)
Matrix3f rotmat;
_sitl->state.quaternion.rotation_matrix(rotmat);
// rotate the antenna offset into the earth frame
Vector3f posRelOffsetEF = rotmat * posRelOffsetBF;
// Add the offset to the latitude, longitude and height using a spherical earth approximation
double const earth_rad_inv = 1.569612305760477e-7; // use Authalic/Volumetric radius
double lng_scale_factor = earth_rad_inv / cos(radians(d.latitude));
d.latitude += degrees(posRelOffsetEF.x * earth_rad_inv);
d.longitude += degrees(posRelOffsetEF.y * lng_scale_factor);
d.altitude -= posRelOffsetEF.z;
// calculate a velocity offset due to the antenna position offset and body rotation rate
// note: % operator is overloaded for cross product
Vector3f gyro(radians(_sitl->state.rollRate),
radians(_sitl->state.pitchRate),
radians(_sitl->state.yawRate));
Vector3f velRelOffsetBF = gyro % posRelOffsetBF;
// rotate the velocity offset into earth frame and add to the c.g. velocity
Vector3f velRelOffsetEF = rotmat * velRelOffsetBF;
d.speedN += velRelOffsetEF.x;
d.speedE += velRelOffsetEF.y;
d.speedD += velRelOffsetEF.z;
}
// get delayed data
d.timestamp_ms = now_ms;
d = interpolate_data(d, params.delay_ms);
// Applying GPS glitch
// Using first gps glitch
Vector3f glitch_offsets = params.glitch;
d.latitude += glitch_offsets.x;
d.longitude += glitch_offsets.y;
d.altitude += glitch_offsets.z;
if (params.jam == 1) {
simulate_jamming(d);
}
backend->publish(&d);
}
void GPS_Backend::update_read()
{
// swallow any config bytes
char c;
read_from_autopilot(&c, 1);
}
/*
get delayed data by interpolation
*/
GPS_Data GPS::interpolate_data(const GPS_Data &d, uint32_t delay_ms)
{
const uint8_t N = ARRAY_SIZE(_gps_history);
const uint32_t now_ms = d.timestamp_ms;
// add in into history array, shifting old elements
memmove(&_gps_history[1], &_gps_history[0], sizeof(_gps_history[0])*(ARRAY_SIZE(_gps_history)-1));
_gps_history[0] = d;
for (uint8_t i=0; i<N-1; i++) {
uint32_t dt1 = now_ms - _gps_history[i].timestamp_ms;
uint32_t dt2 = now_ms - _gps_history[i+1].timestamp_ms;
if (delay_ms >= dt1 && delay_ms <= dt2) {
// we will interpolate this pair of samples. Start with
// the older sample
const GPS_Data &s1 = _gps_history[i+1];
const GPS_Data &s2 = _gps_history[i];
GPS_Data d2 = s1;
const float p = (dt2 - delay_ms) / MAX(1,float(dt2 - dt1));
d2.latitude += p * (s2.latitude - s1.latitude);
d2.longitude += p * (s2.longitude - s1.longitude);
d2.altitude += p * (s2.altitude - s1.altitude);
d2.speedN += p * (s2.speedN - s1.speedN);
d2.speedE += p * (s2.speedE - s1.speedE);
d2.speedD += p * (s2.speedD - s1.speedD);
d2.yaw_deg += p * wrap_180(s2.yaw_deg - s1.yaw_deg);
return d2;
}
}
// delay is too long, use last sample
return _gps_history[N-1];
}
float GPS_Data::ground_track_rad() const
{
return atan2f(speedE, speedN);
}
float GPS_Data::speed_2d() const
{
const auto velocity = Vector2d{speedN, speedE};
return velocity.length();
}
#endif // HAL_SIM_GPS_ENABLED
Reference in New Issue View Git Blame Copy Permalink