Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: stop taking references to ahrs in APM_Control

Browse Source
This commit is contained in:
Peter Barker 2021-07-20 21:15:06 +10:00 committed by Andrew Tridgell
parent cbf549b1eb
commit d2102ce9b7
10 changed files with 35 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/APM_Control/AP_PitchController.cpp
View File

@ -18,6 +18,7 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_PitchController.h" #include "AP_PitchController.h"
#include <AP_AHRS/AP_AHRS.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -133,9 +134,8 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
AP_GROUPEND 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) : aparm(parms)
, _ahrs(ahrs)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
rate_pid.set_slew_limit_scale(45); 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) 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 float dt = AP::scheduler().get_loop_period_s();
const AP_AHRS &_ahrs = AP::ahrs();
const float eas2tas = _ahrs.get_EAS2TAS(); const float eas2tas = _ahrs.get_EAS2TAS();
bool limit_I = fabsf(_last_out) >= 45; bool limit_I = fabsf(_last_out) >= 45;
float rate_y = _ahrs.get_gyro().y; 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) int32_t AP_PitchController::get_rate_out(float desired_rate, float scaler)
{ {
float aspeed; float aspeed;
if (!_ahrs.airspeed_estimate(aspeed)) { if (!AP::ahrs().airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max // If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_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 AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inverted) const
{ {
float rate_offset; float rate_offset;
float bank_angle = _ahrs.roll; float bank_angle = AP::ahrs().roll;
// limit bank angle between +- 80 deg if right way up // limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < radians(90)) { 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)); bank_angle = constrain_float(bank_angle,-radians(180),-radians(100));
} }
} }
const AP_AHRS &_ahrs = AP::ahrs();
if (!_ahrs.airspeed_estimate(aspeed)) { if (!_ahrs.airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max // If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_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 linearly reduce pitch demanded rate when beyond the configured
roll limit, reducing to zero at 90 degrees roll limit, reducing to zero at 90 degrees
*/ */
const AP_AHRS &_ahrs = AP::ahrs();
float roll_wrapped = labs(_ahrs.roll_sensor); float roll_wrapped = labs(_ahrs.roll_sensor);
if (roll_wrapped > 9000) { if (roll_wrapped > 9000) {
roll_wrapped = 18000 - roll_wrapped; roll_wrapped = 18000 - roll_wrapped;

5
libraries/APM_Control/AP_PitchController.h
View File

@ -1,6 +1,5 @@
#pragma once #pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include "AP_AutoTune.h" #include "AP_AutoTune.h"
@ -10,7 +9,7 @@
class AP_PitchController { class AP_PitchController {
public: public:
AP_PitchController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms); AP_PitchController(const AP_Vehicle::FixedWing &parms);
/* Do not allow copies */ /* Do not allow copies */
AP_PitchController(const AP_PitchController &other) = delete; 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); 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; float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
AP_AHRS &_ahrs;
}; };

6
libraries/APM_Control/AP_RollController.cpp
View File

@ -19,6 +19,7 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_RollController.h" #include "AP_RollController.h"
#include <AP_AHRS/AP_AHRS.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -117,9 +118,8 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
}; };
// constructor // constructor
AP_RollController::AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) AP_RollController::AP_RollController(const AP_Vehicle::FixedWing &parms)
: aparm(parms) : aparm(parms)
, _ahrs(ahrs)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
rate_pid.set_slew_limit_scale(45); 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) 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 dt = AP::scheduler().get_loop_period_s();
const float eas2tas = _ahrs.get_EAS2TAS(); const float eas2tas = _ahrs.get_EAS2TAS();
bool limit_I = fabsf(_last_out) >= 45; bool limit_I = fabsf(_last_out) >= 45;

5
libraries/APM_Control/AP_RollController.h
View File

@ -1,6 +1,5 @@
#pragma once #pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include "AP_AutoTune.h" #include "AP_AutoTune.h"
@ -10,7 +9,7 @@
class AP_RollController { class AP_RollController {
public: public:
AP_RollController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms); AP_RollController(const AP_Vehicle::FixedWing &parms);
/* Do not allow copies */ /* Do not allow copies */
AP_RollController(const AP_RollController &other) = delete; AP_RollController(const AP_RollController &other) = delete;
@ -64,6 +63,4 @@ private:
AP_Logger::PID_Info _pid_info; AP_Logger::PID_Info _pid_info;
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator); int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);
AP_AHRS &_ahrs;
}; };

5
libraries/APM_Control/AP_SteerController.cpp
View File

@ -17,6 +17,7 @@
// Based upon the roll controller by Paul Riseborough and Jon Challinger // Based upon the roll controller by Paul Riseborough and Jon Challinger
// //
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_SteerController.h" #include "AP_SteerController.h"
@ -129,6 +130,8 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate)
} }
_last_t = tnow; _last_t = tnow;
AP_AHRS &_ahrs = AP::ahrs();
float speed = _ahrs.groundspeed(); float speed = _ahrs.groundspeed();
if (speed < _minspeed) { if (speed < _minspeed) {
// assume a minimum speed. This stops oscillations when first starting to move // 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) int32_t AP_SteerController::get_steering_out_lat_accel(float desired_accel)
{ {
float speed = _ahrs.groundspeed(); float speed = AP::ahrs().groundspeed();
if (speed < _minspeed) { if (speed < _minspeed) {
// assume a minimum speed. This reduces osciallations when first starting to move // assume a minimum speed. This reduces osciallations when first starting to move
speed = _minspeed; speed = _minspeed;

6
libraries/APM_Control/AP_SteerController.h
View File

@ -1,14 +1,12 @@
#pragma once #pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
class AP_SteerController { class AP_SteerController {
public: public:
AP_SteerController(AP_AHRS &ahrs) AP_SteerController()
: _ahrs(ahrs)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -69,7 +67,5 @@ private:
AP_Logger::PID_Info _pid_info {}; AP_Logger::PID_Info _pid_info {};
AP_AHRS &_ahrs;
bool _reverse; bool _reverse;
}; };

6
libraries/APM_Control/AP_YawController.cpp
View File

@ -19,6 +19,7 @@
// //
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_YawController.h" #include "AP_YawController.h"
#include <AP_AHRS/AP_AHRS.h>
extern const AP_HAL::HAL& hal; 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 // Calculate yaw rate required to keep up with a constant height coordinated turn
float aspeed; float aspeed;
float rate_offset; float rate_offset;
float bank_angle = _ahrs.roll; float bank_angle = AP::ahrs().roll;
// limit bank angle between +- 80 deg if right way up // limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < 1.5707964f) { if (fabsf(bank_angle) < 1.5707964f) {
bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f); bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f);
} }
const AP_AHRS &_ahrs = AP::ahrs();
if (!_ahrs.airspeed_estimate(aspeed)) { if (!_ahrs.airspeed_estimate(aspeed)) {
// If no airspeed available use average of min and max // If no airspeed available use average of min and max
aspeed = 0.5f*(float(aspd_min) + float(aparm.airspeed_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() void AP_YawController::reset_I()
{ {
_integrator = 0; _integrator = 0;
} }

6
libraries/APM_Control/AP_YawController.h
View File

@ -1,6 +1,5 @@
#pragma once #pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle.h> #include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
@ -8,9 +7,8 @@
class AP_YawController { class AP_YawController {
public: public:
AP_YawController(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) AP_YawController(const AP_Vehicle::FixedWing &parms)
: aparm(parms) : aparm(parms)
, _ahrs(ahrs)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
_pid_info.target = 0; _pid_info.target = 0;
@ -54,6 +52,4 @@ private:
float _integrator; float _integrator;
AP_Logger::PID_Info _pid_info; AP_Logger::PID_Info _pid_info;
AP_AHRS &_ahrs;
}; };

17
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -13,6 +13,7 @@
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AR_AttitudeControl.h" #include "AR_AttitudeControl.h"
@ -406,8 +407,7 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
AP_GROUPEND AP_GROUPEND
}; };
AR_AttitudeControl::AR_AttitudeControl(AP_AHRS &ahrs) : AR_AttitudeControl::AR_AttitudeControl() :
_ahrs(ahrs),
_steer_angle_p(AR_ATTCONTROL_STEER_ANG_P), _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), _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), _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 // 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 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) // Calculate the desired turn rate (in radians) from the angle error (also in radians)
float desired_rate = _steer_angle_p.get_p(yaw_error); 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)) { if ((_steer_turn_last_ms == 0) || ((now - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
_steer_rate_pid.reset_filter(); _steer_rate_pid.reset_filter();
_steer_rate_pid.reset_I(); _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; _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 // set PID's dt
_steer_rate_pid.set_dt(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(); output += _steer_rate_pid.get_ff();
// constrain and return final output // constrain and return final output
return output; return output;
@ -538,7 +538,7 @@ bool AR_AttitudeControl::get_lat_accel(float &lat_accel) const
if (!get_forward_speed(speed)) { if (!get_forward_speed(speed)) {
return false; return false;
} }
lat_accel = speed * _ahrs.get_yaw_rate_earth(); lat_accel = speed * AP::ahrs().get_yaw_rate_earth();
return true; return true;
} }
@ -686,7 +686,7 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float
// add feed forward from speed // add feed forward from speed
float output = vehicle_speed_pct * 0.01f * _pitch_to_throttle_speed_ff; 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(); output += _pitch_to_throttle_pid.get_ff();
// constrain and return final output // 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 // set PID's dt
_sailboat_heel_pid.set_dt(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 // get feed-forward
const float ff = _sailboat_heel_pid.get_ff(); 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 bool AR_AttitudeControl::get_forward_speed(float &speed) const
{ {
Vector3f velocity; Vector3f velocity;
const AP_AHRS &_ahrs = AP::ahrs();
if (!_ahrs.get_velocity_NED(velocity)) { if (!_ahrs.get_velocity_NED(velocity)) {
// use less accurate GPS, assuming entire length is along forward/back axis of vehicle // use less accurate GPS, assuming entire length is along forward/back axis of vehicle
if (AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) { if (AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {

6
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -1,6 +1,5 @@
#pragma once #pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AC_PID/AC_PID.h> #include <AC_PID/AC_PID.h>
#include <AC_PID/AC_P.h> #include <AC_PID/AC_P.h>
@ -49,7 +48,7 @@ class AR_AttitudeControl {
public: public:
// constructor // constructor
AR_AttitudeControl(AP_AHRS &ahrs); AR_AttitudeControl();
// //
// steering controller // steering controller
@ -157,9 +156,6 @@ public:
private: private:
// external references
const AP_AHRS &_ahrs;
// parameters // parameters
AC_P _steer_angle_p; // steering angle controller AC_P _steer_angle_p; // steering angle controller
AC_PID _steer_rate_pid; // steering rate controller AC_PID _steer_rate_pid; // steering rate controller