void ModeSteering::update()
{
float desired_steering, desired_throttle;
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);
// get speed forward
float speed;
if (!attitude_control.get_forward_speed(speed)) {
// no valid speed so stop
g2.motors.set_throttle(0.0f);
g2.motors.set_steering(0.0f);
_desired_lat_accel = 0.0f;
return;
}
// determine if pilot is requesting pivot turn
bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (!is_zero(desired_steering));
// In steering mode we control lateral acceleration directly.
// For pivot steering vehicles we use the TURN_MAX_G parameter
// For regular steering vehicles we use the maximum lateral acceleration at full steering lock for this speed: V^2/R where R is the radius of turn.
float max_g_force;
if (is_pivot_turning) {
max_g_force = g.turn_max_g * GRAVITY_MSS;
} else {
max_g_force = speed * speed / MAX(g2.turn_radius, 0.1f);
}
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
// convert pilot steering input to desired lateral acceleration
_desired_lat_accel = max_g_force * (desired_steering / 4500.0f);
// reverse target lateral acceleration if backing up
bool reversed = false;
if (is_negative(target_speed)) {
reversed = true;
_desired_lat_accel = -_desired_lat_accel;
}
// mark us as in_reverse when using a negative throttle
rover.set_reverse(reversed);
// 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(false);
calc_throttle(target_speed, false);
}
}
void ModeManual::update()
{
float desired_steering, desired_throttle;
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// copy RC scaled inputs to outputs
g2.motors.set_throttle(desired_throttle);
g2.motors.set_steering(desired_steering);
// mark us as in_reverse when using a negative throttle to stop AHRS getting off
rover.set_reverse(is_negative(g2.motors.get_throttle()));
}
void ModeManual::update()
{
float desired_steering, desired_throttle, desired_lateral;
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
get_pilot_desired_lateral(desired_lateral);
// if vehicle is balance bot, calculate actual throttle required for balancing
if (rover.is_balancebot()) {
rover.balancebot_pitch_control(desired_throttle);
}
// copy RC scaled inputs to outputs
g2.motors.set_throttle(desired_throttle);
g2.motors.set_steering(desired_steering, false);
g2.motors.set_lateral(desired_lateral);
}
void ModeAcro::update()
{
// get speed forward
float speed, desired_steering;
if (!attitude_control.get_forward_speed(speed)) {
float desired_throttle;
// convert pilot stick input into desired steering and throttle
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// no valid speed, just use the provided throttle
g2.motors.set_throttle(desired_throttle);
} else {
float desired_speed;
// convert pilot stick input into desired steering and speed
get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
calc_throttle(desired_speed, true);
}
float steering_out;
// handle sailboats
if (!is_zero(desired_steering)) {
// steering input return control to user
rover.sailboat_clear_tack();
}
if (g2.motors.has_sail() && rover.sailboat_tacking()) {
// call heading controller during tacking
steering_out = attitude_control.get_steering_out_heading(rover.sailboat_get_tack_heading_rad(),
g2.wp_nav.get_pivot_rate(),
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
} else {
// 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);
// run steering turn rate controller and throttle controller
steering_out = attitude_control.get_steering_out_rate(target_turn_rate,
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
}
set_steering(steering_out * 4500.0f);
}
