zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Revert if-else condition to get rid of unnecessary indentation

master
kamilritz 5 years ago committed by Mathieu Bresciani
parent
fce4958d4f
commit
2af2696b63
2 changed files with 120 additions and 117 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 56
      EKF/ekf.cpp
  2. 181
      EKF/ekf_helper.cpp

56
EKF/ekf.cpp
Unescape View File

@ -188,43 +188,41 @@ bool Ekf::initialiseFilter() @@ -188,43 +188,41 @@ bool Ekf::initialiseFilter()
if (not_enough_baro_samples_accumulated || not_enough_mag_samples_accumulated) {
return false;
}
// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
setControlBaroHeight();
} else {
// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
setControlBaroHeight();
if(!initialiseTilt()){
return false;
}
if(!initialiseTilt()){
return false;
}
// calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false);
// calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false);
// initialise the state covariance matrix now we have starting values for all the states
initialiseCovariance();
// initialise the state covariance matrix now we have starting values for all the states
initialiseCovariance();
// update the yaw angle variance using the variance of the measurement
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
}
// update the yaw angle variance using the variance of the measurement
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
}
// try to initialise the terrain estimator
_terrain_initialised = initHagl();
// try to initialise the terrain estimator
_terrain_initialised = initHagl();
// reset the essential fusion timeout counters
_time_last_hgt_fuse = _time_last_imu;
_time_last_hor_pos_fuse = _time_last_imu;
_time_last_delpos_fuse = _time_last_imu;
_time_last_hor_vel_fuse = _time_last_imu;
_time_last_hagl_fuse = _time_last_imu;
_time_last_of_fuse = _time_last_imu;
// reset the essential fusion timeout counters
_time_last_hgt_fuse = _time_last_imu;
_time_last_hor_pos_fuse = _time_last_imu;
_time_last_delpos_fuse = _time_last_imu;
_time_last_hor_vel_fuse = _time_last_imu;
_time_last_hagl_fuse = _time_last_imu;
_time_last_of_fuse = _time_last_imu;
// reset the output predictor state history to match the EKF initial values
alignOutputFilter();
// reset the output predictor state history to match the EKF initial values
alignOutputFilter();
return true;
}
return true;
}
bool Ekf::initialiseTilt()

181
EKF/ekf_helper.cpp
Unescape View File

@ -369,99 +369,99 @@ bool Ekf::realignYawGPS() @@ -369,99 +369,99 @@ bool Ekf::realignYawGPS()
// Need at least 5 m/s of GPS horizontal speed and
// ratio of velocity error to velocity < 0.15 for a reliable alignment
if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) {
// check for excessive horizontal GPS velocity innovations
const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps;
const bool gps_yaw_alignment_possible = (gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed));
// calculate GPS course over ground angle
const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
if (!gps_yaw_alignment_possible) {
// attempt a normal alignment using the magnetometer
return resetMagHeading(_mag_lpf.getState());
}
// calculate course yaw angle
const float ekfCOG = atan2f(_state.vel(1), _state.vel(0));
// check for excessive horizontal GPS velocity innovations
const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps;
// Check the EKF and GPS course over ground for consistency
const float courseYawError = wrap_pi(gpsCOG - ekfCOG);
// calculate GPS course over ground angle
const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
// If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
const bool badYawErr = fabsf(courseYawError) > 0.5f;
const bool badMagYaw = (badYawErr && badVelInnov);
// calculate course yaw angle
const float ekfCOG = atan2f(_state.vel(1), _state.vel(0));
if (badMagYaw) {
_num_bad_flight_yaw_events ++;
}
// Check the EKF and GPS course over ground for consistency
const float courseYawError = wrap_pi(gpsCOG - ekfCOG);
// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
if (badMagYaw || !_control_status.flags.yaw_align) {
ECL_WARN_TIMESTAMPED("bad yaw, using GPS course");
// If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
const bool badYawErr = fabsf(courseYawError) > 0.5f;
const bool badMagYaw = (badYawErr && badVelInnov);
// declare the magnetometer as failed if a bad yaw has occurred more than once
if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
ECL_WARN_TIMESTAMPED("stopping mag use");
_control_status.flags.mag_fault = true;
}
if (badMagYaw) {
_num_bad_flight_yaw_events ++;
}
// calculate new yaw estimate
float yaw_new;
if (!_control_status.flags.mag_aligned_in_flight) {
// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
Eulerf euler321(_state.quat_nominal);
yaw_new = euler321(2) + courseYawError;
_control_status.flags.mag_aligned_in_flight = true;
} else if (_control_status.flags.wind) {
// we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
// aligned with the wind relative GPS velocity vector
yaw_new = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)),
(_gps_sample_delayed.vel(0) - _state.wind_vel(0)));
// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
if (badMagYaw || !_control_status.flags.yaw_align) {
ECL_WARN_TIMESTAMPED("bad yaw, using GPS course");
} else {
// we don't have wind estimates, so align yaw to the GPS velocity vector
yaw_new = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
// declare the magnetometer as failed if a bad yaw has occurred more than once
if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
ECL_WARN_TIMESTAMPED("stopping mag use");
_control_status.flags.mag_fault = true;
}
}
// calculate new yaw estimate
float yaw_new;
if (!_control_status.flags.mag_aligned_in_flight) {
// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
Eulerf euler321(_state.quat_nominal);
yaw_new = euler321(2) + courseYawError;
_control_status.flags.mag_aligned_in_flight = true;
// use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment
const float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P(4,4) + P(5,5);
const float sineYawError = math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0f, 1.0f);
const float yaw_variance_new = sq(asinf(sineYawError));
} else if (_control_status.flags.wind) {
// we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
// aligned with the wind relative GPS velocity vector
yaw_new = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)),
(_gps_sample_delayed.vel(0) - _state.wind_vel(0)));
// Apply updated yaw and yaw variance to states and covariances
resetQuatStateYaw(yaw_new, yaw_variance_new, true);
} else {
// we don't have wind estimates, so align yaw to the GPS velocity vector
yaw_new = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
// Use the last magnetometer measurements to reset the field states
_state.mag_B.zero();
_R_to_earth = Dcmf(_state.quat_nominal);
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
}
resetMagCov();
// use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment
const float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P(4,4) + P(5,5);
const float sineYawError = math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0f, 1.0f);
const float yaw_variance_new = sq(asinf(sineYawError));
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// Apply updated yaw and yaw variance to states and covariances
resetQuatStateYaw(yaw_new, yaw_variance_new, true);
// If heading was bad, then we also need to reset the velocity and position states
_velpos_reset_request = badMagYaw;
// Use the last magnetometer measurements to reset the field states
_state.mag_B.zero();
_R_to_earth = Dcmf(_state.quat_nominal);
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
return true;
resetMagCov();
} else {
// align mag states only
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
// calculate initial earth magnetic field states
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
// If heading was bad, then we also need to reset the velocity and position states
_velpos_reset_request = badMagYaw;
resetMagCov();
return true;
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
} else {
// align mag states only
return true;
}
// calculate initial earth magnetic field states
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
} else {
// attempt a normal alignment using the magnetometer
return resetMagHeading(_mag_lpf.getState());
resetMagCov();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
return true;
}
}
@ -1687,32 +1687,37 @@ bool Ekf::resetYawToEKFGSF() @@ -1687,32 +1687,37 @@ bool Ekf::resetYawToEKFGSF()
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
// data and the yaw estimate has converged
float new_yaw, new_yaw_variance;
if (yawEstimator.getYawData(&new_yaw, &new_yaw_variance) && new_yaw_variance < sq(_params.EKFGSF_yaw_err_max)) {
resetQuatStateYaw(new_yaw, new_yaw_variance, true);
if (!yawEstimator.getYawData(&new_yaw, &new_yaw_variance)) {
return false;
}
// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
resetVelocity();
resetHorizontalPosition();
const bool has_converged = new_yaw_variance < sq(_params.EKFGSF_yaw_err_max);
// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
_control_status.flags.yaw_align = true;
if (!has_converged) {
return false;
}
if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
ECL_INFO_TIMESTAMPED("Yaw aligned using IMU and GPS");
} else {
// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
// and cause another navigation failure
_control_status.flags.mag_fault = true;
ECL_INFO_TIMESTAMPED("Emergency yaw reset - mag use stopped");
}
resetQuatStateYaw(new_yaw, new_yaw_variance, true);
return true;
}
// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
resetVelocity();
resetHorizontalPosition();
return false;
// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
_control_status.flags.yaw_align = true;
if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
ECL_INFO_TIMESTAMPED("Yaw aligned using IMU and GPS");
} else {
// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
// and cause another navigation failure
_control_status.flags.mag_fault = true;
ECL_INFO_TIMESTAMPED("Emergency yaw reset - mag use stopped");
}
return true;
}
void Ekf::requestEmergencyNavReset()

Write Preview
Loading…
Cancel
Save