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:
parent
00064ac883
commit
e32b73f075
libraries/AP_HAL_AVR_SITL
@ -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);
|
||||
|
@ -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;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user