// calculate pilot input to nudge speed up or down
// target_speed should be in meters/sec
// cruise_speed is vehicle's cruising speed, cruise_throttle is the throttle (from -1 to +1) that achieves the cruising speed
// return value is a new speed (in m/s) which up to the projected maximum speed based on the cruise speed and cruise throttle
float Mode::calc_speed_nudge(float target_speed, float cruise_speed, float cruise_throttle)
{
// return immediately if pilot is not attempting to nudge speed
// pilot can nudge up speed if throttle (in range -100 to +100) is above 50% of center in direction of travel
const int16_t pilot_throttle = constrain_int16(rover.channel_throttle->get_control_in(), -100, 100);
if (((pilot_throttle <= 50) && (target_speed >= 0.0f)) ||
((pilot_throttle >= -50) && (target_speed <= 0.0f))) {
return target_speed;
}
// sanity checks
if (cruise_throttle > 1.0f || cruise_throttle < 0.05f) {
return target_speed;
}
// project vehicle's maximum speed
const float vehicle_speed_max = calc_speed_max(cruise_speed, cruise_throttle);
// return unadjusted target if already over vehicle's projected maximum speed
if (fabsf(target_speed) >= vehicle_speed_max) {
return target_speed;
}
const float speed_increase_max = vehicle_speed_max - fabsf(target_speed);
float speed_nudge = ((static_cast<float>(abs(pilot_throttle)) - 50.0f) * 0.02f) * speed_increase_max;
if (pilot_throttle < 0) {
speed_nudge = -speed_nudge;
}
return target_speed + speed_nudge;
}
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);
}
}
// decode pilot's input and return heading_out (in cd) and speed_out (in m/s)
void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out)
{
// get steering and throttle in the -1 to +1 range
const float desired_steering = constrain_float(rover.channel_steer->norm_input_dz(), -1.0f, 1.0f);
const float desired_throttle = constrain_float(rover.channel_throttle->norm_input_dz(), -1.0f, 1.0f);
// calculate angle of input stick vector
heading_out = wrap_360_cd(atan2f(desired_steering, desired_throttle) * DEGX100);
// calculate throttle using magnitude of input stick vector
const float throttle = MIN(safe_sqrt(sq(desired_throttle) + sq(desired_steering)), 1.0f);
speed_out = throttle * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
}
// decode pilot steering and return steering_out and speed_out (in m/s)
void Mode::get_pilot_desired_steering_and_speed(float &steering_out, float &speed_out)
{
float desired_throttle;
get_pilot_input(steering_out, desired_throttle);
speed_out = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// check for special case of input and output throttle being in opposite directions
float speed_out_limited = g2.attitude_control.get_desired_speed_accel_limited(speed_out, rover.G_Dt);
if ((is_negative(speed_out) != is_negative(speed_out_limited)) &&
((g.pilot_steer_type == PILOT_STEER_TYPE_DEFAULT) ||
(g.pilot_steer_type == PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING))) {
steering_out *= -1;
}
speed_out = speed_out_limited;
}
void ModeSteering::update()
{
// convert pilot throttle input to desired speed (up to twice the cruise speed)
float target_speed = channel_throttle->get_control_in() * 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);
lateral_acceleration = 0.0f;
return;
}
// in steering mode we control lateral acceleration directly. We first calculate the maximum lateral
// acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn.
float 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
lateral_acceleration = max_g_force * (channel_steer->get_control_in() / 4500.0f);
// reverse target lateral acceleration if backing up
bool reversed = false;
if (is_negative(target_speed)) {
reversed = true;
lateral_acceleration = -lateral_acceleration;
}
// 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)) {
stop_vehicle();
} else {
// run steering controller
calc_nav_steer(reversed);
calc_throttle(target_speed, false);
}
}
