APMrover v2.1.3 :

Added option (hold roll to full right + SW7 ON/OFF) to init_home during the wp_list reset
Add ROV_SONAR_TRIG (default = 200 cm) - tested by Franco
Restart_nav() added and heading bug correction, tested OK in the field
RTL then stop update - Tested OK in the field
Added SONAR detection for obstacle avoidance

Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>

APMrover2/APM_Config.h

@@ -16,7 +16,7 @@
 #define CLI_ENABLED         ENABLED
 #define CLI_SLIDER_ENABLED  DISABLED
 #define CLOSED_LOOP_NAV     ENABLED
-#define AUTO_WP_RADIUS      ENABLED
+#define AUTO_WP_RADIUS      DISABLED
 
 #define TRACE               DISABLED
 

APMrover2/APM_Config_HILmode.h

@@ -33,8 +33,6 @@
 #define MAGNETOMETER	    DISABLED
 #define LOGGING_ENABLED	    DISABLED
 
-#define CH7_OPTION	    CH7_CH7_SAVE_WP
-
 #define TURN_GAIN		5
 
 #define CH7_OPTION	    CH7_SAVE_WP
@@ -422,8 +420,8 @@
 // Also, set the value of USE_CURRENT_ALT in meters.  This is mainly intended to allow
 // users to start using the APM without running the WaypointWriter first.
 //
-#define WP_RADIUS_DEFAULT       3   // meters
+#define WP_RADIUS_DEFAULT       1   // meters
-#define LOITER_RADIUS_DEFAULT	10   // 60
+#define LOITER_RADIUS_DEFAULT	5   
 #define USE_CURRENT_ALT		TRUE
 #define ALT_HOLD_HOME		0
 

APMrover2/APM_Config_Rover.h

@@ -12,9 +12,12 @@
 
 #define AIRSPEED_SENSOR	    DISABLED
 
+#define SONAR_ENABLED       DISABLED
+#define SONAR_TRIGGER       200        // trigger distance in cm 
+
 #if LITE == DISABLED
   #define MAGNETOMETER	    ENABLED
-  #define LOGGING_ENABLED	    ENABLED
+  #define LOGGING_ENABLED   ENABLED
 
   #define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_UP_PINS_FORWARD
   #define PARAM_DECLINATION       0.18  // Paris
@@ -46,7 +49,7 @@
 #define TURN_GAIN		5
 
 #define CLOSED_LOOP_NAV       ENABLED     // set to ENABLED if closed loop navigation else set to DISABLED (Return To Lauch)
-#define AUTO_WP_RADIUS        ENABLED
+#define AUTO_WP_RADIUS        DISABLED
 
 #define MAX_DIST             50  //300       // max distance (in m) for the HEADALT mode
 #define SARSEC_BRANCH        50              // Long branch of the SARSEC pattern
@@ -418,8 +421,8 @@
 // Also, set the value of USE_CURRENT_ALT in meters.  This is mainly intended to allow
 // users to start using the APM without running the WaypointWriter first.
 //
-#define WP_RADIUS_DEFAULT       5   // meters
+#define WP_RADIUS_DEFAULT       1   // meters
-#define LOITER_RADIUS_DEFAULT	10   // 60
+#define LOITER_RADIUS_DEFAULT	5   // 60
 #define USE_CURRENT_ALT		TRUE
 #define ALT_HOLD_HOME		0
 

APMrover2/APMrover2.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "APMrover v2.0c JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer
+#define THISFIRMWARE "APMrover v2.13 JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + SONAR
 
 // This is a full version of Arduplane v2.32 specially adapted for a Rover by Jean-Louis Naudin (JLN) 
 
@@ -9,6 +9,8 @@ Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell,
 Thanks to:  Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier 
 Please contribute your ideas!
 
+APMrover alpha version tester: Franco Borasio, Daniel Chapelat... 
+
 
 This firmware is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
@@ -16,7 +18,7 @@ License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.
 
 //
-// JLN updates: last update 2012-05-04
+// JLN updates: last update 2012-05-14
 //
 // DOLIST:
 //  
@@ -24,6 +26,12 @@ version 2.1 of the License, or (at your option) any later version.
 //-------------------------------------------------------------------------------------------------------------------------
 // Dev Startup : 2012-04-21
 //
+//  2012-05-14: Added option (hold roll to full right + SW7 ON/OFF) to init_home during the wp_list reset
+//  2012-05-13: Add ROV_SONAR_TRIG (default = 200 cm)
+//  2012-05-13: Restart_nav() added and heading bug correction, tested OK in the field
+//  2012-05-12: RTL then stop update - Tested in the field
+//  2012-05-11: The rover now STOP after the RTL... (special update for Franco...)
+//  2012-05-11: Added SONAR detection for obstacle avoidance (alpha version for SONAR testing)
 //  2012-05-04: Added #define LITE ENABLED  for the APM1280 or APM2560 CPU IMUless version
 //  2012-05-03: Successful missions tests with a full APM2560 kit (GPS MT3329 + magnetometer HMC5883L)
 //  2012-05-03: removing stick mixing in auto mode
@@ -272,10 +280,23 @@ GCS_MAVLINK	gcs0;
 GCS_MAVLINK	gcs3;
 
 ////////////////////////////////////////////////////////////////////////////////
-// PITOT selection
+// SONAR selection
 ////////////////////////////////////////////////////////////////////////////////
 //
 ModeFilterInt16_Size5 sonar_mode_filter(2);
+#if CONFIG_SONAR == ENABLED
+	#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
+	AP_AnalogSource_ADC sonar_analog_source( &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
+	#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
+		AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN);
+	#endif
+	AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// PITOT selection
+////////////////////////////////////////////////////////////////////////////////
+//
 
 #if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC
 AP_AnalogSource_ADC pitot_analog_source( &adc,
@@ -284,14 +305,6 @@ AP_AnalogSource_ADC pitot_analog_source( &adc,
 AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0);
 #endif
 
-#if SONAR_TYPE == MAX_SONAR_XL
-	AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter);
-#elif SONAR_TYPE == MAX_SONAR_LV
-	// XXX honestly I think these output the same values
-	// If someone knows, can they confirm it?
-	AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter);
-#endif
-
 // Barometer filter
 AverageFilterInt32_Size5 baro_filter;	// filtered pitch acceleration
 
@@ -444,6 +457,7 @@ static float	nav_gain_scaler 		= 1;
 // Direction held during phases of takeoff and landing
 // deg * 100 dir of plane,  A value of -1 indicates the course has not been set/is not in use
 static long    hold_course       	 	= -1;		// deg * 100 dir of plane
+static bool rtl_complete = false;
 
 // There may be two active commands in Auto mode.  
 // This indicates the active navigation command by index number
@@ -475,7 +489,9 @@ static long		airspeed_energy_error;
 static int		airspeed_nudge;
 // Similar to airspeed_nudge, but used when no airspeed sensor.
 // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
-static int     throttle_nudge = 0;                 
+static int     throttle_nudge = 0;
+// The distance as reported by Sonar in cm – Values are 20 to 700 generally.
+static int16_t		sonar_dist;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Ground speed
@@ -538,8 +554,7 @@ static int16_t 	ground_temperature;
 ////////////////////////////////////////////////////////////////////////////////
 // Raw absolute pressure measurement (filtered).  ADC units
 static unsigned long 	abs_pressure;
-// Altitude from the sonar sensor.  Meters.  Not yet implemented.
+
-static int		sonar_alt;
 // The altitude as reported by Baro in cm – Values can be quite high
 static int32_t		baro_alt;
 
@@ -791,6 +806,13 @@ static void fast_loop()
 
 #if LITE == DISABLED
 	ahrs.update();
+	// Read Sonar
+	// ----------
+    # if CONFIG_SONAR == ENABLED
+	if(g.sonar_enabled){
+		sonar_dist = sonar.read();
+	}
+    #endif
 #endif 
 
 	// uses the yaw from the DCM to give more accurate turns
@@ -818,7 +840,7 @@ static void fast_loop()
 
 	// apply desired roll, pitch and yaw to the plane
 	// ----------------------------------------------
-	if (control_mode > MANUAL)
+	if (control_mode > STABILIZE)
 		stabilize();
 
 	// write out the servo PWM values
@@ -1070,9 +1092,10 @@ static void update_GPS(void)
                 if (g.compass_enabled) {
                   hdg=(ahrs.yaw_sensor / 100) % 360;
                   ground_course = hdg * 100;
+                  ground_course = ahrs.yaw_sensor;
                 } else {
 #endif            
-                  ground_course = abs(g_gps->ground_course);
+                  ground_course = g_gps->ground_course;
 #if LITE == DISABLED                  
                 }
 #endif
@@ -1094,7 +1117,7 @@ static void update_current_flight_mode(void)
 				calc_nav_roll();
 				calc_throttle();
 				break;
-		}
+            }
 	}else{
 		switch(control_mode){
 			case RTL:
@@ -1112,6 +1135,13 @@ static void update_current_flight_mode(void)
 			case STABILIZE:
 				nav_roll        = 0;
 				nav_pitch       = 0;
+                       #if X_PLANE == ENABLED
+  				// servo_out is for Sim control only
+				// ---------------------------------
+				g.channel_roll.servo_out = g.channel_roll.pwm_to_angle();
+				g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
+				g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle();                     
+                       #endif
 				// throttle is passthrough
 				break;
 
@@ -1132,7 +1162,7 @@ static void update_current_flight_mode(void)
 				// ---------------------------------
 				g.channel_roll.servo_out = g.channel_roll.pwm_to_angle();
 				g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
-				g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle();
+				g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle();
 				break;
 				//roll: -13788.000,  pitch: -13698.000,   thr: 0.000, rud: -13742.000
 
@@ -1152,10 +1182,15 @@ static void update_navigation()
 
 		switch(control_mode){
 			case LOITER:
-			case RTL:
+			case RTL:        // no loitering around the wp with the rover, goes direct to the wp position
 			case GUIDED:
-				update_loiter();
+				// update_loiter();
+				calc_nav_roll();
 				calc_bearing_error();
+                                if(verify_RTL()) 
+                                {  g.channel_throttle.servo_out = g.throttle_min.get();
+                                   set_mode(MANUAL);
+                                }
 				break;
 
 		}

APMrover2/GCS_Mavlink.pde

@@ -452,7 +452,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
         chan,
         (float)airspeed / 100.0,
         (float)g_gps->ground_speed / 100.0,
-        (ahrs.yaw_sensor / 100) % 360,
+        ground_course,
         (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100
         current_loc.alt / 100.0,
         0);

APMrover2/Log.pde

@@ -351,7 +351,7 @@ static void Log_Write_GPS(	int32_t log_Time, int32_t log_Lattitude, int32_t log_
 	DataFlash.WriteByte(log_NumSats);
 	DataFlash.WriteLong(log_Lattitude);
 	DataFlash.WriteLong(log_Longitude);
-	DataFlash.WriteInt(sonar_alt);				// This one is just temporary for testing out sonar in fixed wing
+	DataFlash.WriteInt(sonar_dist);				// This one is just temporary for testing out sonar in fixed wing
 	DataFlash.WriteLong(log_mix_alt);
 	DataFlash.WriteLong(log_gps_alt);
 	DataFlash.WriteLong(log_Ground_Speed);

APMrover2/Parameters.h

@@ -82,8 +82,12 @@ public:
 	k_param_input_voltage,
 	k_param_pack_capacity,
         k_param_airspeed_offset,
-  
+#if HIL_MODE != HIL_MODE_ATTITUDE
-//	k_param_sonar_enabled,
+#if LITE == DISABLED     
+ 	k_param_sonar_enabled,
+ 	k_param_sonar_type,
+#endif
+#endif
 	k_param_airspeed_enabled,
         
         //
@@ -127,7 +131,7 @@ public:
         
         k_param_closed_loop_nav,
         k_param_auto_wp_radius,
-        k_param_nudgeoffset,
+        k_param_sonar_trigger,
         k_param_turn_gain,
         
 // ************************************************************
@@ -328,8 +332,12 @@ public:
         AP_Float    input_voltage;
 	AP_Int16    pack_capacity;		// Battery pack capacity less reserve
         AP_Int8	    inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
-
+#if HIL_MODE != HIL_MODE_ATTITUDE
-       // AP_Int8	    sonar_enabled;
+#if LITE == DISABLED   
+        AP_Int8	    sonar_enabled;
+	AP_Int8	    sonar_type;   // 0 = XL, 1 = LV, 2 = XLL (XL with 10m range)   
+#endif
+#endif
         AP_Int8	    airspeed_enabled;
         AP_Int8	    flap_1_percent;
         AP_Int8	    flap_1_speed;
@@ -341,7 +349,7 @@ public:
 
         AP_Int8     closed_loop_nav;
         AP_Int8     auto_wp_radius;
-        AP_Int16    nudgeoffset;
+        AP_Int16    sonar_trigger;
         AP_Int16    turn_gain;
     
 // ************************************************************   
@@ -460,8 +468,12 @@ public:
         input_voltage			(INPUT_VOLTAGE),
         pack_capacity	 		(HIGH_DISCHARGE),
         inverted_flight_ch      (0),
-       
+#if HIL_MODE != HIL_MODE_ATTITUDE
-//        sonar_enabled			(SONAR_ENABLED),
+#if LITE == DISABLED          
+        sonar_enabled			(SONAR_ENABLED),
+	sonar_type			(AP_RANGEFINDER_MAXSONARXL),        
+#endif
+#endif
         airspeed_enabled		(AIRSPEED_SENSOR),
         
 // ************ APMrover parameters  ************************
@@ -469,7 +481,7 @@ public:
 
         closed_loop_nav         (CLOSED_LOOP_NAV),
         auto_wp_radius          (AUTO_WP_RADIUS), 
-        nudgeoffset             (NUDGE_OFFSET),
+        sonar_trigger           (SONAR_TRIGGER),
         turn_gain               (TURN_GAIN), 
 
 // ************************************************************   

APMrover2/Parameters.pde

@@ -104,8 +104,17 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	GSCALAR(input_voltage,          "INPUT_VOLTS"),
 	GSCALAR(pack_capacity,          "BATT_CAPACITY"),
 	GSCALAR(inverted_flight_ch,     "INVERTEDFLT_CH"),
-
+#if HIL_MODE != HIL_MODE_ATTITUDE
-	//GSCALAR(sonar_enabled,          "SONAR_ENABLE"),
+#if LITE == DISABLED   
+	// @Param: SONAR_ENABLE
+	// @DisplayName: Enable Sonar
+	// @Description: Setting this to Enabled(1) will enable the sonar. Setting this to Disabled(0) will disable the sonar
+	// @Values: 0:Disabled,1:Enabled
+	// @User: Standard
+	GSCALAR(sonar_enabled,	        "SONAR_ENABLE"),
+	GSCALAR(sonar_type,	        "SONAR_TYPE"),
+#endif	
+#endif
 	GSCALAR(airspeed_enabled,       "ARSPD_ENABLE"),
 
  // ************************************************************
@@ -113,7 +122,7 @@ static const AP_Param::Info var_info[] PROGMEM = {
         
         GSCALAR(closed_loop_nav,        "ROV_CL_NAV"),
         GSCALAR(auto_wp_radius,         "ROV_AWPR_NAV"),
-        GSCALAR(nudgeoffset,            "ROV_NUDGE"),
+        GSCALAR(sonar_trigger,          "ROV_SONAR_TRIG"),
         GSCALAR(turn_gain,              "ROV_GAIN"),
 // ************************************************************
 

APMrover2/commands.pde

@@ -12,6 +12,7 @@
 	void set_next_WP(struct Location *wp)
 	void set_guided_WP(void)
 	void init_home()
+	void restart_nav()
 ************************************************************ 
 */
 
@@ -250,14 +251,14 @@ void init_home()
         home.alt        = g_gps->altitude;;
   					// Home is always 0
 #else
-	struct Location temp = get_cmd_with_index(0);    // JLN update - for HIL test only get the home param stored in the FPL
+//	struct Location temp = get_cmd_with_index(0);    // JLN update - for HIL test only get the home param stored in the FPL
-        if (temp.alt > 0) {
+//        if (temp.alt > 0) {
-        	home.lng 	= temp.lng;			 // Lon * 10**7
+//        	home.lng 	= temp.lng;			 // Lon * 10**7
-        	home.lat 	= temp.lat;			 // Lat * 10**7
+//        	home.lat 	= temp.lat;			 // Lat * 10**7
-        } else {
+//        } else {
         	home.lng 	= g_gps->longitude;		 // Lon * 10**7
         	home.lat 	= g_gps->latitude;		 // Lat * 10**7       
-        }
+//        }
         
          gps_base_alt    = g_gps->altitude;;             // get the stored home altitude as the base ref for AGL calculation.
          home.alt        = g_gps->altitude;;
@@ -282,3 +283,11 @@ void init_home()
 
 }
 
+static void restart_nav()
+{  
+  reset_I();
+  prev_WP = current_loc;
+  nav_command_ID = NO_COMMAND;
+  nav_command_index = 0;
+  process_next_command();
+}

APMrover2/commands_logic.pde

@@ -331,6 +331,7 @@ static bool verify_RTL()
 {
 	if (wp_distance <= g.waypoint_radius) {
 		gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
+                rtl_complete = true;
 		return true;
 	}else{
 		return false;

APMrover2/commands_process.pde

@@ -68,9 +68,9 @@ static void process_next_command()
 			// we are out of commands!
 			gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
 			
-                    if (g.closed_loop_nav)  {    // JLN update - replay the FPL (CLOSED_LOOP_NAV)
+                      if (g.closed_loop_nav)  {    // JLN update - replay the FPL (CLOSED_LOOP_NAV)
-                       change_command(1);
+                         restart_nav();
-                       return;
+                         return;
                     } else {
                      handle_no_commands();
                     }    
@@ -144,3 +144,5 @@ static void process_non_nav_command()
 		non_nav_command_ID = NO_COMMAND;
 	}
 }
+
+

APMrover2/config.h

@@ -50,9 +50,17 @@
 # define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
 #endif
 
+#ifndef X_PLANE
+# define X_PLANE DISABLED
+#endif
+
+#if X_PLANE == ENABLED
+# define LITE ENABLED
+#else
 #ifndef LITE
 # define LITE DISABLED
 #endif
+#endif
 
 #if defined( __AVR_ATmega1280__ )
 #define CLI_ENABLED DISABLED
@@ -181,8 +189,41 @@
 # define SONAR_TYPE             MAX_SONAR_LV	// MAX_SONAR_XL,  
 #endif
 
-#ifndef SONAR_ENABLED
+//////////////////////////////////////////////////////////////////////////////
-#define SONAR_ENABLED DISABLED
+// Sonar
+//
+
+#ifndef CONFIG_SONAR_SOURCE
+# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ADC
+#endif
+
+#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC && CONFIG_ADC == DISABLED
+# warning Cannot use ADC for CONFIG_SONAR_SOURCE, becaude CONFIG_ADC is DISABLED
+# warning Defaulting CONFIG_SONAR_SOURCE to ANALOG_PIN
+# undef CONFIG_SONAR_SOURCE
+# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
+#endif
+
+#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
+# ifndef CONFIG_SONAR_SOURCE_ADC_CHANNEL
+#  define CONFIG_SONAR_SOURCE_ADC_CHANNEL 7
+# endif
+#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
+# ifndef CONFIG_SONAR_SOURCE_ANALOG_PIN
+#  define CONFIG_SONAR_SOURCE_ANALOG_PIN A0
+# endif
+#else
+# warning Invalid value for CONFIG_SONAR_SOURCE, disabling sonar
+# undef SONAR_ENABLED
+# define SONAR_ENABLED DISABLED
+#endif
+
+#ifndef CONFIG_SONAR
+# define CONFIG_SONAR ENABLED
+#endif
+
+#ifndef SONAR_TRIGGER
+# define SONAR_TRIGGER       60        // trigger distance in cm 
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -195,6 +236,9 @@
 #if HIL_MODE != HIL_MODE_DISABLED	// we are in HIL mode
  # undef GPS_PROTOCOL
  # define GPS_PROTOCOL GPS_PROTOCOL_NONE
+ 
+  #undef CONFIG_SONAR
+ #define CONFIG_SONAR DISABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

APMrover2/control_modes.pde

@@ -70,6 +70,8 @@ if (g.ch7_option == CH7_SAVE_WP){         // set to 1
                                         g.command_total = 0;
                                         g.command_index =0;
                                         nav_command_index = 0;
+                                        if(g.channel_roll.control_in > 3000) // if roll is full right store the current location as home
+                                            ground_start_count = 5;
                                         #if X_PLANE == ENABLED
                                                 Serial.printf_P(PSTR("*** RESET the FPL\n"));
                                         #endif

APMrover2/defines.h

@@ -30,6 +30,9 @@
 #define SONAR 0
 #define BARO 1
 
+#define SONAR_SOURCE_ADC 1
+#define SONAR_SOURCE_ANALOG_PIN 2
+
 // CH 7 control
 #define CH7_DO_NOTHING   0
 #define CH7_SAVE_WP      1

APMrover2/navigation.pde

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

APMrover2/radio.pde

@@ -103,12 +103,12 @@ static void read_radio()
 	if (g.channel_throttle.servo_out > 50) {
 		if(g.airspeed_enabled == true) {
 			airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
-                        airspeed_nudge = g.nudgeoffset + airspeed_nudge;
+                        airspeed_nudge = NUDGE_OFFSET + airspeed_nudge;
         } else {
 			throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
 		}
 	} else {
-		airspeed_nudge = g.nudgeoffset;
+		airspeed_nudge = NUDGE_OFFSET;
 		throttle_nudge = 0;
 	}
 

APMrover2/sensors.pde

@@ -5,6 +5,17 @@
 
 void ReadSCP1000(void) {}
 
+#if CONFIG_SONAR == ENABLED
+static void init_sonar(void)
+{
+    #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
+	    sonar.calculate_scaler(g.sonar_type, 3.3);
+	#else
+        sonar.calculate_scaler(g.sonar_type, 5.0);
+	#endif
+}
+#endif
+
 static void init_barometer(void)
 {
 	int flashcount = 0;

APMrover2/system.pde

@@ -195,6 +195,10 @@ static void init_ardupilot()
             compass.null_offsets_enable();
         }
 	}
+	// initialise sonar
+	#if CONFIG_SONAR == ENABLED
+	init_sonar();
+	#endif
 #endif
 #endif
 	// Do GPS init
@@ -358,8 +362,8 @@ static void set_mode(byte mode)
 			break;
 
 		case AUTO:
-                        change_command(1); // restart to the 1st stored wp
+                        rtl_complete = false;
-			//update_auto();
+                        restart_nav();
 			break;
 
 		case RTL: