diff --git a/APMrover2/mode_acro.cpp b/APMrover2/mode_acro.cpp index bad5a89a15..24e935b19b 100644 --- a/APMrover2/mode_acro.cpp +++ b/APMrover2/mode_acro.cpp @@ -17,7 +17,7 @@ void ModeAcro::update() get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle); // convert pilot throttle input to desired speed (up to twice the cruise speed) - const float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f); + float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f); // convert pilot steering input to desired turn rate in radians/sec const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate); @@ -35,6 +35,9 @@ void ModeAcro::update() const bool reversed = is_negative(target_speed); rover.set_reverse(reversed); + // apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration + rover.g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt); + // run steering turn rate controller and throttle controller const float steering_out = attitude_control.get_steering_out_rate(target_turn_rate, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); g2.motors.set_steering(steering_out * 4500.0f); diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index 75d83103e1..9f57850d3f 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -7,7 +7,7 @@ void ModeSteering::update() get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle); // convert pilot throttle input to desired speed (up to twice the cruise speed) - const float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f); + float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f); // get speed forward float speed; @@ -47,12 +47,17 @@ void ModeSteering::update() // mark us as in_reverse when using a negative throttle rover.set_reverse(reversed); + // apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration + rover.g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt); + // run speed to throttle output controller if (is_zero(target_speed) && !is_pivot_turning) { stop_vehicle(); } else { // run lateral acceleration to steering controller calc_steering_from_lateral_acceleration(desired_lat_accel, reversed); + + // run speed to throttle controller calc_throttle(target_speed, false); } }