ardupilot/libraries/AP_AHRS/AP_AHRS_Backend.h

653 lines
23 KiB
C
Raw Normal View History

#pragma once
2012-12-12 17:42:14 -04:00
/*
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
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
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/>.
*/
2012-03-19 03:34:12 -03:00
/*
2012-08-21 23:19:51 -03:00
* AHRS (Attitude Heading Reference System) interface for ArduPilot
*
*/
2012-03-19 03:34:12 -03:00
#include <AP_Math/AP_Math.h>
2012-03-19 03:34:12 -03:00
#include <inttypes.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_Param/AP_Param.h>
#include <AP_Common/Location.h>
2012-03-19 03:34:12 -03:00
class OpticalFlow;
2013-02-19 05:50:57 -04:00
#define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees
#define AP_AHRS_RP_P_MIN 0.05f // minimum value for AHRS_RP_P parameter
#define AP_AHRS_YAW_P_MIN 0.05f // minimum value for AHRS_YAW_P parameter
2013-02-19 05:50:57 -04:00
enum AHRS_VehicleClass : uint8_t {
AHRS_VEHICLE_UNKNOWN,
AHRS_VEHICLE_GROUND,
AHRS_VEHICLE_COPTER,
AHRS_VEHICLE_FIXED_WING,
2017-02-18 12:07:19 -04:00
AHRS_VEHICLE_SUBMARINE,
};
2021-07-20 10:04:20 -03:00
class AP_AHRS_Backend
2012-03-19 03:34:12 -03:00
{
public:
2019-02-19 23:53:28 -04:00
2012-08-21 23:19:51 -03:00
// Constructor
AP_AHRS_Backend() {
// enable centrifugal correction by default
_flags.correct_centrifugal = true;
2016-08-09 15:47:59 -03:00
_last_trim = _trim.get();
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
2012-08-21 23:19:51 -03:00
}
2014-07-15 01:29:34 -03:00
// empty virtual destructor
2021-07-20 10:04:20 -03:00
virtual ~AP_AHRS_Backend() {}
2018-01-01 08:03:28 -04:00
// init sets up INS board orientation
2019-03-04 14:16:06 -04:00
virtual void init();
2012-08-21 23:19:51 -03:00
// Accessors
2012-11-14 12:10:15 -04:00
void set_fly_forward(bool b) {
_flags.fly_forward = b;
2012-08-21 23:19:51 -03:00
}
bool get_fly_forward(void) const {
return _flags.fly_forward;
}
void set_takeoff_expected(bool b);
bool get_takeoff_expected(void) const {
return _flags.takeoff_expected;
}
void set_touchdown_expected(bool b);
bool get_touchdown_expected(void) const {
return _flags.touchdown_expected;
}
AHRS_VehicleClass get_vehicle_class(void) const {
return _vehicle_class;
}
void set_vehicle_class(AHRS_VehicleClass vclass) {
_vehicle_class = vclass;
}
void set_wind_estimation(bool b) {
_flags.wind_estimation = b;
}
2020-04-21 01:39:45 -03:00
// return the index of the primary core or -1 if no primary core selected
virtual int8_t get_primary_core_index() const { return -1; }
// get the index of the current primary accelerometer sensor
virtual uint8_t get_primary_accel_index(void) const {
2018-03-10 05:35:03 -04:00
return AP::ins().get_primary_accel();
}
// get the index of the current primary gyro sensor
virtual uint8_t get_primary_gyro_index(void) const {
2018-03-10 05:35:03 -04:00
return AP::ins().get_primary_gyro();
}
2019-02-19 23:53:28 -04:00
// accelerometer values in the earth frame in m/s/s
2015-09-23 04:29:43 -03:00
virtual const Vector3f &get_accel_ef(uint8_t i) const {
return _accel_ef[i];
}
virtual const Vector3f &get_accel_ef(void) const {
2018-03-10 05:35:03 -04:00
return get_accel_ef(AP::ins().get_primary_accel());
2015-09-23 04:29:43 -03:00
}
// blended accelerometer values in the earth frame in m/s/s
2015-09-23 04:29:43 -03:00
virtual const Vector3f &get_accel_ef_blended(void) const {
return _accel_ef_blended;
}
// get yaw rate in earth frame in radians/sec
2015-09-23 04:29:43 -03:00
float get_yaw_rate_earth(void) const {
return get_gyro() * get_rotation_body_to_ned().c;
2015-09-23 04:29:43 -03:00
}
2012-08-21 23:19:51 -03:00
// Methods
virtual void update(bool skip_ins_update=false) = 0;
2012-08-21 23:19:51 -03:00
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
// requires_position should be true if horizontal position configuration should be checked
virtual bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const = 0;
2015-09-23 04:29:43 -03:00
// check all cores providing consistent attitudes for prearm checks
virtual bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const { return true; }
2019-06-07 20:33:45 -03:00
// see if EKF lane switching is possible to avoid EKF failsafe
virtual void check_lane_switch(void) {}
// check whether external navigation is providing yaw. Allows compass pre-arm checks to be bypassed
virtual bool is_ext_nav_used_for_yaw(void) const { return false; }
// request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
virtual void request_yaw_reset(void) {}
2020-06-17 03:35:07 -03:00
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
virtual void set_posvelyaw_source_set(uint8_t source_set_idx) {}
2012-08-21 23:19:51 -03:00
// Euler angles (radians)
float roll;
float pitch;
float yaw;
float get_roll() const { return roll; }
float get_pitch() const { return pitch; }
float get_yaw() const { return yaw; }
2012-08-21 23:19:51 -03:00
// integer Euler angles (Degrees * 100)
int32_t roll_sensor;
int32_t pitch_sensor;
int32_t yaw_sensor;
// return a smoothed and corrected gyro vector in radians/second
virtual const Vector3f &get_gyro(void) const = 0;
2012-08-21 23:19:51 -03:00
// return primary accels, for lua
const Vector3f &get_accel(void) const {
return AP::ins().get_accel();
}
// return a smoothed and corrected gyro vector in radians/second using the latest ins data (which may not have been consumed by the EKF yet)
Vector3f get_gyro_latest(void) const;
2012-08-21 23:19:51 -03:00
// return the current estimate of the gyro drift
virtual const Vector3f &get_gyro_drift(void) const = 0;
2012-08-21 23:19:51 -03:00
2014-10-28 08:22:48 -03:00
// reset the current gyro drift estimate
// should be called if gyro offsets are recalculated
virtual void reset_gyro_drift(void) = 0;
2012-08-21 23:19:51 -03:00
// reset the current attitude, used on new IMU calibration
virtual void reset(bool recover_eulers=false) = 0;
// return the average size of the roll/pitch error estimate
// since last call
virtual float get_error_rp(void) const = 0;
2012-08-21 23:19:51 -03:00
// return the average size of the yaw error estimate
// since last call
virtual float get_error_yaw(void) const = 0;
2012-08-21 23:19:51 -03:00
// return a DCM rotation matrix representing our current attitude in NED frame
virtual const Matrix3f &get_rotation_body_to_ned(void) const = 0;
// return a Quaternion representing our current attitude in NED frame
void get_quat_body_to_ned(Quaternion &quat) const {
quat.from_rotation_matrix(get_rotation_body_to_ned());
}
2016-08-09 15:47:59 -03:00
const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
2012-08-21 23:19:51 -03:00
// get rotation matrix specifically from DCM backend (used for compass calibrator)
virtual const Matrix3f &get_DCM_rotation_body_to_ned(void) const = 0;
2019-02-19 23:53:28 -04:00
// get our current position estimate. Return true if a position is available,
// otherwise false. This call fills in lat, lng and alt
virtual bool get_position(struct Location &loc) const WARN_IF_UNUSED = 0;
// get latest altitude estimate above ground level in meters and validity flag
virtual bool get_hagl(float &height) const WARN_IF_UNUSED { return false; }
2012-08-21 23:19:51 -03:00
// return a wind estimation vector, in m/s
2017-12-02 09:13:32 -04:00
virtual Vector3f wind_estimate(void) const = 0;
2012-08-21 23:19:51 -03:00
// return an airspeed estimate if available. return true
// if we have an estimate
virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED = 0;
// return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate
bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED {
if (!airspeed_estimate(airspeed_ret)) {
return false;
}
airspeed_ret *= get_EAS2TAS();
return true;
}
// return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable
virtual bool airspeed_vector_true(Vector3f &vec) const WARN_IF_UNUSED {
return false;
}
// return a synthetic airspeed estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified
// on failure.
virtual bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED = 0;
// get apparent to true airspeed ratio
2019-06-26 23:37:09 -03:00
float get_EAS2TAS(void) const;
// return true if airspeed comes from an airspeed sensor, as
// opposed to an IMU estimate
bool airspeed_sensor_enabled(void) const {
const AP_Airspeed *_airspeed = AP::airspeed();
return _airspeed != nullptr && _airspeed->use() && _airspeed->healthy();
}
// return true if airspeed comes from a specific airspeed sensor, as
// opposed to an IMU estimate
bool airspeed_sensor_enabled(uint8_t airspeed_index) const {
const AP_Airspeed *_airspeed = AP::airspeed();
return _airspeed != nullptr && _airspeed->use(airspeed_index) && _airspeed->healthy(airspeed_index);
}
// return the parameter AHRS_WIND_MAX in metres per second
uint8_t get_max_wind() const {
return _wind_max;
}
// return a ground vector estimate in meters/second, in North/East order
virtual Vector2f groundspeed_vector(void);
// return a ground velocity in meters/second, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
2015-09-23 04:29:43 -03:00
// true
virtual bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED {
2015-09-23 04:29:43 -03:00
return false;
}
// returns the estimated magnetic field offsets in body frame
virtual bool get_mag_field_correction(Vector3f &ret) const WARN_IF_UNUSED {
return false;
}
// return a position relative to home in meters, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
2015-09-23 04:29:43 -03:00
// true
virtual bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED {
2015-09-23 04:29:43 -03:00
return false;
}
// return a position relative to origin in meters, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
// true
virtual bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED {
return false;
}
// return a position relative to home in meters, North/East
// order. Return true if estimate is valid
virtual bool get_relative_position_NE_home(Vector2f &vecNE) const WARN_IF_UNUSED {
return false;
}
// return a position relative to origin in meters, North/East
// order. Return true if estimate is valid
virtual bool get_relative_position_NE_origin(Vector2f &vecNE) const WARN_IF_UNUSED {
return false;
}
// return a Down position relative to home in meters
// if EKF is unavailable will return the baro altitude
virtual void get_relative_position_D_home(float &posD) const = 0;
// return a Down position relative to origin in meters
// Return true if estimate is valid
virtual bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED {
return false;
}
// return ground speed estimate in meters/second. Used by ground vehicles.
float groundspeed(void) {
return groundspeed_vector().length();
}
// return true if we will use compass for yaw
virtual bool use_compass(void) = 0;
2012-08-21 23:19:51 -03:00
// return true if yaw has been initialised
bool yaw_initialised(void) const {
return _flags.have_initial_yaw;
2012-08-21 23:19:51 -03:00
}
// set the correct centrifugal flag
// allows arducopter to disable corrections when disarmed
void set_correct_centrifugal(bool setting) {
_flags.correct_centrifugal = setting;
2012-08-21 23:19:51 -03:00
}
// get the correct centrifugal flag
bool get_correct_centrifugal(void) const {
return _flags.correct_centrifugal;
}
2013-02-19 05:50:57 -04:00
// get trim
2015-09-23 04:29:43 -03:00
const Vector3f &get_trim() const {
return _trim.get();
}
2013-02-19 05:50:57 -04:00
// set trim
void set_trim(const Vector3f &new_trim);
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
// helper trig value accessors
2015-09-23 04:29:43 -03:00
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;
}
2020-03-31 22:08:54 -03:00
// return the quaternion defining the rotation from NED to XYZ (body) axes
virtual bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED = 0;
// return secondary attitude solution if available, as eulers in radians
virtual bool get_secondary_attitude(Vector3f &eulers) const WARN_IF_UNUSED {
2015-09-23 04:29:43 -03:00
return false;
}
// return secondary attitude solution if available, as quaternion
virtual bool get_secondary_quaternion(Quaternion &quat) const WARN_IF_UNUSED {
return false;
}
2019-02-19 23:53:28 -04:00
// return secondary position solution if available
virtual bool get_secondary_position(struct Location &loc) const WARN_IF_UNUSED {
2015-09-23 04:29:43 -03:00
return false;
}
// get the home location. This is const to prevent any changes to
2015-09-23 04:29:43 -03:00
// home without telling AHRS about the change
const struct Location &get_home(void) const {
return _home;
}
// functions to handle locking of home. Some vehicles use this to
// allow GCS to lock in a home location.
void lock_home() {
_home_locked = true;
}
bool home_is_locked() const {
return _home_locked;
}
// returns true if home is set
bool home_is_set(void) const {
return _home_is_set;
}
// set the home location in 10e7 degrees. This should be called
// when the vehicle is at this position. It is assumed that the
// current barometer and GPS altitudes correspond to this altitude
virtual bool set_home(const Location &loc) WARN_IF_UNUSED = 0;
2017-04-19 03:28:14 -03:00
// set the EKF's origin location in 10e7 degrees. This should only
// be called when the EKF has no absolute position reference (i.e. GPS)
// from which to decide the origin on its own
virtual bool set_origin(const Location &loc) WARN_IF_UNUSED { return false; }
2017-04-19 03:28:14 -03:00
// returns the inertial navigation origin in lat/lon/alt
virtual bool get_origin(Location &ret) const WARN_IF_UNUSED { return false; }
void Log_Write_Home_And_Origin();
// return true if the AHRS object supports inertial navigation,
// with very accurate position and velocity
2015-09-23 04:29:43 -03:00
virtual bool have_inertial_nav(void) const {
return false;
}
// is the AHRS subsystem healthy?
virtual bool healthy(void) const = 0;
// true if the AHRS has completed initialisation
2015-09-23 04:29:43 -03:00
virtual bool initialised(void) const {
return true;
};
// return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
virtual uint32_t getLastYawResetAngle(float &yawAng) {
2015-10-29 05:31:02 -03:00
return 0;
};
// return the amount of NE position change in metres due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred
virtual uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED {
return 0;
};
// return the amount of NE velocity change in metres/sec due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred
virtual uint32_t getLastVelNorthEastReset(Vector2f &vel) const WARN_IF_UNUSED {
return 0;
};
// return the amount of vertical position change due to the last reset in meters
// returns the time of the last reset or 0 if no reset has ever occurred
virtual uint32_t getLastPosDownReset(float &posDelta) WARN_IF_UNUSED {
return 0;
};
// Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
// Returns true if the height datum reset has been performed
// If using a range finder for height no reset is performed and it returns false
virtual bool resetHeightDatum(void) WARN_IF_UNUSED {
return false;
}
2019-02-19 23:53:28 -04:00
2019-10-04 22:04:00 -03:00
// return the innovations for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
virtual bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const {
return false;
}
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
2017-11-27 08:08:33 -04:00
// indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accepted by the filter
// boolean false is returned if variances are not available
virtual bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const {
return false;
}
2019-02-19 23:53:28 -04:00
// get a source's velocity innovations. source should be from 0 to 7 (see AP_NavEKF_Source::SourceXY)
// returns true on success and results are placed in innovations and variances arguments
virtual bool get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED {
return false;
}
// get the selected ekf type, for allocation decisions
int8_t get_ekf_type(void) const {
return _ekf_type;
}
// Retrieves the corrected NED delta velocity in use by the inertial navigation
2018-03-10 05:35:03 -04:00
virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const {
ret.zero();
AP::ins().get_delta_velocity(ret, dt);
2018-03-10 05:35:03 -04:00
}
// rotate a 2D vector from earth frame to body frame
// in result, x is forward, y is right
Vector2f earth_to_body2D(const Vector2f &ef_vector) const;
// rotate a 2D vector from earth frame to body frame
// in input, x is forward, y is right
Vector2f body_to_earth2D(const Vector2f &bf) const;
2019-02-19 23:53:28 -04:00
// convert a vector from body to earth frame
Vector3f body_to_earth(const Vector3f &v) const {
return v * get_rotation_body_to_ned();
}
// convert a vector from earth to body frame
Vector3f earth_to_body(const Vector3f &v) const {
return get_rotation_body_to_ned().mul_transpose(v);
}
// get_hgt_ctrl_limit - get maximum height to be observed by the
// control loops in meters and a validity flag. It will return
// false when no limiting is required
virtual bool get_hgt_ctrl_limit(float &limit) const WARN_IF_UNUSED { return false; };
// Set to true if the terrain underneath is stable enough to be used as a height reference
// this is not related to terrain following
virtual void set_terrain_hgt_stable(bool stable) {}
// Write position and quaternion data from an external navigation system
2020-04-13 02:03:01 -03:00
virtual void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms) { }
2020-05-12 03:06:24 -03:00
// Write velocity data from an external navigation system
virtual void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) { }
// return current vibration vector for primary IMU
Vector3f get_vibration(void) const;
// set and save the alt noise parameter value
virtual void set_alt_measurement_noise(float noise) {};
// allow threads to lock against AHRS update
HAL_Semaphore &get_semaphore(void) {
return _rsem;
}
2019-02-19 23:53:28 -04:00
2021-01-02 22:54:39 -04:00
// active AHRS type for logging
virtual uint8_t get_active_AHRS_type(void) const { return 0; }
// Logging to disk functions
void Write_AHRS2(void) const;
void Write_Attitude(const Vector3f &targets) const;
void Write_Origin(uint8_t origin_type, const Location &loc) const;
void Write_POS(void) const;
protected:
2019-02-19 23:53:28 -04:00
// multi-thread access support
HAL_Semaphore _rsem;
2019-02-19 23:53:28 -04:00
AHRS_VehicleClass _vehicle_class{AHRS_VEHICLE_UNKNOWN};
// settable parameters
// these are public for ArduCopter
AP_Float _kp_yaw;
AP_Float _kp;
AP_Float gps_gain;
AP_Float beta;
enum class GPSUse : uint8_t {
Disable = 0,
Enable = 1,
EnableWithHeight = 2,
};
AP_Enum<GPSUse> _gps_use;
AP_Int8 _wind_max;
AP_Int8 _board_orientation;
AP_Int8 _gps_minsats;
AP_Int8 _ekf_type;
AP_Float _custom_roll;
AP_Float _custom_pitch;
AP_Float _custom_yaw;
Matrix3f _custom_rotation;
2012-08-21 23:19:51 -03:00
// flags structure
struct ahrs_flags {
uint8_t have_initial_yaw : 1; // whether the yaw value has been intialised with a reference
uint8_t fly_forward : 1; // 1 if we can assume the aircraft will be flying forward on its X axis
uint8_t correct_centrifugal : 1; // 1 if we should correct for centrifugal forces (allows arducopter to turn this off when motors are disarmed)
uint8_t wind_estimation : 1; // 1 if we should do wind estimation
uint8_t takeoff_expected : 1; // 1 if the vehicle is in a state that takeoff might be expected. Ground effect may be in play.
uint8_t touchdown_expected : 1; // 1 if the vehicle is in a state that touchdown might be expected. Ground effect may be in play.
} _flags;
// calculate sin/cos of roll/pitch/yaw from rotation
void calc_trig(const Matrix3f &rot,
float &cr, float &cp, float &cy,
float &sr, float &sp, float &sy) const;
2019-02-19 23:53:28 -04:00
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
// should be called after _dcm_matrix is updated
void update_trig(void);
// update roll_sensor, pitch_sensor and yaw_sensor
void update_cd_values(void);
// update takeoff/touchdown flags
void update_flags();
2012-08-21 23:19:51 -03:00
// pointer to airspeed object, if available
// a vector to capture the difference between the controller and body frames
AP_Vector3f _trim;
2012-03-19 03:34:12 -03:00
2016-08-09 15:47:59 -03:00
// cached trim rotations
Vector3f _last_trim;
Matrix3f _rotation_autopilot_body_to_vehicle_body;
Matrix3f _rotation_vehicle_body_to_autopilot_body;
// accelerometer values in the earth frame in m/s/s
Vector3f _accel_ef[INS_MAX_INSTANCES];
Vector3f _accel_ef_blended;
2015-09-23 04:29:43 -03:00
// 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;
bool _home_is_set :1;
bool _home_locked :1;
// helper trig variables
float _cos_roll{1.0f};
float _cos_pitch{1.0f};
float _cos_yaw{1.0f};
float _sin_roll;
float _sin_pitch;
float _sin_yaw;
// which accelerometer instance is active
uint8_t _active_accel_instance;
2018-01-01 08:03:28 -04:00
private:
uint32_t takeoff_expected_start_ms;
uint32_t touchdown_expected_start_ms;
};