Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: add singleton and get_srate

This commit is contained in:
Randy Mackay 2023-05-26 13:40:39 +09:00 committed by Andrew Tridgell
parent 3784841eaa
commit 68e1769cc1
2 changed files with 21 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -63,6 +63,8 @@
extern const AP_HAL::HAL& hal;
AR_AttitudeControl *AR_AttitudeControl::_singleton;
const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
// @Param: _STR_RAT_P
@ -471,7 +473,8 @@ AR_AttitudeControl::AR_AttitudeControl() :
_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),
_pitch_to_throttle_pid(AR_ATTCONTROL_PITCH_THR_P, AR_ATTCONTROL_PITCH_THR_I, AR_ATTCONTROL_PITCH_THR_D, 0.0f, AR_ATTCONTROL_PITCH_THR_IMAX, 0.0f, AR_ATTCONTROL_PITCH_THR_FILT, 0.0f),
_sailboat_heel_pid(AR_ATTCONTROL_HEEL_SAIL_P, AR_ATTCONTROL_HEEL_SAIL_I, AR_ATTCONTROL_HEEL_SAIL_D, 0.0f, AR_ATTCONTROL_HEEL_SAIL_IMAX, 0.0f, AR_ATTCONTROL_HEEL_SAIL_FILT, 0.0f)
{
{
_singleton = this;
AP_Param::setup_object_defaults(this, var_info);
}
@ -862,6 +865,13 @@ float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt)
return (ff + p + i + d) * -1.0f;
}
// get the slew rate value for speed and steering for oscillation detection in lua scripts
void AR_AttitudeControl::get_srate(float &steering_srate, float &speed_srate)
{
steering_srate = get_steering_rate_pid().get_pid_info().slew_rate;
speed_srate = _throttle_speed_pid_info.slew_rate;
}
// get forward speed in m/s (earth-frame horizontal velocity but only along vehicle x-axis). returns true on success
bool AR_AttitudeControl::get_forward_speed(float &speed) const
{

10
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -10,6 +10,11 @@ public:
// constructor
AR_AttitudeControl();
// do not allow copying
CLASS_NO_COPY(AR_AttitudeControl);
static AR_AttitudeControl *get_singleton() { return _singleton; }
//
// steering controller
//
@ -95,6 +100,9 @@ public:
AC_PID& get_sailboat_heel_pid() { return _sailboat_heel_pid; }
const AP_PIDInfo& get_throttle_speed_pid_info() const { return _throttle_speed_pid_info; }
// get the slew rate value for speed and steering for oscillation detection in lua scripts
void get_srate(float &steering_srate, float &speed_srate);
// get forward speed in m/s (earth-frame horizontal velocity but only along vehicle x-axis). returns true on success
bool get_forward_speed(float &speed) const;
@ -127,6 +135,8 @@ public:
private:
static AR_AttitudeControl *_singleton;
// parameters
AC_P _steer_angle_p; // steering angle controller
AC_PID _steer_rate_pid; // steering rate controller