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

AP_AHRS: use direct assignation and correct some style

This commit is contained in:
khancyr 2017-12-02 14:02:37 +01:00 committed by Francisco Ferreira
parent dae636b39e
commit 2732226664
3 changed files with 20 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -195,9 +195,9 @@ Vector2f AP_AHRS::groundspeed_vector(void)
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);
Vector2f airspeed_vector = Vector2f(cosf(yaw), sinf(yaw)) * airspeed;
const Vector3f wind = wind_estimate();
const Vector2f wind2d(wind.x, wind.y);
const Vector2f airspeed_vector(cosf(yaw) * airspeed, sinf(yaw) * airspeed);
gndVelADS = airspeed_vector - wind2d;
}
@ -245,16 +245,14 @@ void AP_AHRS::calc_trig(const Matrix3f &rot,
float &cr, float &cp, float &cy,
float &sr, float &sp, float &sy) const
{
Vector2f yaw_vector;
Vector2f yaw_vector(rot.a.x, rot.b.x);
yaw_vector.x = rot.a.x;
yaw_vector.y = rot.b.x;
if (fabsf(yaw_vector.x) > 0 ||
fabsf(yaw_vector.y) > 0) {
yaw_vector.normalize();
}
sy = constrain_float(yaw_vector.y, -1.0, 1.0);
cy = constrain_float(yaw_vector.x, -1.0, 1.0);
sy = constrain_float(yaw_vector.y, -1.0f, 1.0f);
cy = constrain_float(yaw_vector.x, -1.0f, 1.0f);
// sanity checks
if (yaw_vector.is_inf() || yaw_vector.is_nan()) {
@ -270,8 +268,8 @@ void AP_AHRS::calc_trig(const Matrix3f &rot,
cp = safe_sqrt(1 - cx2);
cr = rot.c.z / cp;
}
cp = constrain_float(cp, 0, 1.0);
cr = constrain_float(cr, -1.0, 1.0); // this relies on constrain_float() of infinity doing the right thing
cp = constrain_float(cp, 0.0f, 1.0f);
cr = constrain_float(cr, -1.0f, 1.0f); // this relies on constrain_float() of infinity doing the right thing
sp = -rot.c.x;

24
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -155,9 +155,8 @@ AP_AHRS_DCM::reset(bool recover_eulers)
// roll and pitch estimate
// Get body frame accel vector
Vector3f initAccVec;
Vector3f initAccVec = _ins.get_accel();
uint8_t counter = 0;
initAccVec = _ins.get_accel();
// the first vector may be invalid as the filter starts up
while ((initAccVec.length() < 9.0f || initAccVec.length() > 11) && counter++ < 20) {
@ -226,8 +225,6 @@ AP_AHRS_DCM::check_matrix(void)
bool
AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
{
float renorm_val;
// numerical errors will slowly build up over time in DCM,
// causing inaccuracies. We can keep ahead of those errors
// using the renormalization technique from the DCM IMU paper
@ -246,7 +243,7 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
// we don't want to compound the error by making DCM less
// accurate.
renorm_val = 1.0f / a.length();
const float renorm_val = 1.0f / a.length();
// keep the average for reporting
_renorm_val_sum += renorm_val;
@ -282,14 +279,11 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
void
AP_AHRS_DCM::normalize(void)
{
float error;
Vector3f t0, t1, t2;
const float error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19
t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); // eq.19
t2 = t0 % t1; // c= a x b // eq.20
const Vector3f t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19
const Vector3f t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); // eq.19
const Vector3f t2 = t0 % t1; // c= a x b // eq.20
if (!renorm(t0, _dcm_matrix.a) ||
!renorm(t1, _dcm_matrix.b) ||
@ -589,9 +583,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
each sensor, which prevents an aliasing effect
*/
Vector3f delta_velocity;
float delta_velocity_dt;
_ins.get_delta_velocity(i, delta_velocity);
delta_velocity_dt = _ins.get_delta_velocity_dt(i);
const float delta_velocity_dt = _ins.get_delta_velocity_dt(i);
if (delta_velocity_dt > 0) {
_accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt);
// integrate the accel vector in the earth frame between GPS readings
@ -689,8 +682,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// equation 9: get the corrected acceleration vector in earth frame. Units
// are m/s/s
Vector3f GA_e;
GA_e = Vector3f(0, 0, -1.0f);
Vector3f GA_e(0.0f, 0.0f, -1.0f);
if (_ra_deltat <= 0) {
// waiting for more data

8
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -317,18 +317,18 @@ void AP_AHRS_NavEKF::update_SITL(void)
uint32_t timeStamp_ms = AP_HAL::millis();
if (timeStamp_ms - _last_body_odm_update_ms > 50) {
const float quality = 100.0f;
const Vector3f posOffset = Vector3f(0.0f,0.0f,0.0f);
float delTime = 0.001f*(timeStamp_ms - _last_body_odm_update_ms);
const Vector3f posOffset(0.0f, 0.0f, 0.0f);
const float delTime = 0.001f * (timeStamp_ms - _last_body_odm_update_ms);
_last_body_odm_update_ms = timeStamp_ms;
timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay
Vector3f delAng = Vector3f(radians(fdm.rollRate),
Vector3f delAng(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
delAng *= delTime;
// rotate earth velocity into body frame and calculate delta position
Matrix3f Tbn;
Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));
const Vector3f earth_vel = Vector3f(fdm.speedN,fdm.speedE,fdm.speedD);
const Vector3f earth_vel(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);