diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 982e9c4fe2..91ed8675c3 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -43,3 +43,26 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = { AP_GROUPEND }; + +// get pitch rate in earth frame, in radians/s +float AP_AHRS::get_pitch_rate_earth(void) +{ + Vector3f omega = get_gyro(); + return cos(roll) * omega.y - sin(roll) * omega.z; +} + +// get roll rate in earth frame, in radians/s +float AP_AHRS::get_roll_rate_earth(void) { + Vector3f omega = get_gyro(); + return omega.x + tan(pitch)*(omega.y*sin(roll) + omega.z*cos(roll)); +} + +// return airspeed estimate if available +bool AP_AHRS::airspeed_estimate(float *airspeed_ret) +{ + if (_airspeed && _airspeed->use()) { + *airspeed_ret = _airspeed->get_airspeed(); + return true; + } + return false; +} diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index cd801a3843..5a44430f6a 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -75,14 +75,9 @@ public: int32_t pitch_sensor; int32_t yaw_sensor; - float get_pitch_rate_earth(void) { - Vector3f omega = get_gyro(); - return cos(roll) * omega.y - sin(roll) * omega.z; - } - float get_roll_rate_earth(void) { - Vector3f omega = get_gyro(); - return omega.x + tan(pitch)*(omega.y*sin(roll) + omega.z*cos(roll)); - } + // roll and pitch rates in earth frame, in radians/s + float get_pitch_rate_earth(void); + float get_roll_rate_earth(void); // return a smoothed and corrected gyro vector virtual Vector3f get_gyro(void) = 0; @@ -129,6 +124,10 @@ public: return Vector3f(0,0,0); } + // return an airspeed estimate if available. return true + // if we have an estimate + bool airspeed_estimate(float *airspeed_ret); + // return true if yaw has been initialised bool yaw_initialised(void) { return _have_initial_yaw; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 14b60e5386..03c2f0bda5 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -696,3 +696,19 @@ bool AP_AHRS_DCM::get_position(struct Location *loc) return true; } +// return an airspeed estimate if available +bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) +{ + if (_airspeed && _airspeed->use()) { + *airspeed_ret = _airspeed->get_airspeed(); + return true; + } + + // estimate it via GPS speed and wind + if (have_gps()) { + *airspeed_ret = _last_airspeed; + return true; + } + + return false; +} diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 1c1e628103..f6263252ca 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -53,6 +53,10 @@ public: return _wind; } + // return an airspeed estimate if available. return true + // if we have an estimate + bool airspeed_estimate(float *airspeed_ret); + private: float _ki; float _ki_yaw;