diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp index 6e64514014..9cf414831a 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -22,8 +22,6 @@ #include #include -extern const AP_HAL::HAL& hal; - #define AR_POSCON_TIMEOUT_MS 100 // timeout after 0.1 sec #define AR_POSCON_POS_P 0.2f // default position P gain #define AR_POSCON_VEL_P 1.0f // default velocity P gain @@ -35,6 +33,10 @@ extern const AP_HAL::HAL& hal; #define AR_POSCON_VEL_FILT_D 5.0f // default velocity D term filter #define AR_POSCON_DT 0.02f // default dt for PID controllers +extern const AP_HAL::HAL& hal; + +AR_PosControl *AR_PosControl::_singleton; + const AP_Param::GroupInfo AR_PosControl::var_info[] = { // @Param: _POS_P @@ -103,6 +105,7 @@ AR_PosControl::AR_PosControl(AR_AttitudeControl& atc) : _p_pos(AR_POSCON_POS_P), _pid_vel(AR_POSCON_VEL_P, AR_POSCON_VEL_I, AR_POSCON_VEL_D, AR_POSCON_VEL_FF, AR_POSCON_VEL_IMAX, AR_POSCON_VEL_FILT, AR_POSCON_VEL_FILT_D) { + _singleton = this; AP_Param::setup_object_defaults(this, var_info); } @@ -349,6 +352,13 @@ Vector2p AR_PosControl::get_pos_error() const return (_pos_target - curr_pos_NE.topostype()); } +// get the slew rate value for velocity. used for oscillation detection in lua scripts +void AR_PosControl::get_srate(float &velocity_srate) +{ + // slew rate is the same for x and y axis + velocity_srate = _pid_vel.get_pid_info_x().slew_rate; +} + // write PSC logs void AR_PosControl::write_log() { diff --git a/libraries/APM_Control/AR_PosControl.h b/libraries/APM_Control/AR_PosControl.h index d06d852fc4..40748fce30 100644 --- a/libraries/APM_Control/AR_PosControl.h +++ b/libraries/APM_Control/AR_PosControl.h @@ -11,6 +11,11 @@ public: // constructor AR_PosControl(AR_AttitudeControl& atc); + // do not allow copying + CLASS_NO_COPY(AR_PosControl); + + static AR_PosControl *get_singleton() { return _singleton; } + // update navigation void update(float dt); @@ -73,6 +78,9 @@ public: AC_P_2D& get_pos_p() { return _p_pos; } AC_PID_2D& get_vel_pid() { return _pid_vel; } + // get the slew rate value for velocity. used for oscillation detection in lua scripts + void get_srate(float &velocity_srate); + // write PSC logs void write_log(); @@ -81,6 +89,8 @@ public: private: + static AR_PosControl *_singleton; + // initialise and check for ekf position resets void init_ekf_xy_reset(); void handle_ekf_xy_reset();