APMRover2: Steering fix style

This commit is contained in:
Pierre Kancir 2017-01-16 10:18:55 +01:00 committed by Grant Morphett
parent 363574244b
commit e0a3caea13
1 changed files with 32 additions and 32 deletions

View File

@ -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;
}