aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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_PitchController.h"
#include <AP_AHRS/AP_AHRS.h>
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;

5
libraries/APM_Control/AP_PitchController.h
View File

@ -1,6 +1,5 @@
#pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle.h>
#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;
};

6
libraries/APM_Control/AP_RollController.cpp
View File

@ -19,6 +19,7 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_RollController.h"
#include <AP_AHRS/AP_AHRS.h>
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;

5
libraries/APM_Control/AP_RollController.h
View File

@ -1,6 +1,5 @@
#pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle.h>
#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;
};

5
libraries/APM_Control/AP_SteerController.cpp
View File

@ -17,6 +17,7 @@
// 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_HAL/AP_HAL.h>
#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;

6
libraries/APM_Control/AP_SteerController.h
View File

@ -1,14 +1,12 @@
#pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Logger/AP_Logger.h>
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;
};

6
libraries/APM_Control/AP_YawController.cpp
View File

@ -19,6 +19,7 @@
//
#include <AP_HAL/AP_HAL.h>
#include "AP_YawController.h"
#include <AP_AHRS/AP_AHRS.h>
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;
}
}

6
libraries/APM_Control/AP_YawController.h
View File

@ -1,6 +1,5 @@
#pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Logger/AP_Logger.h>
@ -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;
};

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/>.
*/
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#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) {

6
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -1,6 +1,5 @@
#pragma once
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Common/AP_Common.h>
#include <AC_PID/AC_PID.h>
#include <AC_PID/AC_P.h>
@ -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