5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-06 07:58:28 -04:00

SITL: make simulated GPS work as a SerialDevice

This commit is contained in:
Peter Barker 2021-10-16 14:10:40 +11:00 committed by Peter Barker
parent 1ab8a3e3aa
commit 6c16da21c5
5 changed files with 283 additions and 208 deletions

View File

@ -6,109 +6,73 @@
Andrew Tridgell November 2011
*/
#include "SIM_GPS.h"
#if HAL_SIM_GPS_ENABLED
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
#include "AP_HAL_SITL.h"
#include "AP_HAL_SITL_Namespace.h"
#include "HAL_SITL_Class.h"
#include <AP_Math/AP_Math.h>
#include <SITL/SITL.h>
#include "Scheduler.h"
#include "UARTDriver.h"
#include <AP_GPS/AP_GPS.h>
#include <AP_GPS/AP_GPS_UBLOX.h>
#include <AP_Common/NMEA.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <time.h>
#include <stdio.h>
#include <sys/time.h>
#include <sys/stat.h>
// simulated CAN GPS devices get fed from our SITL estimates:
#if HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED
#include <sys/types.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/stat.h>
#include <AP_HAL_SITL/AP_HAL_SITL.h>
extern const HAL_SITL& hal_sitl;
#endif
#pragma GCC diagnostic ignored "-Wunused-result"
// the number of GPS leap seconds - copied from AP_GPS.h
#define GPS_LEAPSECONDS_MILLIS 18000ULL
using namespace HALSITL;
extern const AP_HAL::HAL& hal;
// state of GPS emulation
static struct gps_state {
/* pipe emulating UBLOX GPS serial stream */
int gps_fd, client_fd;
uint32_t last_update; // milliseconds
using namespace SITL;
uint8_t next_index;
uint8_t delay;
} gps_state[2];
/*
hook for reading from the GPS pipe
*/
ssize_t SITL_State::gps_read(int fd, void *buf, size_t count)
GPS::GPS(uint8_t _instance) :
SerialDevice(),
instance{_instance}
{
#ifdef FIONREAD
// use FIONREAD to get exact value if possible
int num_ready;
while (ioctl(fd, FIONREAD, &num_ready) == 0 && num_ready > 3000) {
// the pipe is filling up - drain it
uint8_t tmp[128];
if (read(fd, tmp, sizeof(tmp)) != sizeof(tmp)) {
break;
}
#if HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED
const uint8_t num_gps = 2;
// pipe number is SITL instance number (e.g. -I argument to
// sim_vehicle.py) times the max number of GPSs + the gps instance
// number:
const uint8_t num = num_gps * hal_sitl.get_instance() + instance;
if (asprintf(&_gps_fifo, "/tmp/gps_fifo%u", (unsigned)num) == -1) { // FIXME - needs to work with simulated periph-gps
AP_BoardConfig::allocation_error("gps_fifo filepath");
}
if (mkfifo(_gps_fifo, 0666) < 0) {
printf("MKFIFO failed with %m\n");
}
#endif
return read(fd, buf, count);
}
/*
setup GPS input pipe
*/
int SITL_State::gps_pipe(uint8_t idx)
{
int fd[2];
if (_gps_fifo[idx] == nullptr) {
UNUSED_RESULT(asprintf(&_gps_fifo[idx], "/tmp/gps_fifo%d", (int)(ARRAY_SIZE(_gps_fifo)*_instance + idx)));
}
if (gps_state[idx].client_fd != 0) {
return gps_state[idx].client_fd;
}
pipe(fd);
if (mkfifo(_gps_fifo[idx], 0666) < 0) {
printf("MKFIFO failed with %s\n", strerror(errno));
}
gps_state[idx].gps_fd = fd[1];
gps_state[idx].client_fd = fd[0];
gps_state[idx].last_update = AP_HAL::millis();
fcntl(fd[0], F_SETFD, FD_CLOEXEC);
fcntl(fd[1], F_SETFD, FD_CLOEXEC);
HALSITL::UARTDriver::_set_nonblocking(gps_state[idx].gps_fd);
HALSITL::UARTDriver::_set_nonblocking(fd[0]);
return gps_state[idx].client_fd;
}
/*
write some bytes from the simulated GPS
*/
void SITL_State::_gps_write(const uint8_t *p, uint16_t size, uint8_t instance)
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_disable[instance]) {
return;
}
if (_gps_fifo[instance] == nullptr) {
printf("GPS FIFO path not set\n");
return;
return -1;
}
#if HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED
// also write to external fifo
int fd = open(_gps_fifo[instance], O_WRONLY | O_NONBLOCK);
int fd = open(_gps_fifo, O_WRONLY | O_NONBLOCK);
if (fd >= 0) {
write(fd, p, size);
UNUSED_RESULT(write(fd, p, size));
close(fd);
}
#endif
size_t ret = 0;
while (size--) {
if (_sitl->gps_byteloss[instance] > 0.0f) {
float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
@ -118,11 +82,20 @@ void SITL_State::_gps_write(const uint8_t *p, uint16_t size, uint8_t instance)
continue;
}
}
if (gps_state[instance].gps_fd != 0) {
write(gps_state[instance].gps_fd, p, 1);
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;
}
/*
@ -147,7 +120,7 @@ static void simulation_timeval(struct timeval *tv)
/*
send a UBLOX GPS message
*/
void SITL_State::_gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size, uint8_t instance)
void GPS::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
{
const uint8_t PREAMBLE1 = 0xb5;
const uint8_t PREAMBLE2 = 0x62;
@ -166,9 +139,9 @@ void SITL_State::_gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size, uint8
for (uint16_t i=0; i<size; i++) {
chk[1] += (chk[0] += buf[i]);
}
_gps_write(hdr, sizeof(hdr), instance);
_gps_write(buf, size, instance);
_gps_write(chk, sizeof(chk), instance);
write_to_autopilot((char*)hdr, sizeof(hdr));
write_to_autopilot((char*)buf, size);
write_to_autopilot((char*)chk, sizeof(chk));
}
/*
@ -189,7 +162,7 @@ static void gps_time(uint16_t *time_week, uint32_t *time_week_ms)
/*
send a new set of GPS UBLOX packets
*/
void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
void GPS::update_ubx(const struct gps_data *d)
{
struct PACKED ubx_nav_posllh {
uint32_t time; // GPS msToW
@ -431,7 +404,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
radians(_sitl->state.pitchRate),
radians(_sitl->state.yawRate));
rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw));
const float lag = (1.0/_sitl->gps_hertz[instance]) * gps_state[instance].delay;
const float lag = (1.0/_sitl->gps_hertz[instance]) * delay;
rot.rotate(gyro * (-lag));
rel_antenna_pos = rot * rel_antenna_pos;
relposned.version = 1;
@ -444,14 +417,14 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid;
}
_gps_send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos), instance);
_gps_send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status), instance);
_gps_send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned), instance);
_gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol), instance);
_gps_send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop), instance);
_gps_send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt), instance);
send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status));
send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol));
send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop));
send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt));
if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) {
_gps_send_ubx(MSG_RELPOSNED, (uint8_t*)&relposned, sizeof(relposned), instance);
send_ubx(MSG_RELPOSNED, (uint8_t*)&relposned, sizeof(relposned));
}
if (time_week_ms > _next_nav_sv_info_time) {
@ -470,7 +443,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
svinfo.sv[i].azim = i;
// not bothering to fill in prRes
}
_gps_send_ubx(MSG_SVINFO, (uint8_t*)&svinfo, sizeof(svinfo), instance);
send_ubx(MSG_SVINFO, (uint8_t*)&svinfo, sizeof(svinfo));
_next_nav_sv_info_time = time_week_ms + 10000; // 10 second delay
}
}
@ -478,7 +451,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
/*
formatted print of NMEA message, with checksum appended
*/
void SITL_State::_gps_nmea_printf(uint8_t instance, const char *fmt, ...)
void GPS::nmea_printf(const char *fmt, ...)
{
va_list ap;
@ -486,7 +459,7 @@ void SITL_State::_gps_nmea_printf(uint8_t instance, const char *fmt, ...)
char *s = nmea_vaprintf(fmt, ap);
va_end(ap);
if (s != nullptr) {
_gps_write((const uint8_t*)s, strlen(s), instance);
write_to_autopilot((const char*)s, strlen(s));
free(s);
}
}
@ -495,7 +468,7 @@ void SITL_State::_gps_nmea_printf(uint8_t instance, const char *fmt, ...)
/*
send a new GPS NMEA packet
*/
void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
void GPS::update_nmea(const struct gps_data *d)
{
struct timeval tv;
struct tm *tm;
@ -528,7 +501,7 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
(deg - int(deg))*60,
d->longitude<0?'W':'E');
_gps_nmea_printf(instance, "$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,",
nmea_printf("$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,",
tstring,
lat_string,
lng_string,
@ -543,15 +516,15 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
heading += 360.0f;
}
//$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24
_gps_nmea_printf(instance, "$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A",
//$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24
nmea_printf("$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A",
tstring,
heading,
heading,
speed_knots,
speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6);
_gps_nmea_printf(instance, "$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
nmea_printf("$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
tstring,
d->have_lock?'A':'V',
lat_string,
@ -561,26 +534,26 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
dstring);
if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) {
_gps_nmea_printf(instance, "$GPHDT,%.2f,T", d->yaw);
nmea_printf("$GPHDT,%.2f,T", d->yaw);
}
else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_THS) {
_gps_nmea_printf(instance, "$GPTHS,%.2f,%c,T", d->yaw, d->have_lock ? 'A' : 'V');
nmea_printf("$GPTHS,%.2f,%c,T", d->yaw, d->have_lock ? 'A' : 'V');
}
}
void SITL_State::_sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload, uint8_t instance)
void GPS::sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload)
{
if (len != 0 && payload == 0) {
return; //SBP_NULL_ERROR;
}
uint8_t preamble = 0x55;
_gps_write(&preamble, 1, instance);
_gps_write((uint8_t*)&msg_type, 2, instance);
_gps_write((uint8_t*)&sender_id, 2, instance);
_gps_write(&len, 1, instance);
write_to_autopilot((char*)&preamble, 1);
write_to_autopilot((char*)&msg_type, 2);
write_to_autopilot((char*)&sender_id, 2);
write_to_autopilot((char*)&len, 1);
if (len > 0) {
_gps_write((uint8_t*)payload, len, instance);
write_to_autopilot((char*)payload, len);
}
uint16_t crc;
@ -588,10 +561,10 @@ void SITL_State::_sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_
crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc);
crc = crc16_ccitt(&(len), 1, crc);
crc = crc16_ccitt(payload, len, crc);
_gps_write((uint8_t*)&crc, 2, instance);
write_to_autopilot((char*)&crc, 2);
}
void SITL_State::_update_gps_sbp(const struct gps_data *d, uint8_t instance)
void GPS::update_sbp(const struct gps_data *d)
{
struct sbp_heartbeat_t {
bool sys_error : 1;
@ -654,7 +627,7 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d, uint8_t instance)
t.tow = time_week_ms;
t.ns = 0;
t.flags = 0;
_sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t, instance);
sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t);
if (!d->have_lock) {
return;
@ -670,10 +643,10 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d, uint8_t instance)
// Send single point position solution
pos.flags = 0;
_sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos, instance);
sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
// Send "pseudo-absolute" RTK position solution
pos.flags = 1;
_sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos, instance);
sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
velned.tow = time_week_ms;
velned.n = 1e3 * d->speedN;
@ -683,7 +656,7 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d, uint8_t instance)
velned.v_accuracy = 5e3;
velned.n_sats = _sitl->gps_numsats[instance];
velned.flags = 0;
_sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned, instance);
sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
static uint32_t do_every_count = 0;
if (do_every_count % 5 == 0) {
@ -695,19 +668,19 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d, uint8_t instance)
dops.hdop = 100;
dops.vdop = 1;
dops.flags = 1;
_sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops),
(uint8_t*)&dops, instance);
sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops),
(uint8_t*)&dops);
hb.protocol_major = 0; //Sends protocol version 0
_sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb),
(uint8_t*)&hb, instance);
sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb),
(uint8_t*)&hb);
}
do_every_count++;
}
void SITL_State::_update_gps_sbp2(const struct gps_data *d, uint8_t instance)
void GPS::update_sbp2(const struct gps_data *d)
{
struct sbp_heartbeat_t {
bool sys_error : 1;
@ -771,7 +744,7 @@ void SITL_State::_update_gps_sbp2(const struct gps_data *d, uint8_t instance)
t.tow = time_week_ms;
t.ns = 0;
t.flags = 1;
_sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t, instance);
sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t);
if (!d->have_lock) {
return;
@ -787,10 +760,10 @@ void SITL_State::_update_gps_sbp2(const struct gps_data *d, uint8_t instance)
// Send single point position solution
pos.flags = 1;
_sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos, instance);
sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
// Send "pseudo-absolute" RTK position solution
pos.flags = 4;
_sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos, instance);
sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
velned.tow = time_week_ms;
velned.n = 1e3 * d->speedN;
@ -800,7 +773,7 @@ void SITL_State::_update_gps_sbp2(const struct gps_data *d, uint8_t instance)
velned.v_accuracy = 5e3;
velned.n_sats = _sitl->gps_numsats[instance];
velned.flags = 1;
_sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned, instance);
sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
static uint32_t do_every_count = 0;
if (do_every_count % 5 == 0) {
@ -812,17 +785,17 @@ void SITL_State::_update_gps_sbp2(const struct gps_data *d, uint8_t instance)
dops.hdop = 100;
dops.vdop = 1;
dops.flags = 1;
_sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops),
(uint8_t*)&dops, instance);
sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops),
(uint8_t*)&dops);
hb.protocol_major = 2; //Sends protocol version 2.0
_sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb),
(uint8_t*)&hb, instance);
sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb),
(uint8_t*)&hb);
}
do_every_count++;
}
void SITL_State::_update_gps_nova(const struct gps_data *d, uint8_t instance)
void GPS::update_nova(const struct gps_data *d)
{
static struct PACKED nova_header
{
@ -924,10 +897,10 @@ void SITL_State::_update_gps_nova(const struct gps_data *d, uint8_t instance)
header.sequence += 1;
psrdop.hdop = 1.20;
psrdop.htdop = 1.20;
_nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&psrdop, sizeof(psrdop), instance);
psrdop.htdop = 1.20;
nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&psrdop, sizeof(psrdop));
header.messageid = 99;
header.messagelength = sizeof(bestvel);
header.sequence += 1;
@ -935,10 +908,10 @@ void SITL_State::_update_gps_nova(const struct gps_data *d, uint8_t instance)
bestvel.horspd = norm(d->speedN, d->speedE);
bestvel.trkgnd = ToDeg(atan2f(d->speedE, d->speedN));
bestvel.vertspd = -d->speedD;
_nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestvel, sizeof(bestvel), instance);
nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestvel, sizeof(bestvel));
header.messageid = 42;
header.messagelength = sizeof(bestpos);
header.sequence += 1;
@ -952,23 +925,23 @@ void SITL_State::_update_gps_nova(const struct gps_data *d, uint8_t instance)
bestpos.hgtsdev=0.2;
bestpos.solstat=0;
bestpos.postype=32;
_nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestpos, sizeof(bestpos), instance);
nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestpos, sizeof(bestpos));
}
void SITL_State::_nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen, uint8_t instance)
void GPS::nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen)
{
_gps_write(header, headerlength, instance);
_gps_write(payload, payloadlen, instance);
write_to_autopilot((char*)header, headerlength);
write_to_autopilot((char*)payload, payloadlen);
uint32_t crc = CalculateBlockCRC32(headerlength, header, (uint32_t)0);
crc = CalculateBlockCRC32(payloadlen, payload, crc);
_gps_write((uint8_t*)&crc, 4, instance);
write_to_autopilot((char*)&crc, 4);
}
#define CRC32_POLYNOMIAL 0xEDB88320L
uint32_t SITL_State::CRC32Value(uint32_t icrc)
uint32_t GPS::CRC32Value(uint32_t icrc)
{
int i;
uint32_t crc = icrc;
@ -982,7 +955,7 @@ uint32_t SITL_State::CRC32Value(uint32_t icrc)
return crc;
}
uint32_t SITL_State::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc)
uint32_t GPS::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc)
{
while ( length-- != 0 )
{
@ -994,7 +967,7 @@ uint32_t SITL_State::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint3
/*
temporary method to use file as GPS data
*/
void SITL_State::_update_gps_file(uint8_t instance)
void GPS::update_file()
{
static int fd = -1;
static int fd2 = -1;
@ -1018,7 +991,7 @@ void SITL_State::_update_gps_file(uint8_t instance)
ssize_t ret = ::read(temp_fd, buf, sizeof(buf));
if (ret > 0) {
::printf("wrote gps %u bytes\n", (unsigned)ret);
_gps_write((const uint8_t *)buf, ret, instance);
write_to_autopilot((const char *)buf, ret);
}
if (ret == 0) {
::printf("gps rewind\n");
@ -1029,11 +1002,19 @@ void SITL_State::_update_gps_file(uint8_t instance)
/*
possibly send a new GPS packet
*/
void SITL_State::_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, double speedD,
double yaw, bool _have_lock)
void GPS::update()
{
char c;
if (!init_sitl_pointer()) {
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 double yaw = _sitl->state.yawDeg;
if (AP_HAL::millis() < 20000) {
// apply the init offsets for the first 20s. This allows for
@ -1046,7 +1027,6 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
//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;
@ -1054,28 +1034,26 @@ void SITL_State::_update_gps(double latitude, double longitude, float 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;
}
const uint8_t idx = instance; // alias to avoid code churn
for (uint8_t idx=0; idx<2; idx++) {
struct gps_data d;
// simulate delayed lock times
bool have_lock = (_have_lock && !_sitl->gps_disable[idx] && AP_HAL::millis() >= _sitl->gps_lock_time[idx]*1000UL);
bool 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[idx])) {
continue;
if ((AP_HAL::millis() - last_update) < (uint32_t)(1000/_sitl->gps_hertz[idx])) {
return;
}
// swallow any config bytes
if (gps_state[idx].gps_fd != 0) {
read(gps_state[idx].gps_fd, &c, 1);
}
char c;
read_from_autopilot(&c, 1);
gps_state[idx].last_update = AP_HAL::millis();
last_update = AP_HAL::millis();
d.latitude = latitude;
d.longitude = longitude;
@ -1129,27 +1107,21 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
}
// add in some GPS lag
uint8_t &next_index = gps_state[idx].next_index;
uint8_t &delay = gps_state[idx].delay;
_gps_data[idx][next_index++] = d;
_gps_data[next_index++] = d;
if (next_index >= delay+1) {
next_index = 0;
}
d = _gps_data[idx][next_index];
d = _gps_data[next_index];
if (_sitl->gps_delay[idx] != delay) {
// cope with updates to the delay control
delay = _sitl->gps_delay[idx];
for (uint8_t i=0; i<delay; i++) {
_gps_data[idx][i] = d;
_gps_data[i] = d;
}
}
if (gps_state[idx].gps_fd == 0) {
continue;
}
// Applying GPS glitch
// Using first gps glitch
Vector3f glitch_offsets = _sitl->gps_glitch[idx];
@ -1157,42 +1129,37 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
d.longitude += glitch_offsets.y;
d.altitude += glitch_offsets.z;
if (gps_state[idx].gps_fd != 0) {
_update_gps_instance((SITL::SIM::GPSType)_sitl->gps_type[idx].get(), &d, idx);
}
}
}
void SITL_State::_update_gps_instance(SITL::SIM::GPSType gps_type, const struct gps_data *data, uint8_t instance) {
switch (gps_type) {
case SITL::SIM::GPS_TYPE_NONE:
// do GPS-type-dependent updates:
switch ((Type)_sitl->gps_type[instance].get()) {
case Type::NONE:
// no GPS attached
break;
case SITL::SIM::GPS_TYPE_UBLOX:
_update_gps_ubx(data, instance);
case Type::UBLOX:
update_ubx(&d);
break;
case SITL::SIM::GPS_TYPE_NMEA:
_update_gps_nmea(data, instance);
case Type::NMEA:
update_nmea(&d);
break;
case SITL::SIM::GPS_TYPE_SBP:
_update_gps_sbp(data, instance);
case Type::SBP:
update_sbp(&d);
break;
case SITL::SIM::GPS_TYPE_SBP2:
_update_gps_sbp2(data, instance);
case Type::SBP2:
update_sbp2(&d);
break;
case SITL::SIM::GPS_TYPE_NOVA:
_update_gps_nova(data, instance);
case Type::NOVA:
update_nova(&d);
break;
case SITL::SIM::GPS_TYPE_FILE:
_update_gps_file(instance);
case Type::FILE:
update_file();
break;
}
}
#endif
#endif // HAL_SIM_GPS_ENABLED

117
libraries/SITL/SIM_GPS.h Normal file
View File

@ -0,0 +1,117 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
simulate GPS sensors
Usage example:
param set SERIAL5_PROTOCOL 5
sim_vehicle.py -D --console --map -A --uartB=sim:gps:2
*/
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#ifndef HAL_SIM_GPS_ENABLED
#define HAL_SIM_GPS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH))
#endif
#if HAL_SIM_GPS_ENABLED
#ifndef HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED
#define HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif
#include "SIM_SerialDevice.h"
namespace SITL {
class GPS : public SerialDevice {
public:
enum Type {
NONE = 0,
UBLOX = 1,
NMEA = 5,
SBP = 6,
FILE = 7,
NOVA = 8,
SBP2 = 9,
};
GPS(uint8_t _instance);
// update state
void update();
ssize_t write_to_autopilot(const char *p, size_t size) const override;
private:
uint8_t instance;
int ext_fifo_fd;
uint32_t last_update; // milliseconds
// for delay simulation:
uint8_t next_index;
uint8_t delay;
struct gps_data {
double latitude;
double longitude;
float altitude;
double speedN;
double speedE;
double speedD;
double yaw;
bool have_lock;
};
#define MAX_GPS_DELAY 100
gps_data _gps_data[MAX_GPS_DELAY];
#if HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED
// this will be allocated if needed:
char *_gps_fifo;
#endif
bool _gps_has_basestation_position;
gps_data _gps_basestation_data;
void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
void update_ubx(const struct gps_data *d);
uint8_t nmea_checksum(const char *s);
void nmea_printf(const char *fmt, ...);
void update_nmea(const struct gps_data *d);
void sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
void update_sbp(const struct gps_data *d);
void update_sbp2(const struct gps_data *d);
void update_file();
void update_nova(const struct gps_data *d);
void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen);
uint32_t CRC32Value(uint32_t icrc);
uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
};
}
#endif // HAL_SIM_GPS_ENABLED

View File

@ -34,7 +34,7 @@ public:
int write_fd() const { return read_fd_their_end; }
ssize_t read_from_autopilot(char *buffer, size_t size) const;
ssize_t write_to_autopilot(const char *buffer, size_t size) const;
virtual ssize_t write_to_autopilot(const char *buffer, size_t size) const;
protected:

View File

@ -319,7 +319,7 @@ const AP_Param::GroupInfo SIM::BaroParm::var_info[] = {
const AP_Param::GroupInfo SIM::var_gps[] = {
AP_GROUPINFO("GPS_DISABLE", 1, SIM, gps_disable[0], 0),
AP_GROUPINFO("GPS_DELAY", 2, SIM, gps_delay[0], 1),
AP_GROUPINFO("GPS_TYPE", 3, SIM, gps_type[0], SIM::GPS_TYPE_UBLOX),
AP_GROUPINFO("GPS_TYPE", 3, SIM, gps_type[0], GPS::Type::UBLOX),
AP_GROUPINFO("GPS_BYTELOSS", 4, SIM, gps_byteloss[0], 0),
AP_GROUPINFO("GPS_NUMSATS", 5, SIM, gps_numsats[0], 10),
AP_GROUPINFO("GPS_GLITCH", 6, SIM, gps_glitch[0], 0),
@ -335,7 +335,7 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
AP_GROUPINFO("GPS2_DISABLE", 30, SIM, gps_disable[1], 1),
AP_GROUPINFO("GPS2_DELAY", 31, SIM, gps_delay[1], 1),
AP_GROUPINFO("GPS2_TYPE", 32, SIM, gps_type[1], SIM::GPS_TYPE_UBLOX),
AP_GROUPINFO("GPS2_TYPE", 32, SIM, gps_type[1], GPS::Type::UBLOX),
AP_GROUPINFO("GPS2_BYTELOS", 33, SIM, gps_byteloss[1], 0),
AP_GROUPINFO("GPS2_NUMSATS", 34, SIM, gps_numsats[1], 10),
AP_GROUPINFO("GPS2_GLTCH", 35, SIM, gps_glitch[1], 0),

View File

@ -24,6 +24,7 @@
#include "SIM_FETtecOneWireESC.h"
#include "SIM_IntelligentEnergy24.h"
#include "SIM_Ship.h"
#include "SIM_GPS.h"
#include <AP_RangeFinder/AP_RangeFinder.h>
namespace SITL {
@ -129,16 +130,6 @@ public:
SITL_RCFail_Throttle950 = 2,
};
enum GPSType {
GPS_TYPE_NONE = 0,
GPS_TYPE_UBLOX = 1,
GPS_TYPE_NMEA = 5,
GPS_TYPE_SBP = 6,
GPS_TYPE_FILE = 7,
GPS_TYPE_NOVA = 8,
GPS_TYPE_SBP2 = 9,
};
enum GPSHeading {
GPS_HEADING_NONE = 0,
GPS_HEADING_HDT = 1,
@ -196,7 +187,7 @@ public:
AP_Int16 gps_alt_offset[2]; // gps alt error
AP_Int8 gps_disable[2]; // disable simulated GPS
AP_Int8 gps_delay[2]; // delay in samples
AP_Int8 gps_type[2]; // see enum GPSType
AP_Int8 gps_type[2]; // see enum SITL::GPS::Type
AP_Float gps_byteloss[2];// byte loss as a percent
AP_Int8 gps_numsats[2]; // number of visible satellites
AP_Vector3f gps_glitch[2]; // glitch offsets in lat, lon and altitude