diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index cf96328287..502d6123ce 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -18,6 +18,7 @@ #include #include "AP_PitchController.h" +#include extern const AP_HAL::HAL& hal; @@ -133,9 +134,8 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = { AP_GROUPEND }; -AP_PitchController::AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) +AP_PitchController::AP_PitchController(const AP_Vehicle::FixedWing &parms) : aparm(parms) - , _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info); rate_pid.set_slew_limit_scale(45); @@ -147,6 +147,9 @@ AP_PitchController::AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWin int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed) { const float dt = AP::scheduler().get_loop_period_s(); + + const AP_AHRS &_ahrs = AP::ahrs(); + const float eas2tas = _ahrs.get_EAS2TAS(); bool limit_I = fabsf(_last_out) >= 45; float rate_y = _ahrs.get_gyro().y; @@ -222,7 +225,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool int32_t AP_PitchController::get_rate_out(float desired_rate, float scaler) { float aspeed; - if (!_ahrs.airspeed_estimate(aspeed)) { + if (!AP::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)); } @@ -239,7 +242,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 = AP::ahrs().roll; // limit bank angle between +- 80 deg if right way up if (fabsf(bank_angle) < radians(90)) { @@ -253,6 +256,7 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv bank_angle = constrain_float(bank_angle,-radians(180),-radians(100)); } } + const AP_AHRS &_ahrs = AP::ahrs(); 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)); @@ -324,6 +328,7 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool linearly reduce pitch demanded rate when beyond the configured roll limit, reducing to zero at 90 degrees */ + const AP_AHRS &_ahrs = AP::ahrs(); float roll_wrapped = labs(_ahrs.roll_sensor); if (roll_wrapped > 9000) { roll_wrapped = 18000 - roll_wrapped; diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index aa81552136..6317ae59b6 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include "AP_AutoTune.h" @@ -10,7 +9,7 @@ class AP_PitchController { public: - AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms); + AP_PitchController(const AP_Vehicle::FixedWing &parms); /* Do not allow copies */ AP_PitchController(const AP_PitchController &other) = delete; @@ -60,6 +59,4 @@ 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; }; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 101bc2750e..cd8603d040 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -19,6 +19,7 @@ #include #include "AP_RollController.h" +#include extern const AP_HAL::HAL& hal; @@ -117,9 +118,8 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = { }; // constructor -AP_RollController::AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) +AP_RollController::AP_RollController(const AP_Vehicle::FixedWing &parms) : aparm(parms) - , _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info); rate_pid.set_slew_limit_scale(45); @@ -131,6 +131,8 @@ AP_RollController::AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing */ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator) { + const AP_AHRS &_ahrs = AP::ahrs(); + const float dt = AP::scheduler().get_loop_period_s(); const float eas2tas = _ahrs.get_EAS2TAS(); bool limit_I = fabsf(_last_out) >= 45; diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 9e3f8b8652..227d4b7b1a 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include "AP_AutoTune.h" @@ -10,7 +9,7 @@ class AP_RollController { public: - AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms); + AP_RollController(const AP_Vehicle::FixedWing &parms); /* Do not allow copies */ AP_RollController(const AP_RollController &other) = delete; @@ -64,6 +63,4 @@ private: AP_Logger::PID_Info _pid_info; int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator); - - AP_AHRS &_ahrs; }; diff --git a/libraries/APM_Control/AP_SteerController.cpp b/libraries/APM_Control/AP_SteerController.cpp index 1a43513e8b..055de44101 100644 --- a/libraries/APM_Control/AP_SteerController.cpp +++ b/libraries/APM_Control/AP_SteerController.cpp @@ -17,6 +17,7 @@ // Based upon the roll controller by Paul Riseborough and Jon Challinger // +#include #include #include #include "AP_SteerController.h" @@ -129,6 +130,8 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) } _last_t = tnow; + AP_AHRS &_ahrs = AP::ahrs(); + float speed = _ahrs.groundspeed(); if (speed < _minspeed) { // assume a minimum speed. This stops oscillations when first starting to move @@ -211,7 +214,7 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) */ int32_t AP_SteerController::get_steering_out_lat_accel(float desired_accel) { - float speed = _ahrs.groundspeed(); + float speed = AP::ahrs().groundspeed(); if (speed < _minspeed) { // assume a minimum speed. This reduces osciallations when first starting to move speed = _minspeed; diff --git a/libraries/APM_Control/AP_SteerController.h b/libraries/APM_Control/AP_SteerController.h index 5cad3afe45..6ecb938024 100644 --- a/libraries/APM_Control/AP_SteerController.h +++ b/libraries/APM_Control/AP_SteerController.h @@ -1,14 +1,12 @@ #pragma once -#include #include #include #include class AP_SteerController { public: - AP_SteerController(AP_AHRS &ahrs) - : _ahrs(ahrs) + AP_SteerController() { AP_Param::setup_object_defaults(this, var_info); } @@ -69,7 +67,5 @@ private: AP_Logger::PID_Info _pid_info {}; - AP_AHRS &_ahrs; - bool _reverse; }; diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index fb6daa55e1..1b6fb95a99 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -19,6 +19,7 @@ // #include #include "AP_YawController.h" +#include extern const AP_HAL::HAL& hal; @@ -93,11 +94,12 @@ 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 = AP::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); } + const AP_AHRS &_ahrs = AP::ahrs(); 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)); @@ -179,4 +181,4 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator) void AP_YawController::reset_I() { _integrator = 0; -} \ No newline at end of file +} diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index 611483c4f2..13bd5e5ea2 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include @@ -8,9 +7,8 @@ class AP_YawController { public: - AP_YawController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) + AP_YawController(const AP_Vehicle::FixedWing &parms) : aparm(parms) - , _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info); _pid_info.target = 0; @@ -54,6 +52,4 @@ private: float _integrator; AP_Logger::PID_Info _pid_info; - - AP_AHRS &_ahrs; }; diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 16e8416a74..8e146bdca6 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -13,6 +13,7 @@ along with this program. If not, see . */ +#include #include #include #include "AR_AttitudeControl.h" @@ -406,8 +407,7 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { AP_GROUPEND }; -AR_AttitudeControl::AR_AttitudeControl(AP_AHRS &ahrs) : - _ahrs(ahrs), +AR_AttitudeControl::AR_AttitudeControl() : _steer_angle_p(AR_ATTCONTROL_STEER_ANG_P), _steer_rate_pid(AR_ATTCONTROL_STEER_RATE_P, AR_ATTCONTROL_STEER_RATE_I, AR_ATTCONTROL_STEER_RATE_D, AR_ATTCONTROL_STEER_RATE_FF, AR_ATTCONTROL_STEER_RATE_IMAX, 0.0f, AR_ATTCONTROL_STEER_RATE_FILT, 0.0f, AR_ATTCONTROL_DT), _throttle_speed_pid(AR_ATTCONTROL_THR_SPEED_P, AR_ATTCONTROL_THR_SPEED_I, AR_ATTCONTROL_THR_SPEED_D, 0.0f, AR_ATTCONTROL_THR_SPEED_IMAX, 0.0f, AR_ATTCONTROL_THR_SPEED_FILT, 0.0f, AR_ATTCONTROL_DT), @@ -452,7 +452,7 @@ float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate // return a desired turn-rate given a desired heading in radians float AR_AttitudeControl::get_turn_rate_from_heading(float heading_rad, float rate_max_rads) const { - const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw); + const float yaw_error = wrap_PI(heading_rad - AP::ahrs().yaw); // Calculate the desired turn rate (in radians) from the angle error (also in radians) float desired_rate = _steer_angle_p.get_p(yaw_error); @@ -477,7 +477,7 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l if ((_steer_turn_last_ms == 0) || ((now - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) { _steer_rate_pid.reset_filter(); _steer_rate_pid.reset_I(); - _desired_turn_rate = _ahrs.get_yaw_rate_earth(); + _desired_turn_rate = AP::ahrs().get_yaw_rate_earth(); } _steer_turn_last_ms = now; @@ -505,7 +505,7 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l // set PID's dt _steer_rate_pid.set_dt(dt); - float output = _steer_rate_pid.update_all(_desired_turn_rate, _ahrs.get_yaw_rate_earth(), (motor_limit_left || motor_limit_right)); + float output = _steer_rate_pid.update_all(_desired_turn_rate, AP::ahrs().get_yaw_rate_earth(), (motor_limit_left || motor_limit_right)); output += _steer_rate_pid.get_ff(); // constrain and return final output return output; @@ -538,7 +538,7 @@ bool AR_AttitudeControl::get_lat_accel(float &lat_accel) const if (!get_forward_speed(speed)) { return false; } - lat_accel = speed * _ahrs.get_yaw_rate_earth(); + lat_accel = speed * AP::ahrs().get_yaw_rate_earth(); return true; } @@ -686,7 +686,7 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float // add feed forward from speed float output = vehicle_speed_pct * 0.01f * _pitch_to_throttle_speed_ff; - output += _pitch_to_throttle_pid.update_all(desired_pitch, _ahrs.pitch, (motor_limit_low || motor_limit_high)); + output += _pitch_to_throttle_pid.update_all(desired_pitch, AP::ahrs().pitch, (motor_limit_low || motor_limit_high)); output += _pitch_to_throttle_pid.get_ff(); // constrain and return final output @@ -722,7 +722,7 @@ float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt) // set PID's dt _sailboat_heel_pid.set_dt(dt); - _sailboat_heel_pid.update_all(desired_heel, fabsf(_ahrs.roll)); + _sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().roll)); // get feed-forward const float ff = _sailboat_heel_pid.get_ff(); @@ -751,6 +751,7 @@ float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt) bool AR_AttitudeControl::get_forward_speed(float &speed) const { Vector3f velocity; + const AP_AHRS &_ahrs = AP::ahrs(); if (!_ahrs.get_velocity_NED(velocity)) { // use less accurate GPS, assuming entire length is along forward/back axis of vehicle if (AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) { diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index e63f10b4f8..184e8fdcaa 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include @@ -49,7 +48,7 @@ class AR_AttitudeControl { public: // constructor - AR_AttitudeControl(AP_AHRS &ahrs); + AR_AttitudeControl(); // // steering controller @@ -157,9 +156,6 @@ public: private: - // external references - const AP_AHRS &_ahrs; - // parameters AC_P _steer_angle_p; // steering angle controller AC_PID _steer_rate_pid; // steering rate controller