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;