diff --git a/EKF/control.cpp b/EKF/control.cpp index 970ba2bce9..40c142456f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -58,7 +58,7 @@ void Ekf::controlFusionModes() // Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states // and declare the tilt alignment complete - if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) { + if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) { _control_status.flags.tilt_align = true; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); @@ -346,11 +346,13 @@ void Ekf::controlOpticalFlowFusion() // Check if on ground motion is un-suitable for use of optical flow if (!_control_status.flags.in_air) { // When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation - float accel_norm = _accel_vec_filt.norm(); - bool motion_is_excessive = ((accel_norm > 14.7f) // accel greater than 1.5g - || (accel_norm < 4.9f) // accel less than 0.5g + const float accel_norm = _accel_vec_filt.norm(); + + const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit + || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit || (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit - || (_R_to_earth(2,2) < 0.866f)); // tilted more than 30 degrees + || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively + if (motion_is_excessive) { _time_bad_motion_us = _imu_sample_delayed.time_us; @@ -516,8 +518,7 @@ void Ekf::controlGpsFusion() if (_mag_inhibit_yaw_reset_req) { _mag_inhibit_yaw_reset_req = false; // Zero the yaw bias covariance and set the variance to the initial alignment uncertainty - float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; - setDiag(P, 12, 12, sq(_params.switch_on_gyro_bias * dt)); + setDiag(P, 12, 12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S)); } } @@ -1307,7 +1308,7 @@ void Ekf::controlMagFusion() } else if (_mag_bias_observable) { // monitor yaw rotation in 45 deg sections. // a rotation of 45 deg is sufficient to make the mag bias observable - if (fabsf(_yaw_delta_ef) > 0.7854f) { + if (fabsf(_yaw_delta_ef) > math::radians(45.0f)) { _time_yaw_started = _imu_sample_delayed.time_us; _yaw_delta_ef = 0.0f; } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 215440e934..6e6b0497d8 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -55,7 +55,7 @@ void Ekf::initialiseCovariance() } // calculate average prediction time step in sec - float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; + float dt = FILTER_UPDATE_PERIOD_S; // define the initial angle uncertainty as variances for a rotation vector Vector3f rot_vec_var; @@ -150,7 +150,7 @@ void Ekf::predictCovariance() float dvy_b = _state.accel_bias(1); float dvz_b = _state.accel_bias(2); - float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.0005f * FILTER_UPDATE_PERIOD_MS, 0.002f * FILTER_UPDATE_PERIOD_MS); + float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S); float dt_inv = 1.0f / dt; // compute noise variance for stationary processes @@ -907,7 +907,7 @@ void Ekf::resetWindCovariance() // calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle // used to calculate the initial wind speed float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); - float R_yaw = sq(0.1745f); + float R_yaw = sq(math::radians(10.0f)); // calculate the variance and covariance terms for the wind states float cos_yaw = cosf(yaw); diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index a65256c6f9..06fb8113a1 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -85,7 +85,7 @@ bool Ekf::init(uint64_t timestamp) _control_status.value = 0; _control_status_prev.value = 0; - _dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS); + _dt_ekf_avg = FILTER_UPDATE_PERIOD_S; _fault_status.value = 0; _innov_check_fail_status.value = 0; @@ -307,7 +307,7 @@ void Ekf::predictState() { if (!_earth_rate_initialised) { if (_NED_origin_initialised) { - calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad); + calcEarthRateNED(_earth_rate_NED, (float)_pos_ref.lat_rad); _earth_rate_initialised = true; } } @@ -359,7 +359,7 @@ void Ekf::predictState() float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt); // filter and limit input between -50% and +100% of nominal value - input = math::constrain(input, 0.0005f * (float)(FILTER_UPDATE_PERIOD_MS), 0.002f * (float)(FILTER_UPDATE_PERIOD_MS)); + input = math::constrain(input, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S); _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input; } @@ -391,7 +391,7 @@ bool Ekf::collect_imu(const imuSample &imu) // if the target time delta between filter prediction steps has been exceeded // write the accumulated IMU data to the ring buffer - const float target_dt = FILTER_UPDATE_PERIOD_MS / 1000.0f; + const float target_dt = FILTER_UPDATE_PERIOD_S; if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) { diff --git a/EKF/ekf.h b/EKF/ekf.h index 942a46e9c9..89f4873147 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -237,7 +237,6 @@ public: private: static constexpr uint8_t _k_num_states{24}; ///< number of EKF states - static constexpr float _k_earth_rate{0.000072921f}; ///< earth spin rate (rad/sec) struct { uint8_t velNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) @@ -252,7 +251,7 @@ private: Quatf quat_change; ///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion } _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset information - float _dt_ekf_avg{0.001f * FILTER_UPDATE_PERIOD_MS}; ///< average update rate of the ekf + float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf float _dt_update{0.01f}; ///< delta time since last ekf update. This time can be used for filters which run at the same rate as the Ekf::update() function. (sec) stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon @@ -548,7 +547,7 @@ private: void fuse(float *K, float innovation); // calculate the earth rotation vector from a given latitude - void calcEarthRateNED(Vector3f &omega, double lat_rad) const; + void calcEarthRateNED(Vector3f &omega, float lat_rad) const; // return true id the GPS quality is good enough to set an origin and start aiding bool gps_is_good(struct gps_message *gps); @@ -602,10 +601,7 @@ private: void checkForStuckRange(); // return the square of two floating point numbers - used in auto coded sections - inline float sq(float var) - { - return var * var; - } + static constexpr float sq(float var) { return var * var; } // set control flags to use baro height void setControlBaroHeight(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 276af0b7f0..52e962115c 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -793,7 +793,7 @@ void Ekf::constrainStates() } for (int i = 0; i < 3; i++) { - _state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_ekf_avg, 0.349066f * _dt_ekf_avg); + _state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -math::radians(20.f) * _dt_ekf_avg, math::radians(20.f) * _dt_ekf_avg); } for (int i = 0; i < 3; i++) { @@ -814,11 +814,11 @@ void Ekf::constrainStates() } // calculate the earth rotation vector -void Ekf::calcEarthRateNED(Vector3f &omega, double lat_rad) const +void Ekf::calcEarthRateNED(Vector3f &omega, float lat_rad) const { - omega(0) = _k_earth_rate * cosf((float)lat_rad); + omega(0) = CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad); omega(1) = 0.0f; - omega(2) = -_k_earth_rate * sinf((float)lat_rad); + omega(2) = -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad); } // gets the innovations of velocity and position measurements @@ -1116,7 +1116,7 @@ bool Ekf::reset_imu_bias() zeroRows(P, 10, 15); // Set the corresponding variances to the values use for initial alignment - float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; + float dt = FILTER_UPDATE_PERIOD_S; P[12][12] = P[11][11] = P[10][10] = sq(_params.switch_on_gyro_bias * dt); P[15][15] = P[14][14] = P[13][13] = sq(_params.switch_on_accel_bias * dt); _last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 207b360fd9..1e49cef6fe 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -381,7 +381,8 @@ public: void print_status(); - static const unsigned FILTER_UPDATE_PERIOD_MS = 8; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta + static constexpr unsigned FILTER_UPDATE_PERIOD_MS{8}; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta + static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f}; protected: @@ -391,7 +392,7 @@ protected: OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer which sets the maximum frequency at which we can process non-IMU measurements. Measurements that arrive too soon after the previous measurement will not be processed. - max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001) + max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_S) This can be adjusted to match the max sensor data rate plus some margin for jitter. */ uint8_t _obs_buffer_length{0}; diff --git a/geo/geo.h b/geo/geo.h index bddad85ef2..3859d323a8 100644 --- a/geo/geo.h +++ b/geo/geo.h @@ -61,6 +61,8 @@ static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS = -273.15f; // °C static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000; // meters (m) static constexpr float CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH; // meters (m) +static constexpr float CONSTANTS_EARTH_SPIN_RATE = 7.2921150e-5f; // radians/second (rad/s) + // XXX remove struct crosstrack_error_s {