Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 22:24:47 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

Proper attitude initialization, finite check on attitude outputs

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-25 16:29:17 +02:00
parent 569938e680
commit c71f2ea204
1 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+10 -3
View File
@@ -62,6 +62,7 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter_initialize.h"
#include "codegen/attitudeKalmanfilter.h" #include "codegen/attitudeKalmanfilter.h"
@@ -200,7 +201,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
uint64_t last_run = hrt_absolute_time(); uint64_t last_run = hrt_absolute_time();
struct sensor_combined_s raw; struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
uint64_t last_data = 0; uint64_t last_data = 0;
uint64_t last_measurement = 0; uint64_t last_measurement = 0;
@@ -350,7 +353,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
static bool const_initialized = false; static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */ /* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05 && dt > 0.005) if (!const_initialized && dt < 0.05f && dt > 0.005f)
{ {
dt = 0.005f; dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params); parameters_update(&ekf_param_handles, &ekf_params);
@@ -445,8 +448,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
att.R_valid = true; att.R_valid = true;
// Broadcast if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); // Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
} else {
warnx("NaN in roll/pitch/yaw estimate!");
}
} }
} }
} }