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

AP_NavEKF: added SIM state to playback tool

This commit is contained in:
Andrew Tridgell 2014-01-03 16:07:39 +11:00
parent 2817a11f23
commit 093481786e
5 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -102,7 +102,7 @@ void setup()
plotf = fopen("plot.dat", "w");
plotf2 = fopen("plot2.dat", "w");
fprintf(plotf, "time 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 AHRS.Roll AHRS.Pitch AHRS.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");
}
@ -138,8 +138,11 @@ void loop()
NavEKF.getMagXYZ(magXYZ);
float temp = degrees(ekf_euler.z);
if (temp < 0.0f) temp = temp + 360.0f;
fprintf(plotf, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n",
fprintf(plotf, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n",
hal.scheduler->millis() * 0.001f,
LogReader.get_sim_attitude().x,
LogReader.get_sim_attitude().y,
LogReader.get_sim_attitude().z,
LogReader.get_attitude().x,
LogReader.get_attitude().y,
LogReader.get_attitude().z,

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

@ -151,6 +151,14 @@ bool LogReader::update(uint8_t &type)
attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
case LOG_SIMSTATE_MSG: {
struct log_AHRS msg;
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;
}
}
type = f.type;

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

@ -14,6 +14,7 @@ public:
bool update(uint8_t &type);
bool wait_type(uint8_t type);
const Vector3f &get_attitude(void) const { return attitude; }
const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
private:
int fd;
@ -28,6 +29,7 @@ private:
struct log_Format formats[32];
Vector3f attitude;
Vector3f sim_attitude;
void wait_timestamp(uint32_t timestamp);
};

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

@ -1,3 +1,3 @@
set style data lines
plot "plot.dat" using "ATT.Pitch", "" using "AHRS.Pitch", "" using "EKF.Pitch"
plot "plot.dat" using "SIM.Pitch", "" using "AHRS.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 "ATT.Roll", "" using "AHRS.Roll", "" using "EKF.Roll"
plot "plot.dat" using "SIM.Roll", "" using "AHRS.Roll", "" using "EKF.Roll"
pause -1 "hit return to exit"