mirror of https://github.com/ArduPilot/ardupilot
APMRover2: Steering fix style
This commit is contained in:
Pierre Kancir
Grant Morphett
parent
363574244b
commit
e0a3caea13
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue