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

Merge pull request #156 from PX4/fix_printfs

Browse Source 
EKF: use ECL printfs everywhere
This commit is contained in:
Paul Riseborough
2016-06-07 09:45:00 +10:00
parent 081e17729c ab0ac05b47
commit 3c84d37175
4 changed files with 30 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+11 -10
View File
@@ -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;
EKF/estimator_interface.cpp
+2 -1
View File
@@ -43,6 +43,7 @@
#define __STDC_FORMAT_MACROS
#include <inttypes.h>
#include <math.h>
#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;
}
EKF/gps_checks.cpp
+4 -2
View File
@@ -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;
ecl.h
+13 -1
View File
@@ -36,11 +36,23 @@
* Adapter / shim layer for system calls needed by ECL
*
*/
#pragma once
#if defined(__PX4_POSIX) || defined(__PX4_NUTTX)
#include <drivers/drv_hrt.h>
#include <px4_log.h>
#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