From a7d66fdad69eee4f3b71b5f25e5d893d5ba96275 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 9 Apr 2018 21:12:07 +0900 Subject: [PATCH] Rover: integrate vectored thrust support --- APMrover2/mode.cpp | 14 ++++++++++++-- APMrover2/mode_acro.cpp | 1 + APMrover2/mode_guided.cpp | 7 ++++++- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 4d955eb514..b4bc209b38 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -354,7 +354,12 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse lat_accel = constrain_float(lat_accel, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS); // send final steering command to motor library - const float steering_out = attitude_control.get_steering_out_lat_accel(lat_accel, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); + const float steering_out = attitude_control.get_steering_out_lat_accel(lat_accel, + g2.motors.have_skid_steering(), + g2.motors.have_vectored_thrust(), + g2.motors.limit.steer_left, + g2.motors.limit.steer_right, + reversed); g2.motors.set_steering(steering_out * 4500.0f); } @@ -362,7 +367,12 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse void Mode::calc_steering_to_heading(float desired_heading_cd, bool reversed) { // calculate yaw error (in radians) and pass to steering angle controller - const float steering_out = attitude_control.get_steering_out_heading(radians(desired_heading_cd*0.01f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); + const float steering_out = attitude_control.get_steering_out_heading(radians(desired_heading_cd*0.01f), + g2.motors.have_skid_steering(), + g2.motors.have_vectored_thrust(), + g2.motors.limit.steer_left, + g2.motors.limit.steer_right, + reversed); g2.motors.set_steering(steering_out * 4500.0f); } diff --git a/APMrover2/mode_acro.cpp b/APMrover2/mode_acro.cpp index 95e33949e5..61ce6cc70b 100644 --- a/APMrover2/mode_acro.cpp +++ b/APMrover2/mode_acro.cpp @@ -19,6 +19,7 @@ void ModeAcro::update() const float steering_out = attitude_control.get_steering_out_rate( target_turn_rate, g2.motors.have_skid_steering(), + g2.motors.have_vectored_thrust(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index 2e37e81f0d..9352ff457c 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -65,7 +65,12 @@ void ModeGuided::update() } if (have_attitude_target) { // run steering and throttle controllers - float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0); + float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), + g2.motors.have_skid_steering(), + g2.motors.have_vectored_thrust(), + g2.motors.limit.steer_left, + g2.motors.limit.steer_right, + _desired_speed < 0); g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true, true); } else {