diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index f40b925e69..579066ee32 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -14,7 +14,7 @@ void Rover::throttle_slew_limit(int16_t last_throttle) { } uint16_t pwm; if (SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, pwm)) { - SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,constrain_int16(pwm, last_throttle - temp, last_throttle + temp)); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, constrain_int16(pwm, last_throttle - temp, last_throttle + temp)); } } } @@ -103,10 +103,10 @@ void Rover::calc_throttle(float target_speed) { // If not autostarting OR we are loitering at a waypoint // then set the throttle to minimum if (!auto_check_trigger() || in_stationary_loiter()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get()); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get()); // Stop rotation in case of loitering and skid steering if (g.skid_steer_out) { - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); } return; } @@ -127,7 +127,7 @@ void Rover::calc_throttle(float target_speed) { float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1); float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f; - float reduction = 1.0f - steer_rate*speed_turn_reduction; + float reduction = 1.0f - steer_rate * speed_turn_reduction; if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) { // in auto-modes we reduce speed when approaching waypoints @@ -149,9 +149,9 @@ void Rover::calc_throttle(float target_speed) { throttle *= reduction; if (in_reverse) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(-throttle, -g.throttle_max, -g.throttle_min)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-throttle, -g.throttle_max, -g.throttle_min)); } else { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(throttle, g.throttle_min, g.throttle_max)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(throttle, g.throttle_min, g.throttle_max)); } if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) { @@ -164,7 +164,7 @@ void Rover::calc_throttle(float target_speed) { // is 2*braking_speederr float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1); 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)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min)); // temporarily set us in reverse to allow the PWM setting to // go negative @@ -209,9 +209,9 @@ void Rover::calc_lateral_acceleration() { if (use_pivot_steering()) { int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100; if (bearing_error > 0) { - lateral_acceleration = g.turn_max_g*GRAVITY_MSS; + lateral_acceleration = g.turn_max_g * GRAVITY_MSS; } else { - lateral_acceleration = -g.turn_max_g*GRAVITY_MSS; + lateral_acceleration = -g.turn_max_g * GRAVITY_MSS; } } } @@ -222,7 +222,7 @@ void Rover::calc_lateral_acceleration() { void Rover::calc_nav_steer() { // check to see if the rover is loitering if (in_stationary_loiter()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); return; } @@ -232,9 +232,9 @@ void Rover::calc_nav_steer() { } // constrain to max G force - lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS); + lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,steerController.get_steering_out_lat_accel(lateral_acceleration)); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steerController.get_steering_out_lat_accel(lateral_acceleration)); } /***************************************** @@ -245,41 +245,41 @@ void Rover::set_servos(void) { if (control_mode == MANUAL || control_mode == LEARNING) { // do a direct pass through of radio values - SRV_Channels::set_output_pwm(SRV_Channel::k_steering,channel_steer->read()); - SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->read()); + SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->read()); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->read()); if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) { // suppress throttle if in failsafe and manual - SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->get_radio_trim()); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->get_radio_trim()); // suppress steer if in failsafe and manual and skid steer mode if (g.skid_steer_out) { - SRV_Channels::set_output_pwm(SRV_Channel::k_steering,channel_steer->get_radio_trim()); + SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->get_radio_trim()); } } } else { if (in_reverse) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), -g.throttle_max, -g.throttle_min)); } else { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), g.throttle_min.get(), g.throttle_max.get())); } if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) { // suppress throttle if in failsafe - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); // suppress steer if in failsafe and skid steer mode if (g.skid_steer_out) { - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); } } if (!hal.util->get_soft_armed()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); // suppress steer if in failsafe and skid steer mode if (g.skid_steer_out) { - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); } } @@ -299,12 +299,12 @@ void Rover::set_servos(void) { motor1 = throttle + 0.5*steering motor2 = throttle - 0.5*steering */ - float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering); - float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle); - float motor1 = throttle_scaled + 0.5f*steering_scaled; - float motor2 = throttle_scaled - 0.5f*steering_scaled; - SRV_Channels::set_output_scaled(SRV_Channel::k_steering,4500*motor1); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,100*motor2); + const float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering); + const float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle); + float motor1 = throttle_scaled + 0.5f * steering_scaled; + float motor2 = throttle_scaled - 0.5f * steering_scaled; + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 4500 * motor1); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100 * motor2); } if (!arming.is_armed()) { @@ -317,17 +317,17 @@ void Rover::set_servos(void) { break; case AP_Arming::YES_ZERO_PWM: - SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,0); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, 0); if (g.skid_steer_out) { - SRV_Channels::set_output_pwm(SRV_Channel::k_steering,0); + SRV_Channels::set_output_pwm(SRV_Channel::k_steering, 0); } break; case AP_Arming::YES_MIN_PWM: default: - SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->get_radio_trim()); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->get_radio_trim()); if (g.skid_steer_out) { - SRV_Channels::set_output_pwm(SRV_Channel::k_steering,channel_steer->get_radio_trim()); + SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->get_radio_trim()); } break; }