|
|
|
@ -72,7 +72,7 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed)
@@ -72,7 +72,7 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// call throttle controller and convert output to -100 to +100 range
|
|
|
|
|
float throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.have_skid_steering(), g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f); |
|
|
|
|
float throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f); |
|
|
|
|
|
|
|
|
|
// send to motor
|
|
|
|
|
g2.motors.set_throttle(throttle_out); |
|
|
|
@ -83,7 +83,7 @@ bool Mode::stop_vehicle()
@@ -83,7 +83,7 @@ bool Mode::stop_vehicle()
|
|
|
|
|
{ |
|
|
|
|
// call throttle controller and convert output to -100 to +100 range
|
|
|
|
|
bool stopped = false; |
|
|
|
|
float throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.have_skid_steering(), g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, stopped); |
|
|
|
|
float throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, stopped); |
|
|
|
|
|
|
|
|
|
// send to motor
|
|
|
|
|
g2.motors.set_throttle(throttle_out); |
|
|
|
|