diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index edf11ed9d6..3441ad2b29 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* APM_AHRS.cpp @@ -10,7 +11,7 @@ but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with this program. If not, see . */ @@ -20,7 +21,7 @@ extern const AP_HAL::HAL& hal; // table of user settable parameters const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { - // index 0 and 1 are for old parameters that are no longer not used + // index 0 and 1 are for old parameters that are no longer not used // @Param: GPS_GAIN // @DisplayName: AHRS GPS gain @@ -112,7 +113,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { // of 1 was found to be the best choice // 13 was the old EKF_USE - + #if AP_AHRS_NAVEKF_AVAILABLE // @Param: EKF_TYPE // @DisplayName: Use NavEKF Kalman filter for attitude and position estimation @@ -128,21 +129,21 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { // return airspeed estimate if available bool AP_AHRS::airspeed_estimate(float *airspeed_ret) const { - if (airspeed_sensor_enabled()) { - *airspeed_ret = _airspeed->get_airspeed(); - 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(); - float true_airspeed = *airspeed_ret * get_EAS2TAS(); - true_airspeed = constrain_float(true_airspeed, - gnd_speed - _wind_max, - gnd_speed + _wind_max); - *airspeed_ret = true_airspeed / get_EAS2TAS(); - } - return true; - } - return false; + if (airspeed_sensor_enabled()) { + *airspeed_ret = _airspeed->get_airspeed(); + 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(); + float true_airspeed = *airspeed_ret * get_EAS2TAS(); + true_airspeed = constrain_float(true_airspeed, + gnd_speed - _wind_max, + gnd_speed + _wind_max); + *airspeed_ret = true_airspeed / get_EAS2TAS(); + } + return true; + } + return false; } // set_trim @@ -182,12 +183,12 @@ Vector2f AP_AHRS::groundspeed_vector(void) bool gotAirspeed = airspeed_estimate_true(&airspeed); 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; - gndVelADS = airspeed_vector - wind2d; + Vector3f wind = wind_estimate(); + Vector2f wind2d = Vector2f(wind.x, wind.y); + Vector2f airspeed_vector = Vector2f(cosf(yaw), sinf(yaw)) * airspeed; + gndVelADS = airspeed_vector - wind2d; } - + // Generate estimate of ground speed vector using GPS if (gotGPS) { float cog = radians(_gps.ground_course_cd()*0.01f); @@ -195,32 +196,32 @@ Vector2f AP_AHRS::groundspeed_vector(void) } // If both ADS and GPS data is available, apply a complementary filter if (gotAirspeed && gotGPS) { - // The LPF is applied to the GPS and the HPF is applied to the air data estimate - // before the two are summed - //Define filter coefficients - // alpha and beta must sum to one - // beta = dt/Tau, where - // dt = filter time step (0.1 sec if called by nav loop) - // Tau = cross-over time constant (nominal 2 seconds) - // More lag on GPS requires Tau to be bigger, less lag allows it to be smaller - // To-Do - set Tau as a function of GPS lag. - const float alpha = 1.0f - beta; - // Run LP filters - _lp = gndVelGPS * beta + _lp * alpha; - // Run HP filters - _hp = (gndVelADS - _lastGndVelADS) + _hp * alpha; - // Save the current ADS ground vector for the next time step - _lastGndVelADS = gndVelADS; - // Sum the HP and LP filter outputs - return _hp + _lp; + // The LPF is applied to the GPS and the HPF is applied to the air data estimate + // before the two are summed + //Define filter coefficients + // alpha and beta must sum to one + // beta = dt/Tau, where + // dt = filter time step (0.1 sec if called by nav loop) + // Tau = cross-over time constant (nominal 2 seconds) + // More lag on GPS requires Tau to be bigger, less lag allows it to be smaller + // To-Do - set Tau as a function of GPS lag. + const float alpha = 1.0f - beta; + // Run LP filters + _lp = gndVelGPS * beta + _lp * alpha; + // Run HP filters + _hp = (gndVelADS - _lastGndVelADS) + _hp * alpha; + // Save the current ADS ground vector for the next time step + _lastGndVelADS = gndVelADS; + // Sum the HP and LP filter outputs + return _hp + _lp; } // Only ADS data is available return ADS estimate if (gotAirspeed && !gotGPS) { - return gndVelADS; + return gndVelADS; } // Only GPS data is available so return GPS estimate if (!gotAirspeed && gotGPS) { - return gndVelGPS; + return gndVelGPS; } return Vector2f(0.0f, 0.0f); } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 708cef2753..be15b0d30b 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -35,9 +35,9 @@ // Copter defaults to EKF on by default, all others off #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) - # define AHRS_EKF_USE_ALWAYS 1 +# define AHRS_EKF_USE_ALWAYS 1 #else - # define AHRS_EKF_USE_ALWAYS 0 +# define AHRS_EKF_USE_ALWAYS 0 #endif #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) @@ -155,7 +155,7 @@ public: const OpticalFlow* get_optflow() const { return _optflow; } - + // allow for runtime change of orientation // this makes initial config easier void set_orientation() { @@ -178,29 +178,39 @@ public: } const AP_InertialSensor &get_ins() const { - return _ins; + return _ins; } const AP_Baro &get_baro() const { - return _baro; + return _baro; } // accelerometer values in the earth frame in m/s/s - virtual const Vector3f &get_accel_ef(uint8_t i) const { return _accel_ef[i]; } - virtual const Vector3f &get_accel_ef(void) const { return get_accel_ef(_ins.get_primary_accel()); } + virtual const Vector3f &get_accel_ef(uint8_t i) const { + return _accel_ef[i]; + } + virtual const Vector3f &get_accel_ef(void) const { + return get_accel_ef(_ins.get_primary_accel()); + } // blended accelerometer values in the earth frame in m/s/s - virtual const Vector3f &get_accel_ef_blended(void) const { return _accel_ef_blended; } + virtual const Vector3f &get_accel_ef_blended(void) const { + return _accel_ef_blended; + } // get yaw rate in earth frame in radians/sec - float get_yaw_rate_earth(void) const { return get_gyro() * get_dcm_matrix().c; } + float get_yaw_rate_earth(void) const { + return get_gyro() * get_dcm_matrix().c; + } // Methods virtual void update(void) = 0; // report any reason for why the backend is refusing to initialise - virtual const char *prearm_failure_reason(void) const { return nullptr; } - + virtual const char *prearm_failure_reason(void) const { + return nullptr; + } + // Euler angles (radians) float roll; float pitch; @@ -279,13 +289,17 @@ public: // return a ground velocity in meters/second, North/East/Down // order. This will only be accurate if have_inertial_nav() is - // true - virtual bool get_velocity_NED(Vector3f &vec) const { return false; } + // true + virtual bool get_velocity_NED(Vector3f &vec) const { + return false; + } // return a position relative to home in meters, North/East/Down // order. This will only be accurate if have_inertial_nav() is - // true - virtual bool get_relative_position_NED(Vector3f &vec) const { return false; } + // true + virtual bool get_relative_position_NED(Vector3f &vec) const { + return false; + } // return ground speed estimate in meters/second. Used by ground vehicles. float groundspeed(void) const { @@ -296,7 +310,9 @@ public: } // return true if we will use compass for yaw - virtual bool use_compass(void) { return _compass && _compass->use_for_yaw(); } + virtual bool use_compass(void) { + return _compass && _compass->use_for_yaw(); + } // return true if yaw has been initialised bool yaw_initialised(void) const { @@ -315,7 +331,9 @@ public: } // get trim - const Vector3f &get_trim() const { return _trim.get(); } + const Vector3f &get_trim() const { + return _trim.get(); + } // set trim virtual void set_trim(Vector3f new_trim); @@ -324,25 +342,43 @@ public: virtual void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true); // helper trig value accessors - float cos_roll() const { return _cos_roll; } - float cos_pitch() const { return _cos_pitch; } - float cos_yaw() const { return _cos_yaw; } - float sin_roll() const { return _sin_roll; } - float sin_pitch() const { return _sin_pitch; } - float sin_yaw() const { return _sin_yaw; } + float cos_roll() const { + return _cos_roll; + } + float cos_pitch() const { + return _cos_pitch; + } + float cos_yaw() const { + return _cos_yaw; + } + float sin_roll() const { + return _sin_roll; + } + float sin_pitch() const { + return _sin_pitch; + } + float sin_yaw() const { + return _sin_yaw; + } // for holding parameters static const struct AP_Param::GroupInfo var_info[]; // return secondary attitude solution if available, as eulers in radians - virtual bool get_secondary_attitude(Vector3f &eulers) { return false; } + virtual bool get_secondary_attitude(Vector3f &eulers) { + return false; + } // return secondary position solution if available - virtual bool get_secondary_position(struct Location &loc) { return false; } + virtual bool get_secondary_position(struct Location &loc) { + return false; + } // get the home location. This is const to prevent any changes to - // home without telling AHRS about the change - const struct Location &get_home(void) const { return _home; } + // home without telling AHRS about the change + const struct Location &get_home(void) const { + return _home; + } // set the home location in 10e7 degrees. This should be called // when the vehicle is at this position. It is assumed that the @@ -351,16 +387,22 @@ public: // return true if the AHRS object supports inertial navigation, // with very accurate position and velocity - virtual bool have_inertial_nav(void) const { return false; } + virtual bool have_inertial_nav(void) const { + return false; + } // return the active accelerometer instance - uint8_t get_active_accel_instance(void) const { return _active_accel_instance; } + uint8_t get_active_accel_instance(void) const { + return _active_accel_instance; + } // is the AHRS subsystem healthy? virtual bool healthy(void) const = 0; // true if the AHRS has completed initialisation - virtual bool initialised(void) const { return true; }; + virtual bool initialised(void) const { + return true; + }; // time that the AHRS has been up virtual uint32_t uptime_ms(void) const = 0; @@ -426,11 +468,11 @@ protected: Vector3f _accel_ef[INS_MAX_INSTANCES]; Vector3f _accel_ef_blended; - // Declare filter states for HPF and LPF used by complementary - // filter in AP_AHRS::groundspeed_vector - Vector2f _lp; // ground vector low-pass filter - Vector2f _hp; // ground vector high-pass filter - Vector2f _lastGndVelADS; // previous HPF input + // Declare filter states for HPF and LPF used by complementary + // filter in AP_AHRS::groundspeed_vector + Vector2f _lp; // ground vector low-pass filter + Vector2f _hp; // ground vector high-pass filter + Vector2f _lastGndVelADS; // previous HPF input // reference position for NED positions struct Location _home; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 06de526af4..412b564f7a 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -17,7 +17,7 @@ but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with this program. If not, see . */ @@ -63,7 +63,7 @@ AP_AHRS_DCM::update(void) delta_t = _ins.get_delta_time(); // if the update call took more than 0.2 seconds then discard it, - // otherwise we may move too far. This happens when arming motors + // otherwise we may move too far. This happens when arming motors // in ArduCopter if (delta_t > 0.2f) { memset(&_ra_sum[0], 0, sizeof(_ra_sum)); @@ -105,7 +105,7 @@ AP_AHRS_DCM::matrix_update(float _G_Dt) // systems with more than one gyro. We don't use the 3rd gyro // unless another is unhealthy as 3rd gyro on PH2 has a lot more // noise - uint8_t healthy_count = 0; + uint8_t healthy_count = 0; Vector3f delta_angle; for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { if (_ins.get_gyro_health(i) && healthy_count < 2) { @@ -156,7 +156,7 @@ AP_AHRS_DCM::reset(bool recover_eulers) // reset the current attitude, used by HIL void AP_AHRS_DCM::reset_attitude(const float &_roll, const float &_pitch, const float &_yaw) { - _dcm_matrix.from_euler(_roll, _pitch, _yaw); + _dcm_matrix.from_euler(_roll, _pitch, _yaw); } /* @@ -175,12 +175,12 @@ AP_AHRS_DCM::check_matrix(void) // feed back into the rest of the DCM matrix via the // error_course value. if (!(_dcm_matrix.c.x < 1.0f && - _dcm_matrix.c.x > -1.0f)) { + _dcm_matrix.c.x > -1.0f)) { // We have an invalid matrix. Force a normalisation. normalize(); if (_dcm_matrix.is_nan() || - fabsf(_dcm_matrix.c.x) > 10) { + fabsf(_dcm_matrix.c.x) > 10) { // normalisation didn't fix the problem! We're // in real trouble. All we can do is reset //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", @@ -261,8 +261,8 @@ AP_AHRS_DCM::normalize(void) t2 = t0 % t1; // c= a x b // eq.20 if (!renorm(t0, _dcm_matrix.a) || - !renorm(t1, _dcm_matrix.b) || - !renorm(t2, _dcm_matrix.c)) { + !renorm(t1, _dcm_matrix.b) || + !renorm(t2, _dcm_matrix.c)) { // Our solution is blowing up and we will force back // to last euler angles _last_failure_ms = hal.scheduler->millis(); @@ -320,9 +320,9 @@ AP_AHRS_DCM::_P_gain(float spin_rate) // _yaw_gain reduces the gain of the PI controller applied to heading errors // when observability from change of velocity is good (eg changing speed or turning) // This reduces unwanted roll and pitch coupling due to compass errors for planes. -// High levels of noise on _accel_ef will cause the gain to drop and could lead to +// High levels of noise on _accel_ef will cause the gain to drop and could lead to // increased heading drift during straight and level flight, however some gain is -// always available. TODO check the necessity of adding adjustable acc threshold +// always available. TODO check the necessity of adding adjustable acc threshold // and/or filtering accelerations before getting magnitude float AP_AHRS_DCM::_yaw_gain(void) const @@ -391,7 +391,7 @@ bool AP_AHRS_DCM::use_compass(void) } // use the compass - return true; + return true; } // yaw drift correction using the compass or GPS @@ -433,7 +433,7 @@ AP_AHRS_DCM::drift_correction_yaw(void) we are using GPS for yaw */ if (_gps.last_fix_time_ms() != _gps_last_update && - _gps.ground_speed() >= GPS_SPEED_MIN) { + _gps.ground_speed() >= GPS_SPEED_MIN) { yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f; _gps_last_update = _gps.last_fix_time_ms(); new_value = true; @@ -457,9 +457,9 @@ AP_AHRS_DCM::drift_correction_yaw(void) operator pulls back the plane rapidly enough then on release the GPS heading changes very rapidly */ - if (!_flags.have_initial_yaw || - yaw_deltat > 20 || - (_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) { + if (!_flags.have_initial_yaw || + yaw_deltat > 20 || + (_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) { // reset DCM matrix based on current yaw _dcm_matrix.from_euler(roll, pitch, gps_course_rad); _omega_yaw_P.zero(); @@ -587,9 +587,9 @@ AP_AHRS_DCM::drift_correction(float deltat) // we have integrated over _ra_deltat += deltat; - if (!have_gps() || - _gps.status() < AP_GPS::GPS_OK_FIX_3D || - _gps.num_sats() < _gps_minsats) { + if (!have_gps() || + _gps.status() < AP_GPS::GPS_OK_FIX_3D || + _gps.num_sats() < _gps_minsats) { // no GPS, or not a good lock. From experience we need at // least 6 satellites to get a really reliable velocity number // from the GPS. @@ -649,7 +649,7 @@ AP_AHRS_DCM::drift_correction(float deltat) } else { // update dead-reckoning position estimate _position_offset_north += velocity.x * _ra_deltat; - _position_offset_east += velocity.y * _ra_deltat; + _position_offset_east += velocity.y * _ra_deltat; } // see if this is our first time through - in which case we @@ -806,14 +806,14 @@ AP_AHRS_DCM::drift_correction(float deltat) _omega_P *= 8; } - if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D && - _gps.ground_speed() < GPS_SPEED_MIN && - _ins.get_accel().x >= 7 && - pitch_sensor > -3000 && pitch_sensor < 3000) { - // assume we are in a launch acceleration, and reduce the - // rp gain by 50% to reduce the impact of GPS lag on - // takeoff attitude when using a catapult - _omega_P *= 0.5f; + if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D && + _gps.ground_speed() < GPS_SPEED_MIN && + _ins.get_accel().x >= 7 && + pitch_sensor > -3000 && pitch_sensor < 3000) { + // assume we are in a launch acceleration, and reduce the + // rp gain by 50% to reduce the impact of GPS lag on + // takeoff attitude when using a catapult + _omega_P *= 0.5f; } // accumulate some integrator error @@ -905,13 +905,13 @@ void AP_AHRS_DCM::estimate_wind(void) Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed(); Vector3f wind = velocity - (airspeed * get_EAS2TAS()); _wind = _wind * 0.92f + wind * 0.08f; - } + } } // calculate the euler angles and DCM matrix which will be used for high level -// navigation control. Apply trim such that a positive trim value results in a +// navigation control. Apply trim such that a positive trim value results in a // positive vehicle rotation about that axis (ie a negative offset) void AP_AHRS_DCM::euler_angles(void) @@ -942,33 +942,33 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const // return an airspeed estimate if available bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) const { - bool ret = false; - if (airspeed_sensor_enabled()) { - *airspeed_ret = _airspeed->get_airspeed(); - return true; - } + bool ret = false; + if (airspeed_sensor_enabled()) { + *airspeed_ret = _airspeed->get_airspeed(); + return true; + } if (!_flags.wind_estimation) { return false; } - // estimate it via GPS speed and wind - if (have_gps()) { - *airspeed_ret = _last_airspeed; - ret = true; - } + // estimate it via GPS speed and wind + if (have_gps()) { + *airspeed_ret = _last_airspeed; + ret = true; + } - if (ret && _wind_max > 0 && _gps.status() >= AP_GPS::GPS_OK_FIX_2D) { - // constrain the airspeed by the ground speed - // and AHRS_WIND_MAX + 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(); float true_airspeed = *airspeed_ret * get_EAS2TAS(); - true_airspeed = constrain_float(true_airspeed, - gnd_speed - _wind_max, + true_airspeed = constrain_float(true_airspeed, + gnd_speed - _wind_max, gnd_speed + _wind_max); *airspeed_ret = true_airspeed / get_EAS2TAS(); - } - return ret; + } + return ret; } void AP_AHRS_DCM::set_home(const Location &loc) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index e048dc25ee..bef8bdf2f7 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #ifndef __AP_AHRS_DCM_H__ #define __AP_AHRS_DCM_H__ /* @@ -26,7 +27,7 @@ class AP_AHRS_DCM : public AP_AHRS public: // Constructors AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) : - AP_AHRS(ins, baro, gps), + AP_AHRS(ins, baro, gps), _omega_I_sum_time(0.0f), _renorm_val_sum(0.0f), _renorm_val_count(0), @@ -90,8 +91,12 @@ public: virtual bool get_position(struct Location &loc) const; // status reporting - float get_error_rp(void) const { return _error_rp; } - float get_error_yaw(void) const { return _error_yaw; } + float get_error_rp(void) const { + return _error_rp; + } + float get_error_yaw(void) const { + return _error_yaw; + } // return a wind estimation vector, in m/s Vector3f wind_estimate(void) { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 746ea6728c..c461c28a30 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -117,7 +118,7 @@ void AP_AHRS_NavEKF::update_EKF1(void) // calculate corrected gryo estimate for get_gyro() _gyro_estimate.zero(); - uint8_t healthy_count = 0; + uint8_t healthy_count = 0; for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { if (_ins.get_gyro_health(i) && healthy_count < 2) { _gyro_estimate += _ins.get_gyro(i); @@ -187,7 +188,7 @@ void AP_AHRS_NavEKF::update_EKF2(void) // calculate corrected gryo estimate for get_gyro() _gyro_estimate.zero(); - uint8_t healthy_count = 0; + uint8_t healthy_count = 0; for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { if (_ins.get_gyro_health(i) && healthy_count < 2) { _gyro_estimate += _ins.get_gyro(i); @@ -248,10 +249,10 @@ void AP_AHRS_NavEKF::reset(bool recover_eulers) { AP_AHRS_DCM::reset(recover_eulers); if (ekf1_started) { - ekf1_started = EKF1.InitialiseFilterBootstrap(); + ekf1_started = EKF1.InitialiseFilterBootstrap(); } if (ekf2_started) { - ekf2_started = EKF2.InitialiseFilter(); + ekf2_started = EKF2.InitialiseFilter(); } } @@ -318,7 +319,7 @@ Vector3f AP_AHRS_NavEKF::wind_estimate(void) case EKF_TYPE1: EKF1.getWind(wind); break; - + case EKF_TYPE2: EKF2.getWind(wind); break; @@ -344,7 +345,7 @@ bool AP_AHRS_NavEKF::use_compass(void) case EKF_TYPE2: return EKF2.use_compass(); } - return AP_AHRS_DCM::use_compass(); + return AP_AHRS_DCM::use_compass(); } @@ -381,7 +382,7 @@ bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) // return DCM position AP_AHRS_DCM::get_position(loc); return true; - } + } } // EKF has a better ground speed vector estimate @@ -410,7 +411,7 @@ void AP_AHRS_NavEKF::set_home(const Location &loc) } // return true if inertial navigation is active -bool AP_AHRS_NavEKF::have_inertial_nav(void) const +bool AP_AHRS_NavEKF::have_inertial_nav(void) const { return using_EKF() != EKF_TYPE_NONE; } @@ -492,7 +493,7 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::using_EKF(void) const } #else if (EKF1.healthy()) { - ret = EKF_TYPE1; + ret = EKF_TYPE1; } #endif break; @@ -519,8 +520,8 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::using_EKF(void) const } if (ret != EKF_TYPE_NONE && - (_vehicle_class == AHRS_VEHICLE_FIXED_WING || - _vehicle_class == AHRS_VEHICLE_GROUND)) { + (_vehicle_class == AHRS_VEHICLE_FIXED_WING || + _vehicle_class == AHRS_VEHICLE_GROUND)) { nav_filter_status filt_state; if (ret == EKF_TYPE1) { EKF1.getFilterStatus(filt_state); @@ -538,10 +539,10 @@ AP_AHRS_NavEKF::EKF_TYPE AP_AHRS_NavEKF::using_EKF(void) const return EKF_TYPE_NONE; } if (!filt_state.flags.attitude || - !filt_state.flags.horiz_vel || - !filt_state.flags.vert_vel || - !filt_state.flags.horiz_pos_abs || - !filt_state.flags.vert_pos) { + !filt_state.flags.horiz_vel || + !filt_state.flags.vert_vel || + !filt_state.flags.horiz_pos_abs || + !filt_state.flags.vert_pos) { return EKF_TYPE_NONE; } } @@ -566,8 +567,8 @@ bool AP_AHRS_NavEKF::healthy(void) const return false; } if ((_vehicle_class == AHRS_VEHICLE_FIXED_WING || - _vehicle_class == AHRS_VEHICLE_GROUND) && - using_EKF() != EKF_TYPE1) { + _vehicle_class == AHRS_VEHICLE_GROUND) && + using_EKF() != EKF_TYPE1) { // on fixed wing we want to be using EKF to be considered // healthy if EKF is enabled return false; @@ -581,8 +582,8 @@ bool AP_AHRS_NavEKF::healthy(void) const return false; } if ((_vehicle_class == AHRS_VEHICLE_FIXED_WING || - _vehicle_class == AHRS_VEHICLE_GROUND) && - using_EKF() != EKF_TYPE2) { + _vehicle_class == AHRS_VEHICLE_GROUND) && + using_EKF() != EKF_TYPE2) { // on fixed wing we want to be using EKF to be considered // healthy if EKF is enabled return false; @@ -591,7 +592,7 @@ bool AP_AHRS_NavEKF::healthy(void) const } } - return AP_AHRS_DCM::healthy(); + return AP_AHRS_DCM::healthy(); } void AP_AHRS_NavEKF::set_ekf_use(bool setting) @@ -614,7 +615,7 @@ bool AP_AHRS_NavEKF::initialised(void) const case 2: // initialisation complete 10sec after ekf has started return (ekf2_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); - } + } }; // write optical flow data to EKF diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 9d4a2775c6..9fda7caf02 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #ifndef __AP_AHRS_NAVEKF_H__ #define __AP_AHRS_NAVEKF_H__ /* @@ -35,13 +36,13 @@ class AP_AHRS_NavEKF : public AP_AHRS_DCM { public: // Constructor -AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng, - NavEKF &_EKF1, NavEKF2 &_EKF2) : - AP_AHRS_DCM(ins, baro, gps), - EKF1(_EKF1), - EKF2(_EKF2) - { - } + AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder &rng, + NavEKF &_EKF1, NavEKF2 &_EKF2) : + AP_AHRS_DCM(ins, baro, gps), + EKF1(_EKF1), + EKF2(_EKF2) + { + } // return the smoothed gyro vector corrected for drift const Vector3f &get_gyro(void) const; @@ -78,12 +79,20 @@ AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder & bool use_compass(void); // we will need to remove these to fully hide which EKF we are using - NavEKF &get_NavEKF(void) { return EKF1; } - const NavEKF &get_NavEKF_const(void) const { return EKF1; } + NavEKF &get_NavEKF(void) { + return EKF1; + } + const NavEKF &get_NavEKF_const(void) const { + return EKF1; + } + + NavEKF2 &get_NavEKF2(void) { + return EKF2; + } + const NavEKF2 &get_NavEKF2_const(void) const { + return EKF2; + } - NavEKF2 &get_NavEKF2(void) { return EKF2; } - const NavEKF2 &get_NavEKF2_const(void) const { return EKF2; } - // return secondary attitude solution if available, as eulers in radians bool get_secondary_attitude(Vector3f &eulers); @@ -94,7 +103,9 @@ AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, RangeFinder & Vector2f groundspeed_vector(void); const Vector3f &get_accel_ef(uint8_t i) const; - const Vector3f &get_accel_ef() const { return get_accel_ef(_ins.get_primary_accel()); }; + const Vector3f &get_accel_ef() const { + return get_accel_ef(_ins.get_primary_accel()); + }; // blended accelerometer values in the earth frame in m/s/s const Vector3f &get_accel_ef_blended(void) const;