diff --git a/EKF/control.cpp b/EKF/control.cpp index 3188311189..2f6083ce5c 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -39,6 +39,7 @@ * */ +#include "../ecl.h" #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; @@ -137,7 +138,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"); } } @@ -344,7 +345,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; @@ -355,7 +356,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 @@ -395,7 +396,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; @@ -406,7 +407,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; @@ -439,7 +440,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; @@ -450,7 +451,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; @@ -483,7 +484,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; @@ -492,7 +493,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 7427967785..1474c84493 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -43,6 +43,7 @@ #define __STDC_FORMAT_MACROS #include #include +#include "../ecl.h" #include "estimator_interface.h" #include "mathlib.h" @@ -333,7 +334,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..ffdd04c5e8 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -39,9 +39,11 @@ * */ +#include "../ecl.h" #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..bca60865e9 100644 --- a/ecl.h +++ b/ecl.h @@ -36,11 +36,23 @@ * Adapter / shim layer for system calls needed by ECL * */ +#pragma once + +#if defined(__PX4_POSIX) || defined(__PX4_NUTTX) #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 + +#else + +#define ECL_INFO printf +#define ECL_WARN printf +#define ECL_ERR printf + +#endif