From 846b12807563fe31a20ce46e098210ecb0f6fb02 Mon Sep 17 00:00:00 2001 From: Jean-Louis Naudin Date: Wed, 13 Jun 2012 20:43:35 +0200 Subject: [PATCH] APMrover 2.0a - improving obstacles detection and tuning Signed-off-by: Jean-Louis Naudin --- APMrover2/APMrover2.pde | 13 ++++++++++--- APMrover2/Attitude.pde | 9 +++++++++ APMrover2/navigation.pde | 7 ------- 3 files changed, 19 insertions(+), 10 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index f9f25b5e3a..a61fb6593b 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -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; } diff --git a/APMrover2/Attitude.pde b/APMrover2/Attitude.pde index 791dc171c0..4cd62047f0 100644 --- a/APMrover2/Attitude.pde +++ b/APMrover2/Attitude.pde @@ -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; diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index 202e55a0e9..27257d9699 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -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); }