aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: added SIM_GPS_BYTELOSS option

Browse Source 
this allows testing of protocol recovery after losing bytes on the GPS
serial link
This commit is contained in:
Andrew Tridgell 2013-02-16 22:00:16 +11:00
parent 3a582663fb
commit fb4e68f0f0
4 changed files with 53 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

@ -56,6 +56,25 @@ private:
static void _update_barometer(float height);
static void _update_compass(float roll, float pitch, float yaw);
struct gps_data {
double latitude;
double longitude;
float altitude;
double speedN;
double speedE;
bool have_lock;
};
#define MAX_GPS_DELAY 100
static gps_data _gps_data[MAX_GPS_DELAY];
static void _gps_write(uint8_t *p, uint16_t size);
static void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
static void _update_gps_ubx(const struct gps_data *d);
static void _update_gps_mtk(const struct gps_data *d);
static void _update_gps_mtk16(const struct gps_data *d);
static void _update_gps_mtk19(const struct gps_data *d);
static void _update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, bool have_lock);

57
libraries/AP_HAL_AVR_SITL/sitl_gps.cpp
View File

@ -29,19 +29,9 @@
using namespace AVR_SITL;
extern const AP_HAL::HAL& hal;
#define MAX_GPS_DELAY 100
struct gps_data {
double latitude;
double longitude;
float altitude;
double speedN;
double speedE;
bool have_lock;
} gps_data[MAX_GPS_DELAY];
static uint8_t next_gps_index;
static uint8_t gps_delay;
SITL_State::gps_data SITL_State::_gps_data[MAX_GPS_DELAY];
// state of GPS emulation
static struct {
@ -87,11 +77,28 @@ int SITL_State::gps_pipe(void)
return gps_state.client_fd;
}
/*
write some bytes from the simulated GPS
*/
void SITL_State::_gps_write(uint8_t *p, uint16_t size)
{
while (size--) {
if (_sitl->gps_byteloss > 0.0) {
float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
if (r < _sitl->gps_byteloss) {
// lose the byte
p++;
continue;
}
}
write(gps_state.gps_fd, p++, 1);
}
}
/*
send a UBLOX GPS message
*/
static void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
void SITL_State::_gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
{
const uint8_t PREAMBLE1 = 0xb5;
const uint8_t PREAMBLE2 = 0x62;
@ -110,9 +117,9 @@ static void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
for (uint8_t i=0; i<size; i++) {
chk[1] += (chk[0] += buf[i]);
}
write(gps_state.gps_fd, hdr, sizeof(hdr));
write(gps_state.gps_fd, buf, size);
write(gps_state.gps_fd, chk, sizeof(chk));
_gps_write(hdr, sizeof(hdr));
_gps_write(buf, size);
_gps_write(chk, sizeof(chk));
}
/*
@ -136,7 +143,7 @@ static uint32_t millis_time_of_week(void)
/*
send a new set of GPS UBLOX packets
*/
static void _update_gps_ubx(const struct gps_data *d)
void SITL_State::_update_gps_ubx(const struct gps_data *d)
{
#pragma pack(push,1)
struct ubx_nav_posllh {
@ -257,7 +264,7 @@ static void mtk_checksum(const uint8_t *data, uint8_t n, uint8_t *ck_a, uint8_t
/*
send a new GPS MTK packet
*/
static void _update_gps_mtk(const struct gps_data *d)
void SITL_State::_update_gps_mtk(const struct gps_data *d)
{
#pragma pack(push,1)
struct mtk_msg {
@ -307,13 +314,13 @@ static void _update_gps_mtk(const struct gps_data *d)
swap_uint32((uint32_t *)&p.utc_time, 1);
mtk_checksum(&p.msg_class, sizeof(p)-4, &p.ck_a, &p.ck_b);
write(gps_state.gps_fd, &p, sizeof(p));
_gps_write((uint8_t*)&p, sizeof(p));
}
/*
send a new GPS MTK 1.6 packet
*/
static void _update_gps_mtk16(const struct gps_data *d)
void SITL_State::_update_gps_mtk16(const struct gps_data *d)
{
#pragma pack(push,1)
struct mtk_msg {
@ -365,13 +372,13 @@ static void _update_gps_mtk16(const struct gps_data *d)
mtk_checksum(&p.size, sizeof(p)-4, &p.ck_a, &p.ck_b);
write(gps_state.gps_fd, &p, sizeof(p));
_gps_write((uint8_t*)&p, sizeof(p));
}
/*
send a new GPS MTK 1.9 packet
*/
static void _update_gps_mtk19(const struct gps_data *d)
void SITL_State::_update_gps_mtk19(const struct gps_data *d)
{
#pragma pack(push,1)
struct mtk_msg {
@ -423,7 +430,7 @@ static void _update_gps_mtk19(const struct gps_data *d)
mtk_checksum(&p.size, sizeof(p)-4, &p.ck_a, &p.ck_b);
write(gps_state.gps_fd, &p, sizeof(p));
_gps_write((uint8_t*)&p, sizeof(p));
}
/*
@ -455,18 +462,18 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
d.have_lock = have_lock;
// add in some GPS lag
gps_data[next_gps_index++] = d;
_gps_data[next_gps_index++] = d;
if (next_gps_index >= gps_delay) {
next_gps_index = 0;
}
d = gps_data[next_gps_index];
d = _gps_data[next_gps_index];
if (_sitl->gps_delay != gps_delay) {
// cope with updates to the delay control
gps_delay = _sitl->gps_delay;
for (uint8_t i=0; i<gps_delay; i++) {
gps_data[i] = d;
_gps_data[i] = d;
}
}

1
libraries/SITL/SITL.cpp
View File

@ -27,6 +27,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
AP_GROUPINFO("WIND_DIR", 10, SITL, wind_direction, 180),
AP_GROUPINFO("WIND_TURB", 11, SITL, wind_turbulance, 0.2),
AP_GROUPINFO("GPS_TYPE", 12, SITL, gps_type, SITL::GPS_TYPE_UBLOX),
AP_GROUPINFO("GPS_BYTELOSS", 13, SITL, gps_byteloss, 0),
AP_GROUPEND
};

1
libraries/SITL/SITL.h
View File

@ -57,6 +57,7 @@ public:
AP_Int8 gps_disable; // disable simulated GPS
AP_Int8 gps_delay; // delay in samples
AP_Int8 gps_type; // see enum GPSType
AP_Float gps_byteloss;// byte loss as a percent
// wind control
AP_Float wind_speed;