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

Replay: convert to new GPS API

Browse Source
This commit is contained in:
Andrew Tridgell 2014-03-31 18:46:01 +11:00
parent c51212da53
commit 60ad429d13
3 changed files with 33 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
Tools/Replay/LogReader.cpp
View File

@ -18,7 +18,7 @@
extern const AP_HAL::HAL& hal;
LogReader::LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, GPS *&_gps, AP_Airspeed &_airspeed) :
LogReader::LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed) :
vehicle(VEHICLE_UNKNOWN),
fd(-1),
ins(_ins),
@ -373,14 +373,19 @@ bool LogReader::update(uint8_t &type)
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.apm_time);
gps->setHIL(msg.status==3?GPS::FIX_3D:GPS::FIX_NONE,
Location loc;
loc.lat = msg.latitude;
loc.lng = msg.longitude;
loc.alt = msg.altitude;
loc.options = 0;
Vector3f vel(msg.ground_speed*0.01f*cosf(msg.ground_course*0.01f),
msg.ground_speed*0.01f*sinf(msg.ground_course*0.01f),
msg.vel_z);
gps.setHIL((AP_GPS::GPS_Status)msg.status,
msg.apm_time,
msg.latitude*1.0e-7f,
msg.longitude*1.0e-7f,
msg.altitude*1.0e-2f,
msg.ground_speed*1.0e-2f,
msg.ground_course*1.0e-2f,
0,
loc,
vel,
msg.num_sats);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;

4
Tools/Replay/LogReader.h
View File

@ -19,7 +19,7 @@ enum log_messages {
class LogReader
{
public:
LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, GPS *&_gps, AP_Airspeed &_airspeed);
LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed);
bool open_log(const char *logfile);
bool update(uint8_t &type);
bool wait_type(uint8_t type);
@ -43,7 +43,7 @@ private:
AP_InertialSensor &ins;
AP_Baro_HIL &baro;
AP_Compass_HIL &compass;
GPS *&gps;
AP_GPS &gps;
AP_Airspeed &airspeed;
uint8_t accel_mask;

30
Tools/Replay/Replay.pde
View File

@ -58,12 +58,11 @@ static Parameters g;
static AP_InertialSensor_HIL ins;
static AP_Baro_HIL barometer;
static AP_GPS_HIL gps_driver;
static GPS *g_gps = &gps_driver;
static AP_GPS gps;
static AP_Compass_HIL compass;
static AP_AHRS_NavEKF ahrs(ins, barometer, g_gps);
static GPS_Glitch gps_glitch(g_gps);
static AP_InertialNav inertial_nav(ahrs, barometer, g_gps, gps_glitch);
static AP_AHRS_NavEKF ahrs(ins, barometer, gps);
static GPS_Glitch gps_glitch(gps);
static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch);
static AP_Vehicle::FixedWing aparm;
static AP_Airspeed airspeed(aparm);
@ -73,7 +72,7 @@ SITL sitl;
static const NavEKF &NavEKF = ahrs.get_NavEKF();
static LogReader LogReader(ins, barometer, compass, g_gps, airspeed);
static LogReader LogReader(ins, barometer, compass, gps, airspeed);
static FILE *plotf;
static FILE *plotf2;
@ -221,14 +220,17 @@ void setup()
uint8_t type;
if (!LogReader.update(type)) break;
read_sensors(type);
if (type == LOG_GPS_MSG && g_gps->status() >= GPS::GPS_OK_FIX_3D && done_baro_init && !done_home_init) {
if (type == LOG_GPS_MSG &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
done_baro_init && !done_home_init) {
const Location &loc = gps.location();
::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n",
g_gps->latitude*1.0e-7f,
g_gps->longitude*1.0e-7f,
g_gps->altitude_cm*0.01f,
loc.lat * 1.0e-7f,
loc.lng * 1.0e-7f,
loc.alt * 0.01f,
hal.scheduler->millis()*0.001f);
ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
ahrs.set_home(loc);
compass.set_initial_location(loc.lat, loc.lng);
inertial_nav.setup_home_position();
done_home_init = true;
}
@ -263,8 +265,8 @@ static void read_sensors(uint8_t type)
set_user_parameters();
}
if (type == LOG_GPS_MSG) {
g_gps->update();
if (g_gps->status() >= GPS::GPS_OK_FIX_3D) {
gps.update();
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
ahrs.estimate_wind();
}
} else if (type == LOG_IMU_MSG) {