fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 23:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

-added comments

Browse Source 
-removed unused print functions
-removed false _imu_time_last variable (correct is _time_last_imu)
This commit is contained in:
Roman
2016-02-14 09:49:26 +01:00
parent 9d7340e187
commit ce0ddc0207
7 changed files with 89 additions and 274 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
-81
View File
@@ -40,7 +40,6 @@
*/
#include "ekf.h"
#include <drivers/drv_hrt.h>
Ekf::Ekf():
_control_status{},
@@ -443,83 +442,3 @@ void Ekf::fuseRange()
{
}
void Ekf::printStates()
{
static int counter = 0;
if (counter % 50 == 0) {
printf("quaternion\n");
for (int i = 0; i < 4; i++) {
printf("quat %i %.5f\n", i, (double)_state.quat_nominal(i));
}
matrix::Euler<float> euler(_state.quat_nominal);
printf("yaw pitch roll %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0));
printf("vel\n");
for (int i = 0; i < 3; i++) {
printf("v %i %.5f\n", i, (double)_state.vel(i));
}
printf("pos\n");
for (int i = 0; i < 3; i++) {
printf("p %i %.5f\n", i, (double)_state.pos(i));
}
printf("gyro_scale\n");
for (int i = 0; i < 3; i++) {
printf("gs %i %.5f\n", i, (double)_state.gyro_scale(i));
}
printf("mag earth\n");
for (int i = 0; i < 3; i++) {
printf("mI %i %.5f\n", i, (double)_state.mag_I(i));
}
printf("mag bias\n");
for (int i = 0; i < 3; i++) {
printf("mB %i %.5f\n", i, (double)_state.mag_B(i));
}
counter = 0;
}
counter++;
}
void Ekf::printStatesFast()
{
static int counter_fast = 0;
if (counter_fast % 50 == 0) {
printf("quaternion\n");
for (int i = 0; i < 4; i++) {
printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i));
}
printf("vel\n");
for (int i = 0; i < 3; i++) {
printf("v %i %.5f\n", i, (double)_output_new.vel(i));
}
printf("pos\n");
for (int i = 0; i < 3; i++) {
printf("p %i %.5f\n", i, (double)_output_new.pos(i));
}
counter_fast = 0;
}
counter_fast++;
}