Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: use the next navigation course to adjust steering gain in turns

Browse Source 
this should make waypoints along a straight path not reduce speed
This commit is contained in:
Andrew Tridgell 2014-04-07 09:30:39 +10:00
parent cc212be41e
commit ac7f36494b
3 changed files with 15 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
APMrover2/APMrover2.pde
View File

@ -387,6 +387,9 @@ static bool have_position;
static bool rtl_complete = false;
// angle of our next navigation waypoint
static int32_t next_navigation_leg_cd;
// ground speed error in m/s
static float groundspeed_error;
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel

11
APMrover2/Steering.pde
View File

@ -98,11 +98,18 @@ static void calc_throttle(float target_speed)
*/
float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
steer_rate = constrain_float(steer_rate, 0.0, 1.0);
float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
// use g.speed_turn_gain for a 90 degree turn, and in proportion
// for other turn angles
int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
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.0 - steer_rate*speed_turn_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);
float reduction2 = 1.0 - speed_turn_reduction*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist);
if (reduction2 < reduction) {
reduction = reduction2;
}

3
APMrover2/commands_logic.pde
View File

@ -26,6 +26,9 @@ start_command(const AP_Mission::Mission_Command& cmd)
gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id);
// remember the course of our next navigation leg
next_navigation_leg_cd = mission.get_next_ground_course_cd(0);
switch(cmd.id){
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp(cmd);