diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 30439fad58..0a55756163 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -101,11 +100,11 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p ]\n\n"); - exit(1); } /** @@ -120,6 +119,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) { if (argc < 2) { usage("missing command"); + return 1; } if (!strcmp(argv[1], "start")) { @@ -137,29 +137,29 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) 7700, attitude_estimator_ekf_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { thread_should_exit = true; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); - exit(0); + return 0; } else { warnx("not started"); - exit(1); + return 1; } - exit(0); + return 0; } usage("unrecognized command"); - exit(1); + return 1; } /* @@ -536,7 +536,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) debugOutput); /* swap values for next iteration, check for fatal inputs */ - if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + if (std::isfinite(euler[0]) && std::isfinite(euler[1]) && std::isfinite(euler[2])) { memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); @@ -546,7 +546,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } if (last_data > 0 && raw.timestamp - last_data > 30000) { +<<<<<<< HEAD warnx("sensor data missed! (%llu)\n", raw.timestamp - last_data); +======= + warnx("data missed! (%llu)\n", static_cast(raw.timestamp - last_data)); +>>>>>>> att EKF: Enable execution on Linux } last_data = raw.timestamp; @@ -583,8 +587,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) memcpy(&att.q[0],&q.data[0],sizeof(att.q)); att.R_valid = true; +<<<<<<< HEAD if (isfinite(att.q[0]) && isfinite(att.q[1]) && isfinite(att.q[2]) && isfinite(att.q[3])) { +======= + if (std::isfinite(att.roll) && std::isfinite(att.pitch) && std::isfinite(att.yaw)) { +>>>>>>> att EKF: Enable execution on Linux // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index fe480e12b7..328236d231 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -41,6 +41,7 @@ #include "attitude_estimator_ekf_params.h" #include +#include /* Extended Kalman Filter covariances */