#pragma once /* 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 . */ /* * AHRS (Attitude Heading Reference System) frontend interface for * ArduPilot * */ #include "AP_AHRS_config.h" #include #include #include #include // definitions shared by inertial and ekf nav filters #include "AP_AHRS_DCM.h" #include "AP_AHRS_SIM.h" #include "AP_AHRS_External.h" // forward declare view class class AP_AHRS_View; #define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started // fwd declare GSF estimator class EKFGSF_yaw; class AP_AHRS { friend class AP_AHRS_View; public: enum Flags { FLAG_ALWAYS_USE_EKF = 0x1, }; // Constructor AP_AHRS(uint8_t flags = 0); // initialise void init(void); /* Do not allow copies */ CLASS_NO_COPY(AP_AHRS); // get singleton instance static AP_AHRS *get_singleton() { return _singleton; } // periodically checks to see if we should update the AHRS // orientation (e.g. based on the AHRS_ORIENTATION parameter) // allow for runtime change of orientation // this makes initial config easier void update_orientation(); // allow threads to lock against AHRS update HAL_Semaphore &get_semaphore(void) { return _rsem; } // return the smoothed gyro vector corrected for drift const Vector3f &get_gyro(void) const { return state.gyro_estimate; } // return the current drift correction integrator value const Vector3f &get_gyro_drift(void) const { return state.gyro_drift; } // reset the current gyro drift estimate // should be called if gyro offsets are recalculated void reset_gyro_drift(); void update(bool skip_ins_update=false); void reset(); // get current location estimate bool get_location(Location &loc) const; // get latest altitude estimate above ground level in meters and validity flag bool get_hagl(float &hagl) const WARN_IF_UNUSED; // status reporting of estimated error float get_error_rp() const; float get_error_yaw() const; /* * wind estimation support */ // enable wind estimation void set_wind_estimation_enabled(bool b) { wind_estimation_enabled = b; } // wind_estimation_enabled returns true if wind estimation is enabled bool get_wind_estimation_enabled() const { return wind_estimation_enabled; } // return a wind estimation vector, in m/s; returns 0,0,0 on failure const Vector3f &wind_estimate() const { return state.wind_estimate; } // return a wind estimation vector, in m/s; returns 0,0,0 on failure bool wind_estimate(Vector3f &wind) const; // Determine how aligned heading_deg is with the wind. Return result // is 1.0 when perfectly aligned heading into wind, -1 when perfectly // aligned with-wind, and zero when perfect cross-wind. There is no // distinction between a left or right cross-wind. Wind speed is ignored float wind_alignment(const float heading_deg) const; // returns forward head-wind component in m/s. Negative means tail-wind float head_wind(void) const; // instruct DCM to update its wind estimate: void estimate_wind() { #if AP_AHRS_DCM_ENABLED dcm.estimate_wind(); #endif } // return the parameter AHRS_WIND_MAX in metres per second uint8_t get_max_wind() const { return _wind_max; } /* * airspeed support */ // get apparent to true airspeed ratio float get_EAS2TAS(void) const { return state.EAS2TAS; } // return an airspeed estimate if available. return true // if we have an estimate bool airspeed_estimate(float &airspeed_ret) const; enum AirspeedEstimateType : uint8_t { NO_NEW_ESTIMATE = 0, AIRSPEED_SENSOR = 1, DCM_SYNTHETIC = 2, EKF3_SYNTHETIC = 3, SIM = 4, }; // return an airspeed estimate if available. return true // if we have an estimate bool airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &type) const; // return true if the current AHRS airspeed estimate (from airspeed_estimate method) is directly derived from an airspeed sensor bool using_airspeed_sensor() const; // return a true airspeed estimate (navigation airspeed) if // available. return true if we have an estimate bool airspeed_estimate_true(float &airspeed_ret) const; // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable bool airspeed_vector_true(Vector3f &vec) const; // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed // returns false if the data is unavailable bool airspeed_health_data(float &innovation, float &innovationVariance, uint32_t &age_ms) const; // return true if a airspeed sensor is enabled bool airspeed_sensor_enabled(void) const { // FIXME: make this a method on the active backend return AP_AHRS_Backend::airspeed_sensor_enabled(); } // return true if a airspeed from a specific airspeed sensor is enabled bool airspeed_sensor_enabled(uint8_t airspeed_index) const { // FIXME: make this a method on the active backend return AP_AHRS_Backend::airspeed_sensor_enabled(airspeed_index); } // 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. bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED; // true if compass is being used bool use_compass(); // return the quaternion defining the rotation from NED to XYZ (body) axes bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED; // return secondary attitude solution if available, as eulers in radians bool get_secondary_attitude(Vector3f &eulers) const { eulers = state.secondary_attitude; return state.secondary_attitude_ok; } // return secondary attitude solution if available, as quaternion bool get_secondary_quaternion(Quaternion &quat) const { quat = state.secondary_quat; return state.secondary_quat_ok; } // return secondary position solution if available bool get_secondary_position(Location &loc) const { loc = state.secondary_pos; return state.secondary_pos_ok; } // EKF has a better ground speed vector estimate const Vector2f &groundspeed_vector() const { return state.ground_speed_vec; } // return ground speed estimate in meters/second. Used by ground vehicles. float groundspeed(void) const { return state.ground_speed; } const Vector3f &get_accel_ef() const { return state.accel_ef; } // Retrieves the corrected NED delta velocity in use by the inertial navigation void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { ret = state.corrected_dv; dt = state.corrected_dv_dt; } // 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 bool set_origin(const Location &loc) WARN_IF_UNUSED; #if AP_AHRS_POSITION_RESET_ENABLED // Set the EKF's NE horizontal position states and their corresponding variances from the supplied WGS-84 location // and 1-sigma horizontal position uncertainty. This can be used when the EKF is dead reckoning to periodically // correct the position. If the EKF is is still using data from a postion sensor such as GPS, the position set // will not be performed. // pos_accuracy is the standard deviation of the horizontal position uncertainty in metres. // The altitude element of the location is not used. // Returns true if the set was successful. bool handle_external_position_estimate(const Location &loc, float pos_accuracy, uint32_t timestamp_); #endif // returns the inertial navigation origin in lat/lon/alt bool get_origin(Location &ret) const WARN_IF_UNUSED; bool have_inertial_nav() const; // return a ground velocity in meters/second, North/East/Down // order. Must only be called if have_inertial_nav() is true bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED; // return the relative position NED from either home or origin // return true if the estimate is valid bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED; bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED; // return the relative position NE from home or origin // return true if the estimate is valid bool get_relative_position_NE_home(Vector2f &posNE) const WARN_IF_UNUSED; bool get_relative_position_NE_origin(Vector2f &posNE) const WARN_IF_UNUSED; // return the relative position down from home or origin // baro will be used for the _home relative one if the EKF isn't void get_relative_position_D_home(float &posD) const; bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED; // return location corresponding to vector relative to the // vehicle's origin bool get_location_from_origin_offset(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED; bool get_location_from_home_offset(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED; // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. bool get_vert_pos_rate_D(float &velocity) const; // write optical flow measurements to EKF void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, const float heightOverride); // retrieve latest corrected optical flow samples (used for calibration) bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const; // write body odometry measurements to the EKF void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset); // Writes the default equivalent airspeed and its 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available. void writeDefaultAirSpeed(float airspeed, float uncertainty); // Write position and quaternion data from an external navigation system void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms); // Write velocity data from an external navigation system void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms); // get speed limit void getControlLimits(float &ekfGndSpdLimit, float &controlScaleXY) const; float getControlScaleZ(void) const; // is the AHRS subsystem healthy? bool healthy() const; // 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 bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const; // true if the AHRS has completed initialisation bool initialised() const; #if AP_AHRS_DCM_ENABLED // return true if *DCM* yaw has been initialised bool dcm_yaw_initialised(void) const { return dcm.yaw_initialised(); } #endif // get_filter_status - returns filter status as a series of flags bool get_filter_status(nav_filter_status &status) const; // get compass offset estimates // true if offsets are valid bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const; // 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 uint32_t getLastYawResetAngle(float &yawAng); // return the amount of NE position change in meters due to the last reset // returns the time of the last reset or 0 if no reset has ever occurred uint32_t getLastPosNorthEastReset(Vector2f &pos); // return the amount of NE velocity change in meters/sec due to the last reset // returns the time of the last reset or 0 if no reset has ever occurred uint32_t getLastVelNorthEastReset(Vector2f &vel) const; // 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 uint32_t getLastPosDownReset(float &posDelta); // 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 bool resetHeightDatum(); // send a EKF_STATUS_REPORT for current EKF void send_ekf_status_report(class GCS_MAVLINK &link) const; // get_hgt_ctrl_limit - get maximum height to be observed by the control loops in meters and a validity flag // this is used to limit height during optical flow navigation // it will return invalid when no limiting is required bool get_hgt_ctrl_limit(float &limit) const; // Set to true if the terrain underneath is stable enough to be used as a height reference // this is not related to terrain following void set_terrain_hgt_stable(bool stable); // return the innovations for the specified instance // An out of range instance (eg -1) returns data for the primary instance bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; // returns true when the state estimates are significantly degraded by vibration bool is_vibration_affected() const; // get_variances - provides the innovations normalised using the innovation variance where a value of 0 // indicates perfect consistency between the measurement and the EKF solution and a value of 1 is the maximum // inconsistency that will be accepted by the filter // boolean false is returned if variances are not available bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const; // get a source's velocity innovations // returns true on success and results are placed in innovations and variances arguments bool get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED; // returns the expected NED magnetic field bool get_mag_field_NED(Vector3f& ret) const; // returns the estimated magnetic field offsets in body frame bool get_mag_field_correction(Vector3f &ret) const; // return the index of the airspeed we should use for airspeed measurements // with multiple airspeed sensors and airspeed affinity in EKF3, it is possible to have switched // over to a lane not using the primary airspeed sensor, so AHRS should know which airspeed sensor // to use, i.e, the one being used by the primary lane. A lane switch could have happened due to an // airspeed sensor fault, which makes this even more necessary uint8_t get_active_airspeed_index() const; // return the index of the primary core or -1 if no primary core selected int8_t get_primary_core_index() const { return state.primary_core; } // get the index of the current primary accelerometer sensor uint8_t get_primary_accel_index(void) const { return state.primary_accel; } // get the index of the current primary gyro sensor uint8_t get_primary_gyro_index(void) const { return state.primary_gyro; } // see if EKF lane switching is possible to avoid EKF failsafe void check_lane_switch(void); // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe void request_yaw_reset(void); // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary void set_posvelyaw_source_set(uint8_t source_set_idx); //returns index of active source set used, 0=primary, 1=secondary, 2=tertiary uint8_t get_posvelyaw_source_set() const; void Log_Write(); // check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed bool using_noncompass_for_yaw(void) const; // check if external nav is providing yaw bool using_extnav_for_yaw(void) const; // set and save the ALT_M_NSE parameter value void set_alt_measurement_noise(float noise); // get the selected ekf type, for allocation decisions int8_t get_ekf_type(void) const { return _ekf_type; } enum class EKFType : uint8_t { #if AP_AHRS_DCM_ENABLED DCM = 0, #endif #if HAL_NAVEKF3_AVAILABLE THREE = 3, #endif #if HAL_NAVEKF2_AVAILABLE TWO = 2, #endif #if AP_AHRS_SIM_ENABLED SIM = 10, #endif #if HAL_EXTERNAL_AHRS_ENABLED EXTERNAL = 11, #endif }; // set the selected ekf type, for RC aux control void set_ekf_type(EKFType ahrs_type) { _ekf_type.set(ahrs_type); } // these are only out here so vehicles can reference them for parameters #if HAL_NAVEKF2_AVAILABLE NavEKF2 EKF2; #endif #if HAL_NAVEKF3_AVAILABLE NavEKF3 EKF3; #endif // for holding parameters static const struct AP_Param::GroupInfo var_info[]; // create a view AP_AHRS_View *create_view(enum Rotation rotation, float pitch_trim_deg=0); // write AOA and SSA information to dataflash logs: void Write_AOA_SSA(void) const; // return AOA float getAOA(void) const { return _AOA; } // return SSA float getSSA(void) const { return _SSA; } /* * trim-related functions */ // get trim const Vector3f &get_trim() const { return _trim.get(); } // 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); // trim rotation matrices: 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; } // Logging functions void Log_Write_Home_And_Origin(); void Write_Attitude(const Vector3f &targets) const; enum class LogOriginType { ekf_origin = 0, ahrs_home = 1 }; void Write_Origin(LogOriginType origin_type, const Location &loc) const; void write_video_stabilisation() const; // 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; // get yaw rate in earth frame in radians/sec float get_yaw_rate_earth(void) const { return get_gyro() * get_rotation_body_to_ned().c; } /* * home-related functionality */ // get the home location. This is const to prevent any changes to // home without telling AHRS about the change const 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 bool set_home(const Location &loc) WARN_IF_UNUSED; /* * Attitude-related public methods and attributes: */ // roll/pitch/yaw euler angles, all in 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; } // 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; } // integer Euler angles (Degrees * 100) int32_t roll_sensor; int32_t pitch_sensor; int32_t yaw_sensor; const Matrix3f &get_rotation_body_to_ned(void) const { return state.dcm_matrix; } // return a Quaternion representing our current attitude in NED frame void get_quat_body_to_ned(Quaternion &quat) const; #if AP_AHRS_DCM_ENABLED // get rotation matrix specifically from DCM backend (used for // compass calibrator) const Matrix3f &get_DCM_rotation_body_to_ned(void) const { return dcm_estimates.dcm_matrix; } #endif // 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; // convert a vector from body to earth frame Vector3f body_to_earth(const Vector3f &v) const; // convert a vector from earth to body frame Vector3f earth_to_body(const Vector3f &v) const; /* * methods for the benefit of LUA bindings */ // return current vibration vector for primary IMU Vector3f get_vibration(void) const; // return primary accels, for lua const Vector3f &get_accel(void) const { return AP::ins().get_accel(); } // return primary accel bias. This should be subtracted from // get_accel() vector to get best current body frame accel // estimate const Vector3f &get_accel_bias(void) const { return state.accel_bias; } /* * AHRS is used as a transport for vehicle-takeoff-expected and * vehicle-landing-expected: */ void set_takeoff_expected(bool b); bool get_takeoff_expected(void) const { return takeoff_expected; } void set_touchdown_expected(bool b); bool get_touchdown_expected(void) const { return touchdown_expected; } /* * fly_forward is set by the vehicles to indicate the vehicle * should generally be moving in the direction of its heading. * It is an additional piece of information that the backends can * use to provide additional and/or improved estimates. */ void set_fly_forward(bool b) { fly_forward = b; } bool get_fly_forward(void) const { return fly_forward; } /* we modify our behaviour based on what sort of vehicle the * vehicle code tells us we are. This information is also pulled * from AP_AHRS by other libraries */ enum class VehicleClass : uint8_t { UNKNOWN, GROUND, COPTER, FIXED_WING, SUBMARINE, }; VehicleClass get_vehicle_class(void) const { return _vehicle_class; } void set_vehicle_class(VehicleClass vclass) { _vehicle_class = vclass; } // get the view AP_AHRS_View *get_view(void) const { return _view; }; // get access to an EKFGSF_yaw estimator const EKFGSF_yaw *get_yaw_estimator(void) const; private: // optional view class AP_AHRS_View *_view; static AP_AHRS *_singleton; /* we modify our behaviour based on what sort of vehicle the * vehicle code tells us we are. This information is also pulled * from AP_AHRS by other libraries */ VehicleClass _vehicle_class{VehicleClass::UNKNOWN}; // multi-thread access support HAL_Semaphore _rsem; /* * Parameters */ AP_Int8 _wind_max; AP_Int8 _board_orientation; AP_Enum _ekf_type; /* * DCM-backend parameters; it takes references to these */ // settable parameters AP_Float _kp_yaw; AP_Float _kp; AP_Float gps_gain; AP_Float beta; AP_Enum _gps_use; AP_Int8 _gps_minsats; EKFType active_EKF_type(void) const { return state.active_EKF; } bool always_use_EKF() const { return _ekf_flags & FLAG_ALWAYS_USE_EKF; } // check all cores providing consistent attitudes for prearm checks bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const; // convenience method for setting error string: void set_failure_inconsistent_message(const char *estimator, const char *axis, float diff_rad, char *failure_msg, const uint8_t failure_msg_len) const; /* * Attitude-related private methods and attributes: */ // 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; // 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); // 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; #if HAL_NAVEKF2_AVAILABLE void update_EKF2(void); bool _ekf2_started; #endif #if HAL_NAVEKF3_AVAILABLE bool _ekf3_started; void update_EKF3(void); #endif // rotation from vehicle body to NED frame const uint16_t startup_delay_ms = 1000; uint32_t start_time_ms; uint8_t _ekf_flags; // bitmask from Flags enumeration EKFType ekf_type(void) const; void update_DCM(); /* * home-related state */ void load_watchdog_home(); bool _checked_watchdog_home; Location _home; bool _home_is_set :1; bool _home_locked :1; // avoid setting current state repeatedly across all cores on all EKFs: enum class TriState { False = 0, True = 1, UNKNOWN = 3, }; TriState terrainHgtStableState = TriState::UNKNOWN; /* * private AOA and SSA-related state and methods */ float _AOA, _SSA; uint32_t _last_AOA_update_ms; void update_AOA_SSA(void); EKFType last_active_ekf_type; #if AP_AHRS_SIM_ENABLED void update_SITL(void); #endif #if HAL_EXTERNAL_AHRS_ENABLED void update_external(void); #endif /* * trim-related state and private methods: */ // a vector to capture the difference between the controller and body frames AP_Vector3f _trim; // cached trim rotations Vector3f _last_trim; Matrix3f _rotation_autopilot_body_to_vehicle_body; Matrix3f _rotation_vehicle_body_to_autopilot_body; // last time orientation was updated from AHRS_ORIENTATION: uint32_t last_orientation_update_ms; // updates matrices responsible for rotating vectors from vehicle body // frame to autopilot body frame from _trim variables void update_trim_rotation_matrices(); /* * AHRS is used as a transport for vehicle-takeoff-expected and * vehicle-landing-expected: */ // update takeoff/touchdown flags void update_flags(); bool takeoff_expected; // true if the vehicle is in a state that takeoff might be expected. Ground effect may be in play. uint32_t takeoff_expected_start_ms; bool touchdown_expected; // true if the vehicle is in a state that touchdown might be expected. Ground effect may be in play. uint32_t touchdown_expected_start_ms; /* * wind estimation support */ bool wind_estimation_enabled; /* * fly_forward is set by the vehicles to indicate the vehicle * should generally be moving in the direction of its heading. * It is an additional piece of information that the backends can * use to provide additional and/or improved estimates. */ bool fly_forward; // true if we can assume the vehicle will be flying forward on its X axis // poke AP_Notify based on values from status void update_notify_from_filter_status(const nav_filter_status &status); /* * copy results from a backend over AP_AHRS canonical results. * This updates member variables like roll and pitch, as well as * updating derived values like sin_roll and sin_pitch. */ void copy_estimates_from_backend_estimates(const AP_AHRS_Backend::Estimates &results); // write out secondary estimates: void Write_AHRS2(void) const; // write POS (canonical vehicle position) message out: void Write_POS(void) const; // return an airspeed estimate if available. return true // if we have an estimate bool _airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &status) const; // return secondary attitude solution if available, as eulers in radians bool _get_secondary_attitude(Vector3f &eulers) const; // return secondary attitude solution if available, as quaternion bool _get_secondary_quaternion(Quaternion &quat) const; // get ground speed 2D Vector2f _groundspeed_vector(void); // get active EKF type EKFType _active_EKF_type(void) const; // return a wind estimation vector, in m/s bool _wind_estimate(Vector3f &wind) const WARN_IF_UNUSED; // return a true airspeed estimate (navigation airspeed) if // available. return true if we have an estimate bool _airspeed_estimate_true(float &airspeed_ret) const; // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable bool _airspeed_vector_true(Vector3f &vec) const; // return the quaternion defining the rotation from NED to XYZ (body) axes bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED; // return secondary position solution if available bool _get_secondary_position(Location &loc) const; // return ground speed estimate in meters/second. Used by ground vehicles. float _groundspeed(void); // Retrieves the corrected NED delta velocity in use by the inertial navigation void _getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const; // returns the inertial navigation origin in lat/lon/alt bool _get_origin(Location &ret) const WARN_IF_UNUSED; // return origin for a specified EKF type bool _get_origin(EKFType type, Location &ret) const; // return a ground velocity in meters/second, North/East/Down // order. Must only be called if have_inertial_nav() is true bool _get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED; // get secondary EKF type. returns false if no secondary (i.e. only using DCM) bool _get_secondary_EKF_type(EKFType &secondary_ekf_type) const; // return the index of the primary core or -1 if no primary core selected int8_t _get_primary_core_index() const; // get the index of the current primary accelerometer sensor uint8_t _get_primary_accel_index(void) const; // get the index of the current primary gyro sensor uint8_t _get_primary_gyro_index(void) const; // get the index of the current primary IMU uint8_t _get_primary_IMU_index(void) const; // get current location estimate bool _get_location(Location &loc) const; // return true if a airspeed sensor should be used for the AHRS airspeed estimate bool _should_use_airspeed_sensor(uint8_t airspeed_index) const; /* update state structure */ void update_state(void); // returns an EKF type to be used as active if we decide the // primary is not good enough. EKFType fallback_active_EKF_type(void) const; /* state updated at the end of each update() call */ struct { EKFType active_EKF; uint8_t primary_IMU; uint8_t primary_gyro; uint8_t primary_accel; uint8_t primary_core; Vector3f gyro_estimate; Matrix3f dcm_matrix; Vector3f gyro_drift; Vector3f accel_ef; Vector3f accel_bias; Vector3f wind_estimate; bool wind_estimate_ok; float EAS2TAS; bool airspeed_ok; float airspeed; AirspeedEstimateType airspeed_estimate_type; bool airspeed_true_ok; float airspeed_true; Vector3f airspeed_vec; bool airspeed_vec_ok; Quaternion quat; bool quat_ok; Vector3f secondary_attitude; bool secondary_attitude_ok; Quaternion secondary_quat; bool secondary_quat_ok; Location location; bool location_ok; Location secondary_pos; bool secondary_pos_ok; Vector2f ground_speed_vec; float ground_speed; Vector3f corrected_dv; float corrected_dv_dt; Location origin; bool origin_ok; Vector3f velocity_NED; bool velocity_NED_ok; } state; /* * backends (and their results) */ #if AP_AHRS_DCM_ENABLED AP_AHRS_DCM dcm{_kp_yaw, _kp, gps_gain, beta, _gps_use, _gps_minsats}; struct AP_AHRS_Backend::Estimates dcm_estimates; #endif #if AP_AHRS_SIM_ENABLED #if HAL_NAVEKF3_AVAILABLE AP_AHRS_SIM sim{EKF3}; #else AP_AHRS_SIM sim; #endif struct AP_AHRS_Backend::Estimates sim_estimates; #endif #if HAL_EXTERNAL_AHRS_ENABLED AP_AHRS_External external; struct AP_AHRS_Backend::Estimates external_estimates; #endif enum class Options : uint16_t { DISABLE_DCM_FALLBACK_FW=(1U<<0), DISABLE_DCM_FALLBACK_VTOL=(1U<<1), }; AP_Int16 _options; bool option_set(Options option) const { return (_options & uint16_t(option)) != 0; } }; namespace AP { AP_AHRS &ahrs(); };