Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: added SIM state to playback tool

Browse Source
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"); plotf = fopen("plot.dat", "w");
plotf2 = fopen("plot2.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"); 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); NavEKF.getMagXYZ(magXYZ);
float temp = degrees(ekf_euler.z); float temp = degrees(ekf_euler.z);
if (temp < 0.0f) temp = temp + 360.0f; 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, 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().x,
LogReader.get_attitude().y, LogReader.get_attitude().y,
LogReader.get_attitude().z, 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); attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break; 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; 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 update(uint8_t &type);
bool wait_type(uint8_t type); bool wait_type(uint8_t type);
const Vector3f &get_attitude(void) const { return attitude; } const Vector3f &get_attitude(void) const { return attitude; }
const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
private: private:
int fd; int fd;
@ -28,6 +29,7 @@ private:
struct log_Format formats[32]; struct log_Format formats[32];
Vector3f attitude; Vector3f attitude;
Vector3f sim_attitude;
void wait_timestamp(uint32_t timestamp); 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 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" 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 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" pause -1 "hit return to exit"