fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 15:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Rationalise console messages

Browse Source 
Combine the observation action buffer and alignment messages
Ensure all data timeout messages are warnings.
Use consistent terminology.
This commit is contained in:
Paul Riseborough
2016-12-28 15:58:03 +11:00
committed by Lorenz Meier
parent 934890c2df
commit 3c4c09593f
4 changed files with 29 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+26 -14
View File
@@ -61,7 +61,19 @@ void Ekf::controlFusionModes()
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
ECL_INFO("EKF alignment complete");
// send alignment status message to the console
if (_control_status.flags.baro_hgt) {
ECL_INFO("EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length);
} else if (_control_status.flags.ev_hgt) {
ECL_INFO("EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length);
} else if (_control_status.flags.gps_hgt) {
ECL_INFO("EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length);
} else if (_control_status.flags.rng_hgt) {
ECL_INFO("EKF aligned, (range height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length);
} else {
ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)",(int)_imu_buffer_length,(int)_obs_buffer_length);
}
}
@@ -108,7 +120,7 @@ void Ekf::controlExternalVisionFusion()
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;
ECL_INFO("EKF switching to external vision position fusion");
ECL_INFO("EKF commencing external vision position fusion");
// turn off other forms of height aiding
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
@@ -170,7 +182,7 @@ void Ekf::controlExternalVisionFusion()
_control_status.flags.mag_3D = false;
_control_status.flags.mag_dec = false;
ECL_INFO("EKF switching to external vision yaw fusion");
ECL_INFO("EKF commencing external vision yaw fusion");
}
}
@@ -342,7 +354,7 @@ void Ekf::controlGpsFusion()
}
if (_control_status.flags.gps) {
ECL_INFO("EKF commencing GPS aiding");
ECL_INFO("EKF commencing GPS fusion");
_time_last_gps = _time_last_imu;
}
@@ -366,7 +378,7 @@ void Ekf::controlGpsFusion()
// Reset states to the last GPS measurement
resetPosition();
resetVelocity();
ECL_WARN("EKF GPS fusion timout - resetting to GPS");
ECL_WARN("EKF GPS fusion timeout - reset to GPS");
// Reset the timeout counters
_time_last_pos_fuse = _time_last_imu;
@@ -415,7 +427,7 @@ void Ekf::controlGpsFusion()
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
ECL_WARN("EKF GPS fusion timout - stopping GPS aiding");
ECL_WARN("EKF measurement timeout - stopping navigation");
}
}
@@ -484,7 +496,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_INFO("EKF baro hgt timeout - reset to GPS");
ECL_WARN("EKF baro hgt timeout - reset to GPS");
} else if (reset_to_baro){
// set height sensor health
@@ -498,7 +510,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_INFO("EKF baro hgt timeout - reset to baro");
ECL_WARN("EKF baro hgt timeout - reset to baro");
} else {
// we have nothing we can reset to
@@ -543,7 +555,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_INFO("EKF gps hgt timeout - reset to baro");
ECL_WARN("EKF gps hgt timeout - reset to baro");
} else if (reset_to_gps) {
// set height sensor health
@@ -557,7 +569,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_INFO("EKF gps hgt timeout - reset to GPS");
ECL_WARN("EKF gps hgt timeout - reset to GPS");
} else {
// we have nothing to reset to
@@ -595,7 +607,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_INFO("EKF rng hgt timeout - reset to baro");
ECL_WARN("EKF rng hgt timeout - reset to baro");
} else if (reset_to_rng) {
// set height sensor health
@@ -609,7 +621,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_INFO("EKF rng hgt timeout - reset to rng hgt");
ECL_WARN("EKF rng hgt timeout - reset to rng hgt");
} else {
// we have nothing to reset to
@@ -647,7 +659,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_INFO("EKF ev hgt timeout - reset to baro");
ECL_WARN("EKF ev hgt timeout - reset to baro");
} else if (reset_to_ev) {
// reset the height mode
@@ -658,7 +670,7 @@ void Ekf::controlHeightSensorTimeouts()
// request a reset
reset_height = true;
ECL_INFO("EKF ev hgt timeout - reset to ev hgt");
ECL_WARN("EKF ev hgt timeout - reset to ev hgt");
} else {
// we have nothing to reset to
EKF/ekf.cpp
-5
View File
@@ -400,16 +400,11 @@ bool Ekf::initialiseFilter(void)
baroSample baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt;
_state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance);
ECL_INFO("EKF using range finder height - commencing alignment");
} else if (_control_status.flags.ev_hgt) {
// if we are using external vision data for height, then the vertical position state needs to be reset
// because the initialisation position is not the zero datum
resetHeight();
ECL_INFO("EKF using vision height - commencing alignment");
} else if (_control_status.flags.baro_hgt){
ECL_INFO("EKF using pressure height - commencing alignment");
}
EKF/estimator_interface.cpp
-3
View File
@@ -362,9 +362,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
_obs_buffer_length = math::min(_obs_buffer_length,_imu_buffer_length);
ECL_INFO("EKF IMU buffer length = %i",(int)_imu_buffer_length);
ECL_INFO("EKF observation buffer length = %i",(int)_obs_buffer_length);
if (!(_imu_buffer.allocate(_imu_buffer_length) &&
_gps_buffer.allocate(_obs_buffer_length) &&
_mag_buffer.allocate(_obs_buffer_length) &&
EKF/gps_checks.cpp
+3 -2
View File
@@ -61,7 +61,6 @@ 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) {
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;
@@ -86,12 +85,14 @@ 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) {
ECL_INFO("EKF switching to GPS height");
ECL_INFO("EKF GPS checks passed (WGS-84 origin set, using GPS height)");
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
// zero the sensor offset
_hgt_sensor_offset = 0.0f;
} else {
ECL_INFO("EKF GPS checks passed (WGS-84 origin set)");
}
}
}