Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: reduce throttle more directly

Browse Source 
this gives better throttle control in turns and waypoints
This commit is contained in:
Andrew Tridgell 2013-03-02 11:03:15 +11:00
parent 476a6d0164
commit fc0b50dd82
2 changed files with 35 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/APMrover2.pde
View File

@ -348,7 +348,7 @@ static int32_t target_bearing;
// deg * 100 : 0 to 360
static int32_t crosstrack_bearing;
// A gain scaler to account for ground speed/headwind/tailwind
static float nav_gain_scaler = 1;
static float nav_gain_scaler = 1.0f;
static bool rtl_complete = false;
// There may be two active commands in Auto mode.

59
APMrover2/Steering.pde
View File

@ -17,38 +17,47 @@ static void throttle_slew_limit(int16_t last_throttle)
}
}
/*
calculate the throtte for auto-throttle modes
*/
static void calc_throttle(float target_speed)
{
if (target_speed <= 0) {
// cope with zero requested speed
g.channel_throttle.servo_out = g.throttle_min.get();
return;
}
if (target_speed <= 0) {
// cope with zero requested speed
g.channel_throttle.servo_out = g.throttle_min.get();
return;
}
int throttle_target = g.throttle_cruise + throttle_nudge;
int throttle_target = g.throttle_cruise + throttle_nudge;
/*
reduce target speed in proportion to turning rate, up to the
SPEED_TURN_GAIN percentage.
/*
reduce target speed in proportion to turning rate, up to the
SPEED_TURN_GAIN percentage.
*/
float steer_rate = fabsf(nav_steer / (float)SERVO_MAX);
steer_rate = constrain(steer_rate, 0.0, 1.0);
float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
float steer_rate = fabsf((nav_steer/nav_gain_scaler) / (float)SERVO_MAX);
steer_rate = constrain(steer_rate, 0.0, 1.0);
float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
// in auto-modes we reduce speed when approaching waypoints
float reduction2 = 1.0 - (100-g.speed_turn_gain)*0.01*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist);
if (reduction2 < reduction) {
reduction = reduction2;
}
}
// reduce the target speed by the reduction factor
target_speed *= reduction;
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
// in auto-modes we reduce speed when approaching waypoints
float reduction2 = 1.0 - (100-g.speed_turn_gain)*0.01*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist);
if (reduction2 < reduction) {
reduction = reduction2;
}
}
groundspeed_error = target_speed - ground_speed;
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
target_speed *= reduction;
groundspeed_error = target_speed - ground_speed;
throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
// also reduce the throttle by the reduction factor. This gives a
// much faster response in turns
throttle *= reduction;
g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
}
/*****************************************