From c6e37aaec3a659d74a9ce4274059e3d2f6d642ef Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 14 Aug 2013 14:56:18 +1000 Subject: [PATCH] APM_Control: use a ahrs reference, not pointer saves pointer check --- libraries/APM_Control/AP_PitchController.cpp | 11 +++++------ libraries/APM_Control/AP_PitchController.h | 9 ++++----- libraries/APM_Control/AP_RollController.cpp | 12 +++--------- libraries/APM_Control/AP_RollController.h | 9 ++++----- libraries/APM_Control/AP_YawController.cpp | 11 ++++------- libraries/APM_Control/AP_YawController.h | 14 ++++---------- 6 files changed, 24 insertions(+), 42 deletions(-) diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index d12c51d42f..0ca15a18ea 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -106,11 +106,10 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool } _last_t = tnow; - if(_ahrs == NULL) return 0; float delta_time = (float)dt * 0.001f; // Get body rate vector (radians/sec) - float omega_y = _ahrs->get_gyro().y; + float omega_y = _ahrs.get_gyro().y; // Calculate the pitch rate error (deg/sec) and scale float rate_error = (desired_rate - ToDeg(omega_y)) * scaler; @@ -143,7 +142,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool // Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law // No conversion is required for K_D - float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0) / _ahrs->get_EAS2TAS(); + float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0) / _ahrs.get_EAS2TAS(); // Calculate the demanded control surface deflection // Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward @@ -180,7 +179,7 @@ int32_t AP_PitchController::get_rate_out(float desired_rate, float scaler) float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inverted) const { float rate_offset; - float bank_angle = _ahrs->roll; + float bank_angle = _ahrs.roll; // limit bank angle between +- 80 deg if right way up if (fabsf(bank_angle) < radians(90)) { @@ -194,11 +193,11 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv bank_angle = constrain_float(bank_angle,-radians(180),-radians(100)); } } - if (!_ahrs->airspeed_estimate(&aspeed)) { + if (!_ahrs.airspeed_estimate(&aspeed)) { // If no airspeed available use average of min and max aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max)); } - if (abs(_ahrs->pitch_sensor) > 7000) { + if (abs(_ahrs.pitch_sensor) > 7000) { // don't do turn coordination handling when at very high pitch angles rate_offset = 0; } else { diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index e3551efb3b..e99ffd1600 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -10,14 +10,13 @@ class AP_PitchController { public: - AP_PitchController(const AP_SpdHgtControl::AircraftParameters &parms) : - aparm(parms) + AP_PitchController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) : + aparm(parms), + _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info); } - void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; } - int32_t get_rate_out(float desired_rate, float scaler); int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); float get_coordination_rate_offset(void) const; @@ -44,7 +43,7 @@ private: int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed); float _get_coordination_rate_offset(float &aspeed, bool &inverted) const; - AP_AHRS *_ahrs; + AP_AHRS &_ahrs; }; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index e82d5062cc..b62c5f967f 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -82,16 +82,10 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool } _last_t = tnow; - - if (_ahrs == NULL) { - // can't control without a reference - return 0; - } - // Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law // No conversion is required for K_D float ki_rate = _K_I * _tau; - float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0)/_ahrs->get_EAS2TAS(); + float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0)/_ahrs.get_EAS2TAS(); float delta_time = (float)dt * 0.001f; // Limit the demanded roll rate @@ -102,14 +96,14 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool } // Get body rate vector (radians/sec) - float omega_x = _ahrs->get_gyro().x; + float omega_x = _ahrs.get_gyro().x; // Calculate the roll rate error (deg/sec) and apply gain scaler float rate_error = (desired_rate - ToDeg(omega_x)) * scaler; // Get an airspeed estimate - default to zero if none available float aspeed; - if (!_ahrs->airspeed_estimate(&aspeed)) { + if (!_ahrs.airspeed_estimate(&aspeed)) { aspeed = 0.0f; } diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index e28071cc83..49f59ca5e6 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -10,14 +10,13 @@ class AP_RollController { public: - AP_RollController(const AP_SpdHgtControl::AircraftParameters &parms) : - aparm(parms) + AP_RollController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) : + aparm(parms), + _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info); } - void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; } - int32_t get_rate_out(float desired_rate, float scaler); int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); @@ -40,7 +39,7 @@ private: int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator); - AP_AHRS *_ahrs; + AP_AHRS &_ahrs; }; diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index 74c81c9599..eef37642b2 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -71,9 +71,6 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator) } _last_t = tnow; - if(_ins == NULL) { // can't control without a reference - return 0; - } int16_t aspd_min = aparm.airspeed_min; if (aspd_min < 1) { @@ -85,22 +82,22 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator) // Calculate yaw rate required to keep up with a constant height coordinated turn float aspeed; float rate_offset; - float bank_angle = _ahrs->roll; + float bank_angle = _ahrs.roll; // limit bank angle between +- 80 deg if right way up if (fabsf(bank_angle) < 1.5707964f) { bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f); } - if (!_ahrs->airspeed_estimate(&aspeed)) { + if (!_ahrs.airspeed_estimate(&aspeed)) { // If no airspeed available use average of min and max aspeed = 0.5f*(float(aspd_min) + float(aparm.airspeed_max)); } rate_offset = (GRAVITY_MSS / max(aspeed , float(aspd_min))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF; // Get body rate vector (radians/sec) - float omega_z = _ahrs->get_gyro().z; + float omega_z = _ahrs.get_gyro().z; // Get the accln vector (m/s^2) - float accel_y = _ins->get_accel().y; + float accel_y = _ahrs.get_ins()->get_accel().y; // Subtract the steady turn component of rate from the measured rate // to calculate the rate relative to the turn requirement in degrees/sec diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index 51c556528f..dd3c3ca158 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -10,17 +10,13 @@ class AP_YawController { public: - AP_YawController(const AP_SpdHgtControl::AircraftParameters &parms) : - aparm(parms) + AP_YawController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) : + aparm(parms), + _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info); } - void set_ahrs(AP_AHRS *ahrs) { - _ahrs = ahrs; - _ins = _ahrs->get_ins(); - } - int32_t get_servo_out(float scaler, bool disable_integrator); void reset_I(); @@ -43,9 +39,7 @@ private: float _integrator; - AP_AHRS *_ahrs; - AP_InertialSensor *_ins; - + AP_AHRS &_ahrs; }; #endif // __AP_YAW_CONTROLLER_H__