diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index f8ed4a3ce5..e9f844ba4b 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -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 { diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index b3615f76c9..2a6bc583af 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -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