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

Reduce stored strings, to save flash space (#815)

Browse Source
This commit is contained in:
kritz 2020-05-13 08:09:26 +02:00 committed by GitHub
parent 98801ad17b
commit 24f2e60b7e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 48 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/airspeed_fusion.cpp
View File

@ -94,16 +94,18 @@ void Ekf::fuseAirspeed()
_fault_status.flags.bad_airspeed = true;
// if we are getting aiding from other sources, warn and reset the wind states and covariances only
const char* action_string = nullptr;
if (update_wind_only) {
resetWindStates();
resetWindCovariance();
ECL_ERR_TIMESTAMPED("airspeed fusion badly conditioned - wind covariance reset");
action_string = "wind";
} else {
initialiseCovariance();
_state.wind_vel.setZero();
ECL_ERR_TIMESTAMPED("airspeed fusion badly conditioned - full covariance reset");
action_string = "full";
}
ECL_ERR("airspeed badly conditioned - %s covariance reset", action_string);
return;
}

42
EKF/control.cpp
View File

@ -71,14 +71,14 @@ void Ekf::controlFusionModes()
height_source = "gps";
} else if (_control_status.flags.rng_hgt) {
height_source = "range";
height_source = "rng";
} else {
height_source = "unknown";
}
if(height_source){
ECL_INFO("%llu: EKF aligned, (%s height, IMU buf: %i, OBS buf: %i)",
ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
(unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
}
}
@ -537,7 +537,7 @@ void Ekf::controlGpsFusion()
stopMagHdgFusion();
stopMag3DFusion();
ECL_INFO_TIMESTAMPED("commencing GPS yaw fusion");
ECL_INFO_TIMESTAMPED("starting GPS yaw fusion");
}
}
@ -588,7 +588,7 @@ void Ekf::controlGpsFusion()
}
if (_control_status.flags.gps) {
ECL_INFO_TIMESTAMPED("commencing GPS fusion");
ECL_INFO_TIMESTAMPED("starting GPS fusion");
_time_last_gps = _time_last_imu;
}
}
@ -606,7 +606,7 @@ void Ekf::controlGpsFusion()
if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
resetPosition();
}
ECL_WARN_TIMESTAMPED("GPS data quality poor - stopping use");
ECL_WARN_TIMESTAMPED("GPS quality poor - stopping use");
}
// handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before
@ -760,6 +760,8 @@ void Ekf::controlHeightSensorTimeouts()
if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
bool request_height_reset = false;
const char* failing_height_source = nullptr;
const char* new_height_source = nullptr;
if (_control_status.flags.baro_hgt) {
// check if GPS height is available
@ -780,11 +782,13 @@ void Ekf::controlHeightSensorTimeouts()
startGpsHgtFusion();
request_height_reset = true;
ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to GPS");
failing_height_source = "baro";
new_height_source = "gps";
} else if (!_baro_hgt_faulty) {
request_height_reset = true;
ECL_WARN_TIMESTAMPED("baro hgt timeout - reset to baro");
failing_height_source = "baro";
new_height_source = "baro";
}
} else if (_control_status.flags.gps_hgt) {
@ -806,24 +810,28 @@ void Ekf::controlHeightSensorTimeouts()
startBaroHgtFusion();
request_height_reset = true;
ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to baro");
failing_height_source = "gps";
new_height_source = "baro";
} else if (!_gps_hgt_intermittent) {
request_height_reset = true;
ECL_WARN_TIMESTAMPED("gps hgt timeout - reset to GPS");
failing_height_source = "gps";
new_height_source = "gps";
}
} else if (_control_status.flags.rng_hgt) {
if (_range_sensor.isHealthy()) {
request_height_reset = true;
ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to rng hgt");
failing_height_source = "rng";
new_height_source = "rng";
} else if (!_baro_hgt_faulty) {
startBaroHgtFusion();
request_height_reset = true;
ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to baro");
failing_height_source = "rng";
new_height_source = "baro";
}
} else if (_control_status.flags.ev_hgt) {
@ -833,16 +841,22 @@ void Ekf::controlHeightSensorTimeouts()
if (ev_data_available) {
request_height_reset = true;
ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to ev hgt");
failing_height_source = "ev";
new_height_source = "ev";
} else if (!_baro_hgt_faulty) {
startBaroHgtFusion();
request_height_reset = true;
ECL_WARN_TIMESTAMPED("ev hgt timeout - reset to baro");
failing_height_source = "ev";
new_height_source = "baro";
}
}
if (failing_height_source && new_height_source) {
ECL_WARN("%s hgt timeout - reset to %s", failing_height_source, new_height_source);
}
// Reset vertical position and velocity states to the last measurement
if (request_height_reset) {
resetHeight();
@ -901,7 +915,7 @@ void Ekf::controlHeightFusion()
switch (_params.vdist_sensor_type) {
default:
ECL_ERR("Invalid height mode: %d", _params.vdist_sensor_type);
ECL_ERR("Invalid hgt mode: %d", _params.vdist_sensor_type);
// FALLTHROUGH
case VDIST_SENSOR_BARO:

2
EKF/covariance.cpp
View File

@ -822,7 +822,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
_time_acc_bias_check = _time_last_imu;
_fault_status.flags.bad_acc_bias = false;
ECL_WARN_TIMESTAMPED("invalid accel bias - resetting covariance");
ECL_WARN_TIMESTAMPED("invalid accel bias - covariance reset");
} else if (force_symmetry) {
// ensure the covariance values are symmetrical

10
EKF/ekf.h
View File

@ -574,15 +574,15 @@ private:
// reset velocity states of the ekf
bool resetVelocity();
void resetVelocityToGps();
inline void resetVelocityToGps();
void resetHorizontalVelocityToOpticalFlow();
inline void resetHorizontalVelocityToOpticalFlow();
void resetVelocityToVision();
inline void resetVelocityToVision();
void resetHorizontalVelocityToZero();
inline void resetHorizontalVelocityToZero();
void resetVelocityTo(const Vector3f &vel);
inline void resetVelocityTo(const Vector3f &vel);
inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);

2
EKF/gps_checks.cpp
View File

@ -101,7 +101,7 @@ bool Ekf::collect_gps(const gps_message &gps)
if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
startGpsHgtFusion();
}
ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set)");
ECL_INFO_TIMESTAMPED("GPS checks passed");
}
// start collecting GPS if there is a 3D fix and the NED origin has been set

2
EKF/gps_yaw_fusion.cpp
View File

@ -174,7 +174,7 @@ void Ekf::fuseGpsAntYaw()
// we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
ECL_ERR_TIMESTAMPED("GPS yaw fusion numerical error - covariance reset");
ECL_ERR_TIMESTAMPED("GPS yaw numerical error - covariance reset");
return;
}

7
EKF/mag_fusion.cpp
View File

@ -93,6 +93,7 @@ void Ekf::fuseMag()
// X axis innovation variance
_mag_innov_var[0] = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
// check for a badly conditioned covariance matrix
const char* numerical_error_covariance_reset_string = "numerical error - covariance reset";
if (_mag_innov_var[0] >= R_MAG) {
// the innovation variance contribution from the state covariances is non-negative - no fault
@ -104,7 +105,7 @@ void Ekf::fuseMag()
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
ECL_ERR_TIMESTAMPED("magX fusion numerical error - covariance reset");
ECL_ERR("magX %s", numerical_error_covariance_reset_string);
return;
}
@ -123,7 +124,7 @@ void Ekf::fuseMag()
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
ECL_ERR_TIMESTAMPED("magY fusion numerical error - covariance reset");
ECL_ERR("magY %s", numerical_error_covariance_reset_string);
return;
}
@ -142,7 +143,7 @@ void Ekf::fuseMag()
// we need to re-initialise covariances and abort this fusion step
resetMagRelatedCovariances();
ECL_ERR_TIMESTAMPED("magZ fusion numerical error - covariance reset");
ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
return;
}

6
EKF/sideslip_fusion.cpp
View File

@ -129,16 +129,18 @@ void Ekf::fuseSideslip()
_fault_status.flags.bad_sideslip = true;
// if we are getting aiding from other sources, warn and reset the wind states and covariances only
const char* action_string = nullptr;
if (update_wind_only) {
resetWindStates();
resetWindCovariance();
ECL_ERR_TIMESTAMPED("synthetic sideslip fusion badly conditioned - wind covariance reset");
action_string = "wind";
} else {
initialiseCovariance();
_state.wind_vel.setZero();
ECL_ERR_TIMESTAMPED("synthetic sideslip fusion badly conditioned - full covariance reset");
action_string = "full";
}
ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string);
return;
}