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

Add const modifier

Browse Source
This commit is contained in:
Kamil Ritz
2020-08-15 08:53:21 +02:00
committed by Paul Riseborough
parent aa2c77f92d
commit 1f637af7de
3 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+1 -1
View File
@@ -173,7 +173,7 @@ Vector2f Ekf::getWindVelocityVariance() const
void Ekf::get_true_airspeed(float *tas)
{
float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2)));
const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2)));
memcpy(tas, &tempvar, sizeof(float));
}
EKF/covariance.cpp
+2 -2
View File
@@ -1103,9 +1103,9 @@ void Ekf::resetWindCovariance()
Eulerf euler321(_state.quat_nominal);
const float euler_yaw = euler321(2);
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
const float initial_sideslip_uncertainty = math::radians(15.0f);
constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));
const float R_yaw = sq(math::radians(10.0f));
constexpr float R_yaw = sq(math::radians(10.0f));
// rotate wind velocity into earth frame aligned with vehicle yaw
const float Wx = _state.wind_vel(0) * cosf(euler_yaw) + _state.wind_vel(1) * sinf(euler_yaw);
EKF/mag_fusion.cpp
+2 -2
View File
@@ -812,8 +812,8 @@ void Ekf::fuseHeading()
} else if (_control_status.flags.ev_yaw) {
// calculate the yaw angle for a 312 sequence
// Values from yaw_input_312.c file produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m
float Tbn_0_1_neg = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)-_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2));
float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
const float Tbn_0_1_neg = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)-_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2));
const float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1);
} else {