Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: added simulation of the original MTK GPS

Browse Source
This commit is contained in:
Andrew Tridgell 2013-02-16 20:16:13 +11:00
parent 2ec2c58f67
commit 10cd466035
2 changed files with 135 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

@ -55,8 +55,10 @@ private:
// from the timer // from the timer
static void _update_barometer(float height); static void _update_barometer(float height);
static void _update_compass(float roll, float pitch, float yaw); static void _update_compass(float roll, float pitch, float yaw);
static void _update_gps(double latitude, double longitude, float altitude, static void _update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, bool have_lock); double speedN, double speedE, bool have_lock);
static void _update_ins(float roll, float pitch, float yaw, // Relative to earth static void _update_ins(float roll, float pitch, float yaw, // Relative to earth
double rollRate, double pitchRate,double yawRate, // Local to plane double rollRate, double pitchRate,double yawRate, // Local to plane
double xAccel, double yAccel, double zAccel, // Local to plane double xAccel, double yAccel, double zAccel, // Local to plane
@ -68,7 +70,6 @@ private:
static float _rand_float(void); static float _rand_float(void);
static Vector3f _rand_vec3f(void); static Vector3f _rand_vec3f(void);
static Vector3f _heading_to_mag(float roll, float pitch, float yaw); static Vector3f _heading_to_mag(float roll, float pitch, float yaw);
static void _gps_send(uint8_t msgid, uint8_t *buf, uint16_t size);
// signal handlers // signal handlers
static void _sig_fpe(int signum); static void _sig_fpe(int signum);

176
libraries/AP_HAL_AVR_SITL/sitl_gps.cpp
View File

@ -1,3 +1,4 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/* /*
SITL handling SITL handling
@ -22,6 +23,7 @@
#include "../AP_GPS/AP_GPS_UBLOX.h" #include "../AP_GPS/AP_GPS_UBLOX.h"
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <unistd.h> #include <unistd.h>
#include <time.h>
using namespace AVR_SITL; using namespace AVR_SITL;
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -88,7 +90,7 @@ int SITL_State::gps_pipe(void)
/* /*
send a UBLOX GPS message send a UBLOX GPS message
*/ */
void SITL_State::_gps_send(uint8_t msgid, uint8_t *buf, uint16_t size) static void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
{ {
const uint8_t PREAMBLE1 = 0xb5; const uint8_t PREAMBLE1 = 0xb5;
const uint8_t PREAMBLE2 = 0x62; const uint8_t PREAMBLE2 = 0x62;
@ -114,10 +116,9 @@ void SITL_State::_gps_send(uint8_t msgid, uint8_t *buf, uint16_t size)
/* /*
possibly send a new GPS UBLOX packet send a new set of GPS UBLOX packets
*/ */
void SITL_State::_update_gps(double latitude, double longitude, float altitude, static void _update_gps_ubx(const struct gps_data *d)
double speedN, double speedE, bool have_lock)
{ {
#pragma pack(push,1) #pragma pack(push,1)
struct ubx_nav_posllh { struct ubx_nav_posllh {
@ -173,9 +174,123 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
const uint8_t MSG_STATUS = 0x3; const uint8_t MSG_STATUS = 0x3;
const uint8_t MSG_VELNED = 0x12; const uint8_t MSG_VELNED = 0x12;
const uint8_t MSG_SOL = 0x6; const uint8_t MSG_SOL = 0x6;
pos.time = hal.scheduler->millis(); // FIX
pos.longitude = d->longitude * 1.0e7;
pos.latitude = d->latitude * 1.0e7;
pos.altitude_ellipsoid = d->altitude*1000.0;
pos.altitude_msl = d->altitude*1000.0;
pos.horizontal_accuracy = 5;
pos.vertical_accuracy = 10;
status.time = pos.time;
status.fix_type = d->have_lock?3:0;
status.fix_status = d->have_lock?1:0;
status.differential_status = 0;
status.res = 0;
status.time_to_first_fix = 0;
status.uptime = hal.scheduler->millis();
velned.time = pos.time;
velned.ned_north = 100.0 * d->speedN;
velned.ned_east = 100.0 * d->speedE;
velned.ned_down = 0;
velned.speed_2d = pythagorous2(d->speedN, d->speedE) * 100;
velned.speed_3d = velned.speed_2d;
velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0;
if (velned.heading_2d < 0.0) {
velned.heading_2d += 360.0 * 100000.0;
}
velned.speed_accuracy = 2;
velned.heading_accuracy = 4;
memset(&sol, 0, sizeof(sol));
sol.fix_type = d->have_lock?3:0;
sol.fix_status = 221;
sol.satellites = d->have_lock?10:3;
_gps_send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
_gps_send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status));
_gps_send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
_gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol));
}
static void swap_uint32(uint32_t *v, uint8_t n)
{
while (n--) {
*v = htonl(*v);
v++;
}
}
/*
MTK type simple checksum
*/
static void mtk_checksum(const uint8_t *data, uint8_t n, uint8_t *ck_a, uint8_t *ck_b)
{
*ck_a = *ck_b = 0;
while (n--) {
*ck_a += *data++;
*ck_b += *ck_a;
}
}
/*
send a new GPS MTK packet
*/
static void _update_gps_mtk(const struct gps_data *d)
{
#pragma pack(push,1)
struct mtk_msg {
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_time;
uint8_t ck_a;
uint8_t ck_b;
} p;
#pragma pack(pop)
p.preamble1 = 0xb5;
p.preamble2 = 0x62;
p.msg_class = 1;
p.msg_id = 5;
p.latitude = d->latitude * 1.0e6;
p.longitude = d->longitude * 1.0e6;
p.altitude = d->altitude * 100;
p.ground_speed = pythagorous2(d->speedN, d->speedE) * 100;
p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 1000000.0;
if (p.ground_course < 0.0) {
p.ground_course += 360.0 * 1000000.0;
}
p.satellites = d->have_lock?10:3;
p.fix_type = d->have_lock?3:1;
p.utc_time = time(NULL);
swap_uint32((uint32_t *)&p.latitude, 5);
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));
}
/*
possibly send a new GPS packet
*/
void SITL_State::_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, bool have_lock)
{
struct gps_data d; struct gps_data d;
// 5Hz, to match the real UBlox config in APM // 5Hz, to match the real config in APM
if (hal.scheduler->millis() - gps_state.last_update < 200) { if (hal.scheduler->millis() - gps_state.last_update < 200) {
return; return;
} }
@ -204,48 +319,23 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude,
} }
} }
pos.time = hal.scheduler->millis(); // FIX
pos.longitude = d.longitude * 1.0e7;
pos.latitude = d.latitude * 1.0e7;
pos.altitude_ellipsoid = d.altitude*1000.0;
pos.altitude_msl = d.altitude*1000.0;
pos.horizontal_accuracy = 5;
pos.vertical_accuracy = 10;
status.time = pos.time;
status.fix_type = d.have_lock?3:0;
status.fix_status = d.have_lock?1:0;
status.differential_status = 0;
status.res = 0;
status.time_to_first_fix = 0;
status.uptime = hal.scheduler->millis();
velned.time = pos.time;
velned.ned_north = 100.0 * d.speedN;
velned.ned_east = 100.0 * d.speedE;
velned.ned_down = 0;
velned.speed_2d = pythagorous2(d.speedN, d.speedE) * 100;
velned.speed_3d = velned.speed_2d;
velned.heading_2d = ToDeg(atan2f(d.speedE, d.speedN)) * 100000.0;
if (velned.heading_2d < 0.0) {
velned.heading_2d += 360.0 * 100000.0;
}
velned.speed_accuracy = 2;
velned.heading_accuracy = 4;
memset(&sol, 0, sizeof(sol));
sol.fix_type = d.have_lock?3:0;
sol.fix_status = 221;
sol.satellites = d.have_lock?10:3;
if (gps_state.gps_fd == 0) { if (gps_state.gps_fd == 0) {
return; return;
} }
_gps_send(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); switch ((SITL::GPSType)_sitl->gps_type.get()) {
_gps_send(MSG_STATUS, (uint8_t*)&status, sizeof(status)); case SITL::GPS_TYPE_UBLOX:
_gps_send(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); _update_gps_ubx(&d);
_gps_send(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); break;
case SITL::GPS_TYPE_MTK:
_update_gps_mtk(&d);
break;
case SITL::GPS_TYPE_MTK16:
case SITL::GPS_TYPE_MTK19:
break;
}
} }
#endif #endif