Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: fixed log playback for copter

This commit is contained in:
Andrew Tridgell 2014-01-05 11:39:43 +11:00
parent 78ff078988
commit c33da7b3a2
5 changed files with 213 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde
View File

@ -39,6 +39,7 @@
#include <AP_Compass.h>
#include <AP_Baro.h>
#include <AP_InertialSensor.h>
#include <AP_InertialNav.h>
#include <AP_NavEKF.h>
#include <stdio.h>
@ -51,21 +52,25 @@ static AP_Baro_HIL barometer;
static AP_GPS_HIL gps_driver;
static GPS *g_gps = &gps_driver;
static AP_Compass_HIL compass;
static AP_AHRS_DCM ahrs(ins, barometer, g_gps);
static AP_AHRS_NavEKF ahrs(ins, barometer, g_gps);
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
SITL sitl;
#endif
static NavEKF NavEKF(&ahrs, barometer);
static const NavEKF &NavEKF = ahrs.get_NavEKF();
static LogReader LogReader(ins, barometer, compass, g_gps);
static FILE *plotf;
static FILE *plotf2;
static uint32_t gps_fix_count;
void setup()
{
::printf("Starting\n");
LogReader.open_log("log.bin");
LogReader.wait_type(LOG_GPS_MSG);
@ -81,29 +86,49 @@ void setup()
compass.init();
ins.init(AP_InertialSensor::WARM_START, AP_InertialSensor::RATE_50HZ);
::printf("Waiting for 3D fix\n");
uint8_t goodFrameCount = 0;
while (goodFrameCount <= 10) // wait for readings to stabilise
{
LogReader.wait_type(LOG_GPS_MSG);
g_gps->update();
compass.read();
barometer.read();
LogReader.wait_type(LOG_IMU_MSG);
ahrs.update();
if ((g_gps->status() >= GPS::GPS_OK_FIX_3D) && (ahrs.yaw_sensor != 0)) goodFrameCount +=1;
printf("%u\n",hal.scheduler->millis());
}
barometer.calibrate();
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
NavEKF.InitialiseFilter();
plotf = fopen("plot.dat", "w");
plotf2 = fopen("plot2.dat", "w");
fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw ATT.Roll ATT.Pitch ATT.Yaw AHRS.Roll AHRS.Pitch AHRS.Yaw EKF.Roll EKF.Pitch EKF.Yaw\n");
fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw ATT.Roll ATT.Pitch ATT.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw\n");
fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ AX AY AZ MN ME MD MX MY MZ E1ref E2ref E3ref\n");
ahrs.set_ekf_use(true);
::printf("Waiting for InertialNav to start\n");
while (!ahrs.have_inertial_nav()) {
uint8_t type;
if (!LogReader.update(type)) break;
read_sensors(type);
if (type == LOG_GPS_MSG && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
gps_fix_count++;
if (gps_fix_count == 2) {
::printf("GPS Lock at %.7f %.7f %.2fm\n",
g_gps->latitude*1.0e-7f,
g_gps->longitude*1.0e-7f,
g_gps->altitude_cm*0.01f);
ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
barometer.calibrate();
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
}
}
}
if (!ahrs.have_inertial_nav()) {
::printf("Failed to start NavEKF\n");
exit(1);
}
}
static void read_sensors(uint8_t type)
{
if (type == LOG_GPS_MSG) {
g_gps->update();
barometer.read();
} else if (type == LOG_IMU_MSG) {
ahrs.update();
} else if ((type == LOG_PLANE_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
(type == LOG_COPTER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) {
compass.read();
}
}
void loop()
@ -115,13 +140,10 @@ void loop()
fclose(plotf);
exit(0);
}
switch (type) {
case LOG_GPS_MSG:
g_gps->update();
barometer.read();
break;
read_sensors(type);
case LOG_ATTITUDE_MSG: {
if ((type == LOG_PLANE_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
(type == LOG_COPTER_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) {
Vector3f ekf_euler;
Vector3f velNED;
Vector3f posNED;
@ -129,6 +151,8 @@ void loop()
Vector3f accelBias;
Vector3f magNED;
Vector3f magXYZ;
Vector3f DCM_attitude;
ahrs.get_secondary_attitude(DCM_attitude);
NavEKF.getEulerAngles(ekf_euler);
NavEKF.getVelNED(velNED);
NavEKF.getPosNED(posNED);
@ -146,9 +170,9 @@ void loop()
LogReader.get_attitude().x,
LogReader.get_attitude().y,
LogReader.get_attitude().z,
ahrs.roll_sensor*0.01f,
ahrs.pitch_sensor*0.01f,
ahrs.yaw_sensor*0.01f,
degrees(DCM_attitude.x),
degrees(DCM_attitude.y),
degrees(DCM_attitude.z),
degrees(ekf_euler.x),
degrees(ekf_euler.y),
degrees(ekf_euler.z));
@ -178,35 +202,6 @@ void loop()
LogReader.get_attitude().x,
LogReader.get_attitude().y,
LogReader.get_attitude().z);
#if 0
::printf("t=%.3f ATT: (%.1f %.1f %.1f) AHRS: (%.1f %.1f %.1f) EKF: (%.1f %.1f %.1f) ALT: %.1f GPS: %u %f %f\n",
hal.scheduler->millis() * 0.001f,
LogReader.get_attitude().x,
LogReader.get_attitude().y,
LogReader.get_attitude().z,
ahrs.roll_sensor*0.01f,
ahrs.pitch_sensor*0.01f,
ahrs.yaw_sensor*0.01f,
degrees(ekf_euler.x),
degrees(ekf_euler.y),
temp,
barometer.get_altitude(),
(unsigned)g_gps->status(),
g_gps->latitude*1.0e-7f,
g_gps->longitude*1.0e-7f);
#endif
break;
}
case LOG_COMPASS_MSG:
compass.read();
break;
case LOG_IMU_MSG:
ahrs.update();
NavEKF.UpdateFilter();
break;
}
}
}

168
libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.cpp
View File

@ -18,7 +18,10 @@
extern const AP_HAL::HAL& hal;
extern uint32_t gps_fix_count;
LogReader::LogReader(AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, GPS *&_gps) :
vehicle(VEHICLE_UNKNOWN),
fd(-1),
ins(_ins),
baro(_baro),
@ -36,7 +39,7 @@ bool LogReader::open_log(const char *logfile)
}
struct PACKED log_Compass {
struct PACKED log_Plane_Compass {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t mag_x;
@ -47,20 +50,20 @@ struct PACKED log_Compass {
int16_t offset_z;
};
struct PACKED log_Nav_Tuning {
struct PACKED log_Copter_Compass {
LOG_PACKET_HEADER;
uint32_t time_ms;
uint16_t yaw;
uint32_t wp_distance;
uint16_t target_bearing_cd;
uint16_t nav_bearing_cd;
int16_t altitude_error_cm;
int16_t airspeed_cm;
float altitude;
uint32_t groundspeed_cm;
int16_t mag_x;
int16_t mag_y;
int16_t mag_z;
int16_t offset_x;
int16_t offset_y;
int16_t offset_z;
int16_t motor_offset_x;
int16_t motor_offset_y;
int16_t motor_offset_z;
};
struct PACKED log_Attitude {
struct PACKED log_Plane_Attitude {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t roll;
@ -70,6 +73,89 @@ struct PACKED log_Attitude {
uint16_t error_yaw;
};
struct PACKED log_Copter_Attitude {
LOG_PACKET_HEADER;
int16_t control_roll;
int16_t roll;
int16_t control_pitch;
int16_t pitch;
uint16_t control_yaw;
uint16_t yaw;
};
struct PACKED log_Copter_Control_Tuning {
LOG_PACKET_HEADER;
int16_t throttle_in;
int16_t sonar_alt;
int32_t baro_alt;
float next_wp_alt;
int16_t desired_sonar_alt;
int16_t angle_boost;
int16_t climb_rate;
int16_t throttle_out;
int16_t desired_climb_rate;
};
void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length)
{
switch (type) {
case LOG_PLANE_COMPASS_MSG: {
struct log_Plane_Compass msg;
if(sizeof(msg) != length) {
printf("Bad COMPASS length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
compass.setHIL(Vector3i(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
compass.set_offsets(msg.offset_x, msg.offset_y, msg.offset_z);
break;
}
case LOG_PLANE_ATTITUDE_MSG: {
struct log_Plane_Attitude msg;
if(sizeof(msg) != length) {
printf("Bad ATTITUDE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
}
}
void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length)
{
switch (type) {
case LOG_COPTER_COMPASS_MSG: {
struct log_Copter_Compass msg;
if(sizeof(msg) != length) {
printf("Bad COMPASS length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
//wait_timestamp(msg.time_ms);
compass.setHIL(Vector3i(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
compass.set_offsets(msg.offset_x, msg.offset_y, msg.offset_z);
break;
}
case LOG_COPTER_ATTITUDE_MSG: {
struct log_Copter_Attitude msg;
if(sizeof(msg) != length) {
printf("Bad ATTITUDE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
//wait_timestamp(msg.time_ms);
attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
}
}
bool LogReader::update(uint8_t &type)
{
uint8_t hdr[3];
@ -77,6 +163,7 @@ bool LogReader::update(uint8_t &type)
return false;
}
if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
printf("bad log header\n");
return false;
}
@ -107,8 +194,32 @@ bool LogReader::update(uint8_t &type)
}
switch (f.type) {
case LOG_MESSAGE_MSG: {
struct log_Message msg;
if(sizeof(msg) != f.length) {
printf("Bad MESSAGE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
if (strncmp(msg.msg, "ArduPlane", strlen("ArduPlane")) == 0) {
vehicle = VEHICLE_PLANE;
::printf("Detected Plane\n");
} else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) {
vehicle = VEHICLE_COPTER;
::printf("Detected Copter\n");
} else if (strncmp(msg.msg, "APMRover2", strlen("APMRover2")) == 0) {
vehicle = VEHICLE_ROVER;
::printf("Detected Rover\n");
}
break;
}
case LOG_IMU_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
ins.set_gyro(Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
@ -118,6 +229,10 @@ bool LogReader::update(uint8_t &type)
case LOG_GPS_MSG: {
struct log_GPS msg;
if(sizeof(msg) != f.length) {
printf("Bad GPS length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.apm_time);
gps->setHIL(msg.apm_time,
@ -135,30 +250,25 @@ bool LogReader::update(uint8_t &type)
break;
}
case LOG_COMPASS_MSG: {
struct log_Compass msg;
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
//compass.setHIL(Vector3i(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z));
compass.setHIL(Vector3i(msg.mag_x, msg.mag_y, msg.mag_z));
break;
}
case LOG_ATTITUDE_MSG: {
struct log_Attitude msg;
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
case LOG_SIMSTATE_MSG: {
struct log_AHRS msg;
if(sizeof(msg) != f.length) {
printf("Bad SIMSTATE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
default:
if (vehicle == VEHICLE_PLANE) {
process_plane(f.type, data, f.length);
} else if (vehicle == VEHICLE_COPTER) {
process_copter(f.type, data, f.length);
}
break;
}
type = f.type;

21
libraries/AP_NavEKF/examples/AP_NavEKF/LogReader.h
View File

@ -1,8 +1,14 @@
enum log_messages {
LOG_NTUN_MSG = 2,
LOG_ATTITUDE_MSG = 10,
LOG_COMPASS_MSG = 12
// plane specific messages
LOG_PLANE_NTUN_MSG = 2,
LOG_PLANE_ATTITUDE_MSG = 10,
LOG_PLANE_COMPASS_MSG = 12,
// copter specific messages
LOG_COPTER_ATTITUDE_MSG = 1,
LOG_COPTER_COMPASS_MSG = 15,
LOG_COPTER_CONTROL_TUNING_MSG = 4
};
@ -16,6 +22,10 @@ public:
const Vector3f &get_attitude(void) const { return attitude; }
const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
enum vehicle_type { VEHICLE_UNKNOWN, VEHICLE_COPTER, VEHICLE_PLANE, VEHICLE_ROVER };
vehicle_type vehicle;
private:
int fd;
AP_InertialSensor &ins;
@ -26,10 +36,13 @@ private:
uint32_t ground_alt_cm;
uint8_t num_formats;
struct log_Format formats[32];
struct log_Format formats[100];
Vector3f attitude;
Vector3f sim_attitude;
void wait_timestamp(uint32_t timestamp);
void process_plane(uint8_t type, uint8_t *data, uint16_t length);
void process_copter(uint8_t type, uint8_t *data, uint16_t length);
};

2
libraries/AP_NavEKF/examples/AP_NavEKF/pitch.gnu
View File

@ -1,3 +1,3 @@
set style data lines
plot "plot.dat" using "SIM.Pitch", "" using "AHRS.Pitch", "" using "EKF.Pitch"
plot "plot.dat" using "SIM.Pitch", "" using "DCM.Pitch", "" using "EKF.Pitch"
pause -1 "hit return to exit"

2
libraries/AP_NavEKF/examples/AP_NavEKF/roll.gnu
View File

@ -1,3 +1,3 @@
set style data lines
plot "plot.dat" using "SIM.Roll", "" using "AHRS.Roll", "" using "EKF.Roll"
plot "plot.dat" using "SIM.Roll", "" using "DCM.Roll", "" using "EKF.Roll"
pause -1 "hit return to exit"