Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: correct some constcorrectness

This commit is contained in:
khancyr 2017-12-03 11:10:19 +01:00 committed by Francisco Ferreira
parent d89d496c8f
commit dae636b39e
3 changed files with 50 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -134,7 +134,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
Vector3f AP_AHRS::get_gyro_latest(void) const
{
uint8_t primary_gyro = get_primary_gyro_index();
const uint8_t primary_gyro = get_primary_gyro_index();
return get_ins().get_gyro(primary_gyro) + get_gyro_drift();
}
@ -146,7 +146,7 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret) const
if (_wind_max > 0 && _gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
float gnd_speed = _gps.ground_speed();
const float gnd_speed = _gps.ground_speed();
float true_airspeed = *airspeed_ret * get_EAS2TAS();
true_airspeed = constrain_float(true_airspeed,
gnd_speed - _wind_max,
@ -192,8 +192,8 @@ Vector2f AP_AHRS::groundspeed_vector(void)
Vector2f gndVelADS;
Vector2f gndVelGPS;
float airspeed;
bool gotAirspeed = airspeed_estimate_true(&airspeed);
bool gotGPS = (_gps.status() >= AP_GPS::GPS_OK_FIX_2D);
const bool gotAirspeed = airspeed_estimate_true(&airspeed);
const bool gotGPS = (_gps.status() >= AP_GPS::GPS_OK_FIX_2D);
if (gotAirspeed) {
Vector3f wind = wind_estimate();
Vector2f wind2d = Vector2f(wind.x, wind.y);
@ -203,7 +203,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
// Generate estimate of ground speed vector using GPS
if (gotGPS) {
float cog = radians(_gps.ground_course_cd()*0.01f);
const float cog = radians(_gps.ground_course_cd()*0.01f);
gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * _gps.ground_speed();
}
// If both ADS and GPS data is available, apply a complementary filter
@ -261,8 +261,8 @@ void AP_AHRS::calc_trig(const Matrix3f &rot,
sy = 0.0f;
cy = 1.0f;
}
float cx2 = rot.c.x * rot.c.x;
const float cx2 = rot.c.x * rot.c.x;
if (cx2 >= 1.0f) {
cp = 0;
cr = 1.0f;
@ -343,7 +343,7 @@ AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation)
void AP_AHRS::update_AOA_SSA(void)
{
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
uint32_t now = AP_HAL::millis();
const uint32_t now = AP_HAL::millis();
if (now - _last_AOA_update_ms < 50) {
// don't update at more than 20Hz
return;
@ -366,7 +366,7 @@ void AP_AHRS::update_AOA_SSA(void)
// calculate relative velocity in body coordinates
aoa_velocity = aoa_velocity - aoa_wind;
float vel_len = aoa_velocity.length();
const float vel_len = aoa_velocity.length();
// do not calculate if speed is too low
if (vel_len < 2.0) {

60
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -358,7 +358,7 @@ AP_AHRS_DCM::_P_gain(float spin_rate)
float
AP_AHRS_DCM::_yaw_gain(void) const
{
float VdotEFmag = norm(_accel_ef[_active_accel_instance].x,
const float VdotEFmag = norm(_accel_ef[_active_accel_instance].x,
_accel_ef[_active_accel_instance].y);
if (VdotEFmag <= 4.0f) {
return 0.2f*(4.5f - VdotEFmag);
@ -410,7 +410,7 @@ bool AP_AHRS_DCM::use_compass(void)
// degrees and the estimated wind speed is less than 80% of the
// ground speed, then switch to GPS navigation. This will help
// prevent flyaways with very bad compass offsets
int32_t error = abs(wrap_180_cd(yaw_sensor - _gps.ground_course_cd()));
const int32_t error = abs(wrap_180_cd(yaw_sensor - _gps.ground_course_cd()));
if (error > 4500 && _wind.length() < _gps.ground_speed()*0.8f) {
if (AP_HAL::millis() - _last_consistent_heading > 2000) {
// start using the GPS for heading if the compass has been
@ -446,7 +446,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
// here. This has the effect of throwing away
// the first compass value, which can be bad
if (!_flags.have_initial_yaw && _compass->read()) {
float heading = _compass->calculate_heading(_dcm_matrix);
const float heading = _compass->calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero();
_flags.have_initial_yaw = true;
@ -468,8 +468,8 @@ AP_AHRS_DCM::drift_correction_yaw(void)
yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f;
_gps_last_update = _gps.last_fix_time_ms();
new_value = true;
float gps_course_rad = ToRad(_gps.ground_course_cd() * 0.01f);
float yaw_error_rad = wrap_PI(gps_course_rad - yaw);
const float gps_course_rad = ToRad(_gps.ground_course_cd() * 0.01f);
const float yaw_error_rad = wrap_PI(gps_course_rad - yaw);
yaw_error = sinf(yaw_error_rad);
/* reset yaw to match GPS heading under any of the
@ -509,11 +509,11 @@ AP_AHRS_DCM::drift_correction_yaw(void)
}
// convert the error vector to body frame
float error_z = _dcm_matrix.c.z * yaw_error;
const float error_z = _dcm_matrix.c.z * yaw_error;
// the spin rate changes the P gain, and disables the
// integration at higher rates
float spin_rate = _omega.length();
const float spin_rate = _omega.length();
// sanity check _kp_yaw
if (_kp_yaw < AP_AHRS_YAW_P_MIN) {
@ -551,7 +551,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
Vector3f AP_AHRS_DCM::ra_delayed(uint8_t instance, const Vector3f &ra)
{
// get the old element, and then fill it with the new element
Vector3f ret = _ra_delay_buffer[instance];
const Vector3f ret = _ra_delay_buffer[instance];
_ra_delay_buffer[instance] = ra;
if (ret.is_zero()) {
// use the current vector if the previous vector is exactly
@ -602,7 +602,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
//update _accel_ef_blended
if (_ins.get_accel_count() == 2 && _ins.use_accel(0) && _ins.use_accel(1)) {
float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f;
const float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f;
// slew _imu1_weight over one second
_imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat);
_accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight);
@ -701,8 +701,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS);
if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
float v_scale = gps_gain.get() * ra_scale;
Vector3f vdelta = (velocity - _last_velocity) * v_scale;
const float v_scale = gps_gain.get() * ra_scale;
const Vector3f vdelta = (velocity - _last_velocity) * v_scale;
GA_e += vdelta;
GA_e.normalize();
if (GA_e.is_inf()) {
@ -752,7 +752,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
error[i] = GA_b[i] % GA_e;
// Take dot product to catch case vectors are opposite sign and parallel
error_dirn[i] = GA_b[i] * GA_e;
float error_length = error[i].length();
const float error_length = error[i].length();
if (besti == -1 || error_length < best_error) {
besti = i;
best_error = error_length;
@ -775,16 +775,16 @@ AP_AHRS_DCM::drift_correction(float deltat)
#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
// step 2 calculate earth_error_Z
float earth_error_Z = error.z;
const float earth_error_Z = error.z;
// equation 10
float tilt = norm(GA_e.x, GA_e.y);
const float tilt = norm(GA_e.x, GA_e.y);
// equation 11
float theta = atan2f(GA_b[besti].y, GA_b[besti].x);
const float theta = atan2f(GA_b[besti].y, GA_b[besti].x);
// equation 12
Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);
const Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);
// step 6
error = GA_b[besti] % GA_e2;
@ -823,7 +823,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
_error_rp = 0.8f * _error_rp + 0.2f * best_error;
// base the P gain on the spin rate
float spin_rate = _omega.length();
const float spin_rate = _omega.length();
// sanity check _kp value
if (_kp < AP_AHRS_RP_P_MIN) {
@ -859,7 +859,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// reported maximum gyro drift rate. This ensures that
// short term errors don't cause a buildup of omega_I
// beyond the physical limits of the device
float change_limit = _gyro_drift_limit * _omega_I_sum_time;
const float change_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
_omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
_omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
@ -889,9 +889,9 @@ void AP_AHRS_DCM::estimate_wind(void)
// this is based on the wind speed estimation code from MatrixPilot by
// Bill Premerlani. Adaption for ArduPilot by Jon Challinger
// See http://gentlenav.googlecode.com/files/WindEstimation.pdf
Vector3f fuselageDirection = _dcm_matrix.colx();
Vector3f fuselageDirectionDiff = fuselageDirection - _last_fuse;
uint32_t now = AP_HAL::millis();
const Vector3f fuselageDirection = _dcm_matrix.colx();
const Vector3f fuselageDirectionDiff = fuselageDirection - _last_fuse;
const uint32_t now = AP_HAL::millis();
// scrap our data and start over if we're taking too long to get a direction change
if (now - _last_wind_time > 10000) {
@ -906,20 +906,20 @@ void AP_AHRS_DCM::estimate_wind(void)
// when turning, use the attitude response to estimate
// wind speed
float V;
Vector3f velocityDiff = velocity - _last_vel;
const Vector3f velocityDiff = velocity - _last_vel;
// estimate airspeed it using equation 6
V = velocityDiff.length() / diff_length;
Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse;
Vector3f velocitySum = velocity + _last_vel;
const Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse;
const Vector3f velocitySum = velocity + _last_vel;
_last_fuse = fuselageDirection;
_last_vel = velocity;
float theta = atan2f(velocityDiff.y, velocityDiff.x) - atan2f(fuselageDirectionDiff.y, fuselageDirectionDiff.x);
float sintheta = sinf(theta);
float costheta = cosf(theta);
const float theta = atan2f(velocityDiff.y, velocityDiff.x) - atan2f(fuselageDirectionDiff.y, fuselageDirectionDiff.x);
const float sintheta = sinf(theta);
const float costheta = cosf(theta);
Vector3f wind = Vector3f();
wind.x = velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y);
@ -934,8 +934,8 @@ void AP_AHRS_DCM::estimate_wind(void)
_last_wind_time = now;
} else if (now - _last_wind_time > 2000 && airspeed_sensor_enabled()) {
// when flying straight use airspeed to get wind estimate if available
Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed();
Vector3f wind = velocity - (airspeed * get_EAS2TAS());
const Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed();
const Vector3f wind = velocity - (airspeed * get_EAS2TAS());
_wind = _wind * 0.92f + wind * 0.08f;
}
}
@ -994,7 +994,7 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) const
if (ret && _wind_max > 0 && _gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
float gnd_speed = _gps.ground_speed();
const float gnd_speed = _gps.ground_speed();
float true_airspeed = *airspeed_ret * get_EAS2TAS();
true_airspeed = constrain_float(true_airspeed,
gnd_speed - _wind_max,

22
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -161,7 +161,7 @@ void AP_AHRS_NavEKF::update_EKF2(void)
update_trig();
// Use the primary EKF to select the primary gyro
int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
const int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
// get gyro bias for primary EKF and change sign to give gyro drift
// Note sign convention used by EKF is bias = measurement - truth
@ -231,7 +231,7 @@ void AP_AHRS_NavEKF::update_EKF3(void)
update_trig();
// Use the primary EKF to select the primary gyro
int8_t primary_imu = EKF3.getPrimaryCoreIMUIndex();
const int8_t primary_imu = EKF3.getPrimaryCoreIMUIndex();
// get gyro bias for primary EKF and change sign to give gyro drift
// Note sign convention used by EKF is bias = measurement - truth
@ -303,7 +303,7 @@ void AP_AHRS_NavEKF::update_SITL(void)
radians(fdm.yawRate));
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
Vector3f accel(fdm.xAccel,
const Vector3f accel(fdm.xAccel,
fdm.yAccel,
fdm.zAccel);
_accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
@ -328,8 +328,8 @@ void AP_AHRS_NavEKF::update_SITL(void)
// rotate earth velocity into body frame and calculate delta position
Matrix3f Tbn;
Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));
Vector3f earth_vel = Vector3f(fdm.speedN,fdm.speedE,fdm.speedD);
Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
const Vector3f earth_vel = Vector3f(fdm.speedN,fdm.speedE,fdm.speedD);
const Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
// write to EKF
EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
}
@ -586,8 +586,8 @@ void AP_AHRS_NavEKF::set_home(const Location &loc)
// from which to decide the origin on its own
bool AP_AHRS_NavEKF::set_origin(const Location &loc)
{
bool ret2 = EKF2.setOriginLLH(loc);
bool ret3 = EKF3.setOriginLLH(loc);
const bool ret2 = EKF2.setOriginLLH(loc);
const bool ret3 = EKF3.setOriginLLH(loc);
// return success if active EKF's origin was set
switch (active_EKF_type()) {
@ -778,7 +778,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const
case EKF_TYPE_SITL: {
Location loc;
get_position(loc);
Vector2f diff2d = location_diff(get_home(), loc);
const Vector2f diff2d = location_diff(get_home(), loc);
const struct SITL::sitl_fdm &fdm = _sitl->state;
vec = Vector3f(diff2d.x, diff2d.y,
-(fdm.altitude - get_home().alt*0.01f));
@ -799,7 +799,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_home(Vector3f &vec) const
return false;
}
Vector3f offset = location_3d_diff_NED(originLLH, _home);
const Vector3f offset = location_3d_diff_NED(originLLH, _home);
vec.x = originNED.x - offset.x;
vec.y = originNED.y - offset.y;
@ -848,7 +848,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_home(Vector2f &posNE) const
return false;
}
Vector2f offset = location_diff(originLLH, _home);
const Vector2f offset = location_diff(originLLH, _home);
posNE.x = originNE.x - offset.x;
posNE.y = originNE.y - offset.y;
@ -1223,7 +1223,7 @@ bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
// Retrieves the NED delta velocity corrected
void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
{
EKF_TYPE type = active_EKF_type();
const EKF_TYPE type = active_EKF_type();
if (type == EKF_TYPE2 || type == EKF_TYPE3) {
int8_t imu_idx = 0;
Vector3f accel_bias;