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

AP_HAL_AVR_SITL: Adding Swift Binary Protocol stand-alone simulator

This commit is contained in:
Niels Joubert 2014-04-03 21:59:04 -07:00 committed by Andrew Tridgell
parent 00064ac883
commit e32b73f075
2 changed files with 182 additions and 0 deletions
libraries/AP_HAL_AVR_SITL

View File

@ -75,6 +75,8 @@ private:
#define MAX_GPS_DELAY 100
static gps_data _gps_data[MAX_GPS_DELAY];
static bool _gps_has_basestation_position;
static gps_data _gps_basestation_data;
static void _gps_write(const 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);
@ -84,6 +86,8 @@ private:
static uint16_t _gps_nmea_checksum(const char *s);
static void _gps_nmea_printf(const char *fmt, ...);
static void _update_gps_nmea(const struct gps_data *d);
static void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
static void _update_gps_sbp(const struct gps_data *d, bool sim_rtk);
static void _update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, double speedD, bool have_lock);

View File

@ -33,6 +33,8 @@ extern const AP_HAL::HAL& hal;
static uint8_t next_gps_index;
static uint8_t gps_delay;
SITL_State::gps_data SITL_State::_gps_data[MAX_GPS_DELAY];
bool SITL_State::_gps_has_basestation_position = false;
SITL_State::gps_data SITL_State::_gps_basestation_data;
// state of GPS emulation
static struct gps_state {
@ -550,6 +552,159 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d)
dstring);
}
void SITL_State::_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);
_gps_write((uint8_t*)&msg_type, 2);
_gps_write((uint8_t*)&sender_id, 2);
_gps_write(&len, 1);
if (len > 0) {
_gps_write((uint8_t*)payload, len);
}
uint16_t crc;
crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0);
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);
}
void SITL_State::_update_gps_sbp(const struct gps_data *d, bool sim_rtk)
{
struct PACKED sbp_gps_time_t {
uint16_t wn; //< GPS week number
uint32_t tow; //< GPS Time of Week rounded to the nearest ms
int32_t ns; //< Nanosecond remainder of rounded tow
uint8_t flags; //< Status flags (reserved)
} t;
struct PACKED sbp_pos_llh_t {
uint32_t tow; //< GPS Time of Week
double lat; //< Latitude
double lon; //< Longitude
double height; //< Height
uint16_t h_accuracy; //< Horizontal position accuracy estimate
uint16_t v_accuracy; //< Vertical position accuracy estimate
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags
} pos;
struct PACKED sbp_vel_ned_t {
uint32_t tow; //< GPS Time of Week
int32_t n; //< Velocity North coordinate
int32_t e; //< Velocity East coordinate
int32_t d; //< Velocity Down coordinate
uint16_t h_accuracy; //< Horizontal velocity accuracy estimate
uint16_t v_accuracy; //< Vertical velocity accuracy estimate
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags (reserved)
} velned;
struct PACKED sbp_dops_t {
uint32_t tow; //< GPS Time of Week
uint16_t gdop; //< Geometric Dilution of Precision
uint16_t pdop; //< Position Dilution of Precision
uint16_t tdop; //< Time Dilution of Precision
uint16_t hdop; //< Horizontal Dilution of Precision
uint16_t vdop; //< Vertical Dilution of Precision
} dops;
struct PACKED sbp_baseline_ecef_t {
uint32_t tow; //< GPS Time of Week
int32_t x; //< Baseline ECEF X coordinate
int32_t y; //< Baseline ECEF Y coordinate
int32_t z; //< Baseline ECEF Z coordinate
uint16_t accuracy; //< Position accuracy estimate
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags (reserved)
} baseline;
static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100;
static const uint16_t SBP_DOPS_MSGTYPE = 0x0206;
static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201;
static const uint16_t SBP_BASELINE_ECEF_MSGTYPE = 0x0202;
static const uint16_t SBP_VEL_NED_MSGTYPE = 0x0205;
uint16_t time_week;
uint32_t time_week_ms;
gps_time(&time_week, &time_week_ms);
t.wn = time_week;
t.tow = time_week_ms;
t.ns = 0;
t.flags = 0;
_sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t);
if (!d->have_lock) {
return;
}
pos.tow = time_week_ms;
pos.lon = d->longitude;
pos.lat= d->latitude;
pos.height = d->altitude;
pos.h_accuracy = 5e3;
pos.v_accuracy = 10e3;
pos.n_sats = _sitl->gps_numsats;
pos.flags = 0;
_sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
velned.tow = time_week_ms;
velned.n = 1e3 * d->speedN;
velned.e = 1e3 * d->speedE;
velned.d = 1e3 * d->speedD;
velned.h_accuracy = 5e3;
velned.v_accuracy = 5e3;
velned.n_sats = _sitl->gps_numsats;
velned.flags = 0;
_sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
dops.tow = time_week_ms;
dops.gdop = 1;
dops.pdop = 1;
dops.tdop = 1;
dops.hdop = 100;
dops.vdop = 1;
_sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), (uint8_t*)&dops);
//Also send baseline messages
if (sim_rtk && _gps_has_basestation_position) {
Vector3d homeLLH;
Vector3d currentLLH;
Vector3d homeECEF;
Vector3d currentECEF;
Vector3d baselineVector;
homeLLH[0] = _gps_basestation_data.latitude * DEG_TO_RAD_DOUBLE;
homeLLH[1] = _gps_basestation_data.longitude * DEG_TO_RAD_DOUBLE;
homeLLH[2] = _gps_basestation_data.altitude;
currentLLH[0] = d->latitude * DEG_TO_RAD_DOUBLE;
currentLLH[1] = d->longitude * DEG_TO_RAD_DOUBLE;
currentLLH[2] = d->altitude;
wgsllh2ecef(homeLLH, homeECEF);
wgsllh2ecef(currentLLH, currentECEF);
baselineVector = currentECEF - homeECEF;
baseline.tow = time_week_ms;
baseline.x = (int32_t) (baselineVector[0]*1e3); //Convert to MM
baseline.y = (int32_t) (baselineVector[1]*1e3); //Convert to MM
baseline.z = (int32_t) (baselineVector[2]*1e3); //Convert to MM
baseline.accuracy = 5e3;
baseline.n_sats = _sitl->gps_numsats;
baseline.flags = 0;
//printf("Sending baseline with length %f\n",baselineVector.length());
_sbp_send_message(SBP_BASELINE_ECEF_MSGTYPE, 0x2222, sizeof(baseline), (uint8_t*)&baseline);
}
}
/*
possibly send a new GPS packet
*/
@ -560,6 +715,20 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
char c;
Vector3f glitch_offsets = _sitl->gps_glitch;
//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;
}
}
// run at configured GPS rate (default 5Hz)
if ((hal.scheduler->millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) {
return;
@ -628,6 +797,15 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
case SITL::GPS_TYPE_NMEA:
_update_gps_nmea(&d);
break;
case SITL::GPS_TYPE_SBP:
_update_gps_sbp(&d, false);
break;
case SITL::GPS_TYPE_SBP_RTK:
_update_gps_sbp(&d, true);
break;
}
}