void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
{
// add in speed nudging
if (nudge_allowed) {
target_speed = calc_speed_nudge(target_speed, g.speed_cruise, g.throttle_cruise * 0.01f);
}
// get acceleration limited target speed
target_speed = attitude_control.get_desired_speed_accel_limited(target_speed, rover.G_Dt);
// apply object avoidance to desired speed using half vehicle's maximum deceleration
if (avoidance_enabled) {
g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_decel_max(), ahrs.yaw, target_speed, rover.G_Dt);
}
// call throttle controller and convert output to -100 to +100 range
float throttle_out;
// call speed or stop controller
if (is_zero(target_speed)) {
bool stopped;
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, rover.G_Dt, stopped);
} else {
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, rover.G_Dt);
}
// if vehicle is balance bot, calculate actual throttle required for balancing
if (rover.is_balancebot()) {
rover.balancebot_pitch_control(throttle_out, rover.arming.is_armed());
}
// send to motor
g2.motors.set_throttle(throttle_out);
}
void Mode::calc_throttle(float target_speed, bool nudge_allowed)
{
// add in speed nudging
if (nudge_allowed) {
target_speed = calc_speed_nudge(target_speed, g.speed_cruise, g.throttle_cruise * 0.01f);
}
// 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.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);
}
