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

AP_AHRS: run astyle for formatting

This commit is contained in:
Andrew Tridgell 2015-09-23 17:29:43 +10:00
parent cbc62238a9
commit 0677c2c80c
6 changed files with 221 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
@ -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);
}

112
libraries/AP_AHRS/AP_AHRS.h
View File

@ -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;

92
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
@ -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)

11
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -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) {

43
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -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

37
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -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;