Rover: steering mark floats as float and fix parenthesis
This commit is contained in:
parent
97595f98ac
commit
ee28e49790
@ -118,7 +118,7 @@ void Rover::calc_throttle(float target_speed) {
|
||||
// use g.speed_turn_gain for a 90 degree turn, and in proportion
|
||||
// for other turn angles
|
||||
const int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
|
||||
const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1);
|
||||
const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0.0f, 1.0f);
|
||||
const float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
|
||||
|
||||
float reduction = 1.0f - steer_rate * speed_turn_reduction;
|
||||
@ -136,7 +136,7 @@ void Rover::calc_throttle(float target_speed) {
|
||||
|
||||
groundspeed_error = fabsf(target_speed) - ground_speed;
|
||||
|
||||
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
|
||||
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100.0f) / 100.0f);
|
||||
|
||||
// also reduce the throttle by the reduction factor. This gives a
|
||||
// much faster response in turns
|
||||
@ -156,7 +156,7 @@ void Rover::calc_throttle(float target_speed) {
|
||||
// We use a linear gain, with 0 gain at a ground speed error
|
||||
// of braking_speederr, and 100% gain when groundspeed_error
|
||||
// is 2*braking_speederr
|
||||
const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
|
||||
const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
|
||||
const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
|
||||
|
||||
@ -238,8 +238,8 @@ void Rover::calc_nav_steer() {
|
||||
*/
|
||||
void Rover::mix_skid_steering(void)
|
||||
{
|
||||
const float steering_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_steering) / 4500.0;
|
||||
const float throttle_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0;
|
||||
const float steering_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_steering) / 4500.0f;
|
||||
const float throttle_scaled = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f;
|
||||
float motor1 = throttle_scaled + 0.5f * steering_scaled;
|
||||
float motor2 = throttle_scaled - 0.5f * steering_scaled;
|
||||
|
||||
@ -250,8 +250,8 @@ void Rover::mix_skid_steering(void)
|
||||
}
|
||||
|
||||
// first new-style skid steering
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 1000 * motor1);
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000 * motor2);
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 1000.0f * motor1);
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 1000.0f * motor2);
|
||||
|
||||
// now old style skid steering with skid_steer_out
|
||||
if (g.skid_steer_out) {
|
||||
@ -326,7 +326,7 @@ void Rover::set_servos(void) {
|
||||
|
||||
if (have_skid_steering()) {
|
||||
mix_skid_steering();
|
||||
}
|
||||
}
|
||||
|
||||
if (!arming.is_armed()) {
|
||||
// Some ESCs get noisy (beep error msgs) if PWM == 0.
|
||||
|
Loading…
Reference in New Issue
Block a user