From 106b8abab771209dfed77dce28e09e45b7bb9957 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 8 Aug 2018 14:42:09 +0900 Subject: [PATCH] AR_AttitudeControl: get_throttle_out_from_pitch accepts vehicle speed --- libraries/APM_Control/AR_AttitudeControl.cpp | 23 +++++++++++++++----- libraries/APM_Control/AR_AttitudeControl.h | 10 +++++---- 2 files changed, 23 insertions(+), 10 deletions(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 3f7fa87c1d..0e0cc973f0 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -210,7 +210,15 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { // @Units: Hz // @User: Standard AP_SUBGROUPINFO(_pitch_to_throttle_pid, "_BAL_", 10, AR_AttitudeControl, AC_PID), - + + // @Param: _BAL_SPD_FF + // @DisplayName: Pitch control feed forward from speed + // @Description: Pitch control feed forward from speed + // @Range: 0.0 10.0 + // @Increment: 0.01 + // @User: Standard + AP_GROUPINFO("_BAL_SPD_FF", 11, AR_AttitudeControl, _pitch_to_throttle_speed_ff, AR_ATTCONTROL_BAL_SPEED_FF), + AP_GROUPEND }; @@ -484,10 +492,10 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor return get_throttle_out_speed(desired_speed_limited, motor_limit_low, motor_limit_high, cruise_speed, cruise_throttle, dt); } -// for balancebot -// return a throttle output from -1 to +1, given a desired pitch angle -// desired_pitch is in radians -float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool motor_limit_low, bool motor_limit_high, float dt) +// balancebot pitch to throttle controller +// returns a throttle output from -100 to +100 given a desired pitch angle and vehicle's current speed (from wheel encoders) +// desired_pitch is in radians, veh_speed_pct is supplied as a percentage (-100 to +100) of vehicle's top speed +float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float vehicle_speed_pct, bool motor_limit_low, bool motor_limit_high, float dt) { // sanity check dt dt = constrain_float(dt, 0.0f, 1.0f); @@ -527,8 +535,11 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, bool // get d const float d = _pitch_to_throttle_pid.get_d(); + // add feed forward from speed + const float spd_ff = vehicle_speed_pct * 0.01f * _pitch_to_throttle_speed_ff; + // constrain and return final output - return (ff + p + i + d); + return (ff + p + i + d + spd_ff); } // get latest desired pitch in radians for reporting purposes diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index 8097f32d5f..748150da89 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -25,6 +25,7 @@ #define AR_ATTCONTROL_PITCH_THR_D 0.03f #define AR_ATTCONTROL_PITCH_THR_IMAX 1.0f #define AR_ATTCONTROL_PITCH_THR_FILT 10.0f +#define AR_ATTCONTROL_BAL_SPEED_FF 1.0f #define AR_ATTCONTROL_DT 0.02f #define AR_ATTCONTROL_TIMEOUT_MS 200 @@ -89,10 +90,10 @@ public: // return a throttle output from -1 to +1 to perform a controlled stop. stopped is set to true once stop has been completed float get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt, bool &stopped); - // for balancebot - // return a throttle output from -1 to +1 given a desired pitch angle - // desired_pitch is in radians - float get_throttle_out_from_pitch(float desired_pitch, bool motor_limit_low, bool motor_limit_high, float dt); + // balancebot pitch to throttle controller + // returns a throttle output from -100 to +100 given a desired pitch angle and vehicle's current speed (from wheel encoders) + // desired_pitch is in radians, veh_speed_pct is supplied as a percentage (-100 to +100) of vehicle's top speed + float get_throttle_out_from_pitch(float desired_pitch, float veh_speed_pct, bool motor_limit_low, bool motor_limit_high, float dt); // get latest desired pitch in radians for reporting purposes float get_desired_pitch() const; @@ -137,6 +138,7 @@ private: AC_PID _steer_rate_pid; // steering rate controller AC_PID _throttle_speed_pid; // throttle speed controller AC_PID _pitch_to_throttle_pid;// balancebot pitch controller + AP_Float _pitch_to_throttle_speed_ff; // balancebot feed forward from speed AP_Float _throttle_accel_max; // speed/throttle control acceleration (and deceleration) maximum in m/s/s. 0 to disable limits AP_Float _throttle_decel_max; // speed/throttle control deceleration maximum in m/s/s. 0 to use ATC_ACCEL_MAX for deceleration