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 // deg * 100 : 0 to 360
static int32_t crosstrack_bearing; static int32_t crosstrack_bearing;
// A gain scaler to account for ground speed/headwind/tailwind // 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; static bool rtl_complete = false;
// There may be two active commands in Auto mode. // 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) static void calc_throttle(float target_speed)
{ {
if (target_speed <= 0) { if (target_speed <= 0) {
// cope with zero requested speed // cope with zero requested speed
g.channel_throttle.servo_out = g.throttle_min.get(); g.channel_throttle.servo_out = g.throttle_min.get();
return; 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 reduce target speed in proportion to turning rate, up to the
SPEED_TURN_GAIN percentage. SPEED_TURN_GAIN percentage.
*/ */
float steer_rate = fabsf(nav_steer / (float)SERVO_MAX); float steer_rate = fabsf((nav_steer/nav_gain_scaler) / (float)SERVO_MAX);
steer_rate = constrain(steer_rate, 0.0, 1.0); steer_rate = constrain(steer_rate, 0.0, 1.0);
float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01; 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) { groundspeed_error = target_speed - ground_speed;
// 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); throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
if (reduction2 < reduction) {
reduction = reduction2;
}
}
target_speed *= reduction; // also reduce the throttle by the reduction factor. This gives a
// much faster response in turns
groundspeed_error = target_speed - ground_speed; throttle *= reduction;
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());
g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
} }
/***************************************** /*****************************************