From e2ba59ab8fa547bc45681eeb9f5ee8f14e4d80f9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 9 Apr 2018 21:11:54 +0900 Subject: [PATCH] AR_AttitudeControl: support vectored thrust steering output is not scaled for speed steering integrator does not build up at low speed --- libraries/APM_Control/AR_AttitudeControl.cpp | 15 ++++++++------- libraries/APM_Control/AR_AttitudeControl.h | 6 +++--- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index d201a57277..e1e8cd5881 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -173,7 +173,7 @@ AR_AttitudeControl::AR_AttitudeControl(AP_AHRS &ahrs) : // return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s. // positive lateral acceleration is to the right. -float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed) +float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool skid_steering, bool vect_thrust, bool motor_limit_left, bool motor_limit_right, bool reversed) { // record desired accel for reporting purposes _steer_lat_accel_last_ms = AP_HAL::millis(); @@ -203,11 +203,11 @@ float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool s desired_rate *= -1.0f; } - return get_steering_out_rate(desired_rate, skid_steering, motor_limit_left, motor_limit_right, reversed); + return get_steering_out_rate(desired_rate, skid_steering, vect_thrust, motor_limit_left, motor_limit_right, reversed); } // return a steering servo output from -1 to +1 given a heading in radians -float AR_AttitudeControl::get_steering_out_heading(float heading_rad, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed) +float AR_AttitudeControl::get_steering_out_heading(float heading_rad, bool skid_steering, bool vect_thrust, bool motor_limit_left, bool motor_limit_right, bool reversed) { // calculate heading error (in radians) const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw); @@ -215,12 +215,12 @@ float AR_AttitudeControl::get_steering_out_heading(float heading_rad, bool skid_ // Calculate the desired turn rate (in radians) from the angle error (also in radians) const float desired_rate = _steer_angle_p.get_p(yaw_error); - return get_steering_out_rate(desired_rate, skid_steering, motor_limit_left, motor_limit_right, reversed); + return get_steering_out_rate(desired_rate, skid_steering, vect_thrust, motor_limit_left, motor_limit_right, reversed); } // return a steering servo output from -1 to +1 given a // desired yaw rate in radians/sec. Positive yaw is to the right. -float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed) +float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_steering, bool vect_thrust, bool motor_limit_left, bool motor_limit_right, bool reversed) { // calculate dt const uint32_t now = AP_HAL::millis(); @@ -250,8 +250,9 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_st float scaler = 1.0f; bool low_speed = false; - // scaler to linearize output because turn rate increases as vehicle speed increases on non-skid steering vehicles - if (!skid_steering) { + // scaler to linearize output because turn rate increases as vehicle speed increases + // on non-skid steering and non-vectored thrust vehicles + if (!skid_steering && !vect_thrust) { // get speed forward float speed; diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index cec7dea7ca..e6f5a2d0d4 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -45,14 +45,14 @@ public: // return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s. // positive lateral acceleration is to the right. - float get_steering_out_lat_accel(float desired_accel, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed); + float get_steering_out_lat_accel(float desired_accel, bool skid_steering, bool vect_thrust, bool motor_limit_left, bool motor_limit_right, bool reversed); // return a steering servo output from -1 to +1 given a heading in radians - float get_steering_out_heading(float heading_rad, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed); + float get_steering_out_heading(float heading_rad, bool skid_steering, bool vect_thrust, bool motor_limit_left, bool motor_limit_right, bool reversed); // return a steering servo output from -1 to +1 given a // desired yaw rate in radians/sec. Positive yaw is to the right. - float get_steering_out_rate(float desired_rate, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed); + float get_steering_out_rate(float desired_rate, bool skid_steering, bool vect_thrust, bool motor_limit_left, bool motor_limit_right, bool reversed); // get latest desired turn rate in rad/sec recorded during calls to get_steering_out_rate. For reporting purposes only float get_desired_turn_rate() const;