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

AP_NavEKF: support dual sensors in log replay, and fix flight altitude

Browse Source
This commit is contained in:
Andrew Tridgell 2014-02-23 08:17:55 +11:00
parent 707cc2b532
commit ffbd655ba0
3 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -237,7 +237,7 @@ void loop()
LogReader.get_attitude().z,
LogReader.get_inavpos().x,
LogReader.get_inavpos().y,
LogReader.get_inavpos().z,
LogReader.get_relalt(),
degrees(DCM_attitude.x),
degrees(DCM_attitude.y),
degrees(DCM_attitude.z),

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

@ -256,8 +256,21 @@ bool LogReader::update(uint8_t &type)
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
ins.set_gyro(Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
ins.set_accel(Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
break;
}
case LOG_IMU2_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU2 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
break;
}
@ -280,6 +293,7 @@ bool LogReader::update(uint8_t &type)
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
rel_altitude = msg.rel_altitude*0.01f;
break;
}

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

@ -23,6 +23,7 @@ public:
const Vector3f &get_attitude(void) const { return attitude; }
const Vector3f &get_inavpos(void) const { return inavpos; }
const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
const float &get_relalt(void) const { return rel_altitude; }
enum vehicle_type { VEHICLE_UNKNOWN, VEHICLE_COPTER, VEHICLE_PLANE, VEHICLE_ROVER };
@ -44,6 +45,7 @@ private:
Vector3f attitude;
Vector3f sim_attitude;
Vector3f inavpos;
float rel_altitude;
void wait_timestamp(uint32_t timestamp);