From ecfd8c867ac97444ae7611ce7ad3b2d42568384c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 2 Jun 2016 15:58:26 +0100 Subject: [PATCH] EKF: use ECL printfs everywhere - Changes all printfs to ECL printfs - Add ECL_ERR. - Include ecl.h where needed. - Add forgotten pragma once. --- EKF/control.cpp | 21 +++++++++++---------- EKF/estimator_interface.cpp | 3 ++- EKF/gps_checks.cpp | 6 ++++-- ecl.h | 4 +++- 4 files changed, 20 insertions(+), 14 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 7a8ff9fff1..da5625b143 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -39,6 +39,7 @@ * */ +#include #include "ekf.h" void Ekf::controlFusionModes() @@ -79,7 +80,7 @@ void Ekf::controlExternalVisionAiding() if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // turn on use of external vision measurements for position and height _control_status.flags.ev_pos = true; - printf("EKF switching to external vision position fusion\n"); + ECL_INFO("EKF switching to external vision position fusion"); // turn off other forms of height aiding _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; @@ -119,7 +120,7 @@ void Ekf::controlExternalVisionAiding() _control_status.flags.mag_3D = false; _control_status.flags.mag_dec = false; - printf("EKF switching to external vision yaw fusion\n"); + ECL_INFO("EKF switching to external vision yaw fusion"); } } @@ -326,7 +327,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.ev_hgt = false; // request a reset reset_height = true; - printf("EKF baro hgt timeout - reset to GPS\n"); + ECL_INFO("EKF baro hgt timeout - reset to GPS"); } else if (reset_to_baro){ // set height sensor health _baro_hgt_faulty = false; @@ -337,7 +338,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.ev_hgt = false; // request a reset reset_height = true; - printf("EKF baro hgt timeout - reset to baro\n"); + ECL_INFO("EKF baro hgt timeout - reset to baro"); } else { // we have nothing we can reset to // deny a reset @@ -377,7 +378,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.ev_hgt = false; // request a reset reset_height = true; - printf("EKF gps hgt timeout - reset to baro\n"); + ECL_INFO("EKF gps hgt timeout - reset to baro"); } else if (reset_to_gps) { // set height sensor health _gps_hgt_faulty = false; @@ -388,7 +389,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.ev_hgt = false; // request a reset reset_height = true; - printf("EKF gps hgt timeout - reset to GPS\n"); + ECL_INFO("EKF gps hgt timeout - reset to GPS"); } else { // we have nothing to reset to reset_height = false; @@ -421,7 +422,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.ev_hgt = false; // request a reset reset_height = true; - printf("EKF rng hgt timeout - reset to baro\n"); + ECL_INFO("EKF rng hgt timeout - reset to baro"); } else if (reset_to_rng) { // set height sensor health _rng_hgt_faulty = false; @@ -432,7 +433,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.ev_hgt = false; // request a reset reset_height = true; - printf("EKF rng hgt timeout - reset to rng hgt\n"); + ECL_INFO("EKF rng hgt timeout - reset to rng hgt"); } else { // we have nothing to reset to reset_height = false; @@ -465,7 +466,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.ev_hgt = false; // request a reset reset_height = true; - printf("EKF ev hgt timeout - reset to baro\n"); + ECL_INFO("EKF ev hgt timeout - reset to baro"); } else if (reset_to_ev) { // reset the height mode _control_status.flags.baro_hgt = false; @@ -474,7 +475,7 @@ void Ekf::controlHeightSensorTimeouts() _control_status.flags.ev_hgt = true; // request a reset reset_height = true; - printf("EKF ev hgt timeout - reset to ev hgt\n"); + ECL_INFO("EKF ev hgt timeout - reset to ev hgt"); } else { // we have nothing to reset to reset_height = false; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index f615e4ab86..1a6bda9e55 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -43,6 +43,7 @@ #define __STDC_FORMAT_MACROS #include #include +#include #include "estimator_interface.h" #include "mathlib.h" @@ -335,7 +336,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _flow_buffer.allocate(OBS_BUFFER_LENGTH) && _ext_vision_buffer.allocate(OBS_BUFFER_LENGTH) && _output_buffer.allocate(IMU_BUFFER_LENGTH))) { - printf("EKF buffer allocation failed!"); + ECL_ERR("EKF buffer allocation failed!"); unallocate_buffers(); return false; } diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 184b3c4edf..91066bd897 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -39,9 +39,11 @@ * */ +#include #include "ekf.h" #include "mathlib.h" #include "geo.h" + // GPS pre-flight check bit locations #define MASK_GPS_NSATS (1<<0) #define MASK_GPS_GDOP (1<<1) @@ -59,7 +61,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) if (!_NED_origin_initialised) { // we have good GPS data so can now set the origin's WGS-84 position if (gps_is_good(gps) && !_NED_origin_initialised) { - printf("EKF gps is good - setting origin\n"); + ECL_INFO("EKF gps is good - setting origin"); // Set the origin's WGS-84 position to the last gps fix double lat = gps->lat / 1.0e7; double lon = gps->lon / 1.0e7; @@ -84,7 +86,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) // if the user has selected GPS as the primary height source, switch across to using it if (_primary_hgt_source == VDIST_SENSOR_GPS) { - printf("EKF switching to GPS height\n"); + ECL_INFO("EKF switching to GPS height"); _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; diff --git a/ecl.h b/ecl.h index 39e1f50809..5b36aa0492 100644 --- a/ecl.h +++ b/ecl.h @@ -36,11 +36,13 @@ * Adapter / shim layer for system calls needed by ECL * */ +#pragma once #include #include #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time -#define ECL_WARN PX4_WARN #define ECL_INFO PX4_INFO +#define ECL_WARN PX4_WARN +#define ECL_ERR PX4_ERR