Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
APMrover2/Steering.cpp
View File

@ -14,7 +14,7 @@ void Rover::throttle_slew_limit(int16_t last_throttle) {
} }
uint16_t pwm; uint16_t pwm;
if (SRV_Channels::get_output_pwm(SRV_Channel::k_throttle, 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 // If not autostarting OR we are loitering at a waypoint
// then set the throttle to minimum // then set the throttle to minimum
if (!auto_check_trigger() || in_stationary_loiter()) { 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 // Stop rotation in case of loitering and skid steering
if (g.skid_steer_out) { 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; 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_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 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) { if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
// in auto-modes we reduce speed when approaching waypoints // in auto-modes we reduce speed when approaching waypoints
@ -149,9 +149,9 @@ void Rover::calc_throttle(float target_speed) {
throttle *= reduction; throttle *= reduction;
if (in_reverse) { 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 { } 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) { 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 // is 2*braking_speederr
float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1); 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; 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 // temporarily set us in reverse to allow the PWM setting to
// go negative // go negative
@ -209,9 +209,9 @@ void Rover::calc_lateral_acceleration() {
if (use_pivot_steering()) { if (use_pivot_steering()) {
int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100; int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
if (bearing_error > 0) { if (bearing_error > 0) {
lateral_acceleration = g.turn_max_g*GRAVITY_MSS; lateral_acceleration = g.turn_max_g * GRAVITY_MSS;
} else { } 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() { void Rover::calc_nav_steer() {
// check to see if the rover is loitering // check to see if the rover is loitering
if (in_stationary_loiter()) { 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; return;
} }
@ -232,9 +232,9 @@ void Rover::calc_nav_steer() {
} }
// constrain to max G force // 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) { if (control_mode == MANUAL || control_mode == LEARNING) {
// do a direct pass through of radio values // 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_steering, channel_steer->read());
SRV_Channels::set_output_pwm(SRV_Channel::k_throttle,channel_throttle->read()); SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->read());
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) { if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
// suppress throttle if in failsafe and manual // 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 // suppress steer if in failsafe and manual and skid steer mode
if (g.skid_steer_out) { 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 { } else {
if (in_reverse) { 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_max,
-g.throttle_min)); -g.throttle_min));
} else { } 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_min.get(),
g.throttle_max.get())); g.throttle_max.get()));
} }
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) { if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// suppress throttle if in failsafe // 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 // suppress steer if in failsafe and skid steer mode
if (g.skid_steer_out) { 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()) { 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 // suppress steer if in failsafe and skid steer mode
if (g.skid_steer_out) { 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 motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering motor2 = throttle - 0.5*steering
*/ */
float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering); const float steering_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_steering);
float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle); const float throttle_scaled = SRV_Channels::get_output_norm(SRV_Channel::k_throttle);
float motor1 = throttle_scaled + 0.5f*steering_scaled; float motor1 = throttle_scaled + 0.5f * steering_scaled;
float motor2 = 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_steering, 4500 * motor1);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,100*motor2); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100 * motor2);
} }
if (!arming.is_armed()) { if (!arming.is_armed()) {
@ -317,17 +317,17 @@ void Rover::set_servos(void) {
break; break;
case AP_Arming::YES_ZERO_PWM: 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) { 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; break;
case AP_Arming::YES_MIN_PWM: case AP_Arming::YES_MIN_PWM:
default: 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) { 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; break;
} }