|
|
|
@ -118,7 +118,7 @@ void Rover::calc_throttle(float target_speed) {
@@ -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) {
@@ -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) {
@@ -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() {
@@ -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)
@@ -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) {
@@ -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.
|
|
|
|
|