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

APMrover 2.0a - improving obstacles detection and tuning 

Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>
This commit is contained in:
Jean-Louis Naudin 2012-06-13 20:43:35 +02:00
parent 65050775e1
commit 846b128075
3 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
APMrover2/APMrover2.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "APMrover v2.20 JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + SONAR
#define THISFIRMWARE "APMrover v2.20a JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + SONAR
// This is the APMrover firmware derived from the Arduplane v2.32 by Jean-Louis Naudin (JLN)
/*
@ -20,7 +20,7 @@ version 2.1 of the License, or (at your option) any later version.
// Dev Startup : 2012-04-21
//
// 2012-06-13: use RangeFinder optical SharpGP2Y instead of ultrasonic sonar
// 2012-05-13: added Test sonar
// 2012-06-13: added Test sonar
// 2012-05-17: added speed_boost during straight line
// 2012-05-17: New update about the throttle rate control based on the field test done by Franco Borasio (Thanks Franco..)
// 2012-05-15: The Throttle rate can be controlled by the THROTTLE_SLEW_LIMIT (the value give the step increase, 1 = 0.1)
@ -500,6 +500,7 @@ static int airspeed_nudge;
static int throttle_nudge = 0;
// The distance as reported by Sonar in cm – Values are 20 to 700 generally.
static int16_t sonar_dist;
static bool obstacle = false;
////////////////////////////////////////////////////////////////////////////////
// Ground speed
@ -822,6 +823,12 @@ static void fast_loop()
#if CONFIG_SONAR == ENABLED
if(g.sonar_enabled){
sonar_dist = sonar.read();
if(sonar_dist <= g.sonar_trigger) { // obstacle detected in front
obstacle = true;
} else {
obstacle = false;
}
}
#endif
@ -1034,7 +1041,7 @@ static void slow_loop()
// (float)ground_course/100, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);
// Serial.printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"),
// g.command_total, g.command_index, nav_command_index);
Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, sonar_dist = %d\n"), (float)ground_course/100, (int)sonar_dist);
Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, sonar_dist = %d obstacle = %d\n"), (float)ground_course/100, (int)sonar_dist, obstacle);
#endif
break;
}

9
APMrover2/Attitude.pde
View File

@ -72,6 +72,11 @@ static void calc_nav_roll()
// positive error = right turn
nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100
if(obstacle) { // obstacle avoidance
nav_roll += 9000; // if obstacle in front turn 90° right
speed_boost = false;
}
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
}
@ -106,6 +111,10 @@ static void set_servos(void)
if((control_mode == MANUAL) || (control_mode == LEARNING)){
// do a direct pass through of radio values
g.channel_roll.radio_out = g.channel_roll.radio_in;
if(obstacle) // obstacle in front, turn right in Stabilize mode
g.channel_roll.radio_out -= 500;
g.channel_pitch.radio_out = g.channel_pitch.radio_in;
g.channel_throttle.radio_out = g.channel_throttle.radio_in;

7
APMrover2/navigation.pde
View File

@ -77,13 +77,6 @@ static void calc_gndspeed_undershoot()
static void calc_bearing_error()
{
#if LITE == DISABLED
#if CONFIG_SONAR == ENABLED
if((g.sonar_enabled) && (sonar_dist < g.sonar_trigger)) {
nav_bearing += 9000; // if obstacle in front turn 90° right
}
#endif
#endif
bearing_error = nav_bearing - ground_course;
bearing_error = wrap_180(bearing_error);
}