APMrover v2.20 - use RangeFinder optical SharpGP2Y instead of ultrasonic sonar

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

APMrover2/APM_Config.h

@@ -8,9 +8,9 @@
 //#define SERIAL3_BAUD        38400
 //#define GCS_PROTOCOL        GCS_PROTOCOL_NONE
 
-#define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
+#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
 
-#define LITE  DISABLED    // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329
+#define LITE  ENABLED    // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329
                           // if LITE is DISABLED, this is for a full APM v1 (Oilpan + GPS MT3329 + Magnetometer HMC5883L) or APM v2
 
 #define CLI_ENABLED         ENABLED
@@ -18,7 +18,7 @@
 #define CLOSED_LOOP_NAV     ENABLED
 #define AUTO_WP_RADIUS      DISABLED
 
-#define TRACE               DISABLED
+#define TRACE               ENABLED
 
 //#include "APM_Config_HILmode.h"  // for test in HIL mode with AeroSIM Rc 3.83
 #include "APM_Config_Rover.h"      // to be used with the real Traxxas model Monster Jam Grinder

APMrover2/APM_Config_Rover.h

@@ -16,17 +16,19 @@
 #define SONAR_TRIGGER       200        // trigger distance in cm 
 
 #if LITE == DISABLED
-  #define MAGNETOMETER	    ENABLED
   #define LOGGING_ENABLED   ENABLED
-
-  #define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_UP_PINS_FORWARD
-  #define PARAM_DECLINATION       0.18  // Paris
 #endif
+
+// for an accurate navigation a magnetometer must be used with the APM1 
+#define MAGNETOMETER	    ENABLED
+//#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_UP_PINS_FORWARD
+//#define PARAM_DECLINATION       0.18  // Paris
+
 //////////////////////////////////////////////////////////////////////////////
 // Serial port speeds.
 //
 #define SERIAL0_BAUD        115200
-#define SERIAL3_BAUD        115200
+#define SERIAL3_BAUD        57600
 
 //////////////////////////////////////////////////////////////////////////////
 // GPS_PROTOCOL 
@@ -59,7 +61,7 @@ the when the rover approach the wp, it slow down to 4 m/s (TRIM_ARSPD_CM)...
 This feature works only if the ROV_AWPR_NAV is set to 0
 */
 
-#define BOOSTER              2    // booster factor x2
+#define BOOSTER              1    // booster factor x1 = 1 or x2 = 2
 #define AUTO_WP_RADIUS       DISABLED
 #define AIRSPEED_CRUISE      4    // 4m/s
 #define THROTTLE_SLEW_LIMIT  2    // set to 2 for a smooth acceleration by 0.2 step

APMrover2/APMrover2.pde

@@ -1,31 +1,26 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "APMrover v2.16a JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + SONAR
+#define THISFIRMWARE "APMrover v2.20 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) 
 
+// This is the APMrover firmware derived from the Arduplane v2.32 by Jean-Louis Naudin (JLN) 
 /*
 Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin
 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
 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-20
 //
+// JLN updates: last update 2012-06-13
 // DOLIST:
-//  
-//
 //-------------------------------------------------------------------------------------------------------------------------
 // Dev Startup : 2012-04-21
 //
+//  2012-06-13: use RangeFinder optical SharpGP2Y instead of ultrasonic sonar
+//  2012-05-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)
@@ -294,12 +289,16 @@ GCS_MAVLINK	gcs3;
 //
 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);
+*/
+    AP_AnalogSource_Arduino sonar_analog_source(A0);   // use AN0 analog pin for APM2 on left
+    AP_RangeFinder_SharpGP2Y sonar(&sonar_analog_source, &sonar_mode_filter);
 #endif
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -817,13 +816,13 @@ static void fast_loop()
 
 #if LITE == DISABLED
 	ahrs.update();
+#endif 
 	// Read Sonar
 	// ----------
-    # if CONFIG_SONAR == ENABLED
+#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
@@ -884,19 +883,23 @@ static void medium_loop()
                 calc_gndspeed_undershoot();
             }
             
-#if LITE == DISABLED
+//#if LITE == DISABLED
 			#if HIL_MODE != HIL_MODE_ATTITUDE
             if (g.compass_enabled && compass.read()) {
                 ahrs.set_compass(&compass);
                 // Calculate heading
+#if LITE == DISABLED                
                 Matrix3f m = ahrs.get_dcm_matrix();              
                 compass.calculate(m);
+#else
+                compass.calculate(0,0);  // roll = 0, pitch = 0
+#endif
                 compass.null_offsets();
             } else {
                 ahrs.set_compass(NULL);
             }
 			#endif
-#endif
+//#endif
 /*{
 Serial.print(ahrs.roll_sensor, DEC);	Serial.printf_P(PSTR("\t"));
 Serial.print(ahrs.pitch_sensor, DEC);	Serial.printf_P(PSTR("\t"));
@@ -1027,8 +1030,11 @@ static void slow_loop()
 #endif
 
 #if TRACE == ENABLED
-              Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"), 
+         //     Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"), 
-                    (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);                                
+         //           (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);                
 #endif                          
 			break;
 	}
@@ -1077,12 +1083,12 @@ static void update_GPS(void)
 				} else if (ENABLE_AIR_START == 0) {
 					init_home();
 				}
-#if LITE == DISABLED
+//#if LITE == DISABLED
 				if (g.compass_enabled) {
 					// Set compass declination automatically
 					compass.set_initial_location(g_gps->latitude, g_gps->longitude);
 				}
-#endif
+//#endif
 				ground_start_count = 0;
 			}
 		}
@@ -1099,7 +1105,12 @@ static void update_GPS(void)
                   ground_course = ahrs.yaw_sensor;
                 } else {
 #endif                
-                  ground_course = g_gps->ground_course;
+    long magheading;
+    magheading = ToDeg(compass.heading) * 100;
+    if (magheading > 36000)	  magheading -= 36000;
+    if (magheading < 0)           magheading += 36000;
+
+    ground_course = magheading;
 #if LITE == DISABLED                  
                 }
 #endif

APMrover2/Attitude.pde

@@ -76,34 +76,6 @@ static void calc_nav_roll()
 
 }
 
-/*****************************************
- * Roll servo slew limit
- *****************************************/
-/*
-float roll_slew_limit(float servo)
-{
-	static float last;
-	float temp = constrain(servo, last-ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f);
-	last = servo;
-	return temp;
-}*/
-
-/*****************************************
- * Throttle slew limit
- *****************************************/
-static void throttle_slew_limit()
-{
-  /*
-	static int last = 1000;
-	if(g.throttle_slewrate) {		// if slew limit rate is set to zero then do not slew limit
-
-		float temp = g.throttle_slewrate * G_Dt * 10.f;		//  * 10 to scale % to pwm range of 1000 to 2000
-		g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
-		last = g.channel_throttle.radio_out;
-	}
-*/
-}
-
 // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
 // Keeps outdated data out of our calculations
 static void reset_I(void)
@@ -152,9 +124,6 @@ static void set_servos(void)
         if (control_mode >= FLY_BY_WIRE_B) {
           // convert 0 to 100% into PWM
             g.channel_throttle.calc_pwm();
-            /* only do throttle slew limiting in modes where throttle
-               control is automatic */
-            throttle_slew_limit();
         }
 
 

APMrover2/Parameters.h

@@ -83,7 +83,7 @@ public:
 	k_param_pack_capacity,
         k_param_airspeed_offset,
 #if HIL_MODE != HIL_MODE_ATTITUDE
-#if LITE == DISABLED     
+#if CONFIG_SONAR == ENABLED       
  	k_param_sonar_enabled,
  	k_param_sonar_type,
 #endif
@@ -334,7 +334,7 @@ public:
 	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
-#if LITE == DISABLED   
+#if CONFIG_SONAR == ENABLED     
         AP_Int8	    sonar_enabled;
 	AP_Int8	    sonar_type;   // 0 = XL, 1 = LV, 2 = XLL (XL with 10m range)   
 #endif
@@ -471,7 +471,7 @@ public:
         pack_capacity	 		(HIGH_DISCHARGE),
         inverted_flight_ch      (0),
 #if HIL_MODE != HIL_MODE_ATTITUDE
-#if LITE == DISABLED          
+#if CONFIG_SONAR == ENABLED            
         sonar_enabled			(SONAR_ENABLED),
 	sonar_type			(AP_RANGEFINDER_MAXSONARXL),        
 #endif

APMrover2/Parameters.pde

@@ -105,7 +105,7 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	GSCALAR(pack_capacity,          "BATT_CAPACITY"),
 	GSCALAR(inverted_flight_ch,     "INVERTEDFLT_CH"),
 #if HIL_MODE != HIL_MODE_ATTITUDE
-#if LITE == DISABLED   
+#if CONFIG_SONAR == ENABLED     
 	// @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

APMrover2/sensors.pde

@@ -1,21 +1,23 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-#if LITE == DISABLED
-// Sensors are not available in HIL_MODE_ATTITUDE
-#if HIL_MODE != HIL_MODE_ATTITUDE
-
-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
 
+#if LITE == DISABLED
+// Sensors are not available in HIL_MODE_ATTITUDE
+#if HIL_MODE != HIL_MODE_ATTITUDE
+
+void ReadSCP1000(void) {}
+
 static void init_barometer(void)
 {
 	int flashcount = 0;

APMrover2/system.pde

@@ -195,11 +195,45 @@ static void init_ardupilot()
             compass.null_offsets_enable();
         }
 	}
+#else
+  I2c.begin();
+  I2c.timeOut(20);
+
+  // I2c.setSpeed(true);
+
+  if (!compass.init()) {
+	  Serial.println("compass initialisation failed!");
+	  while (1) ;
+  }
+
+  compass.set_orientation(MAG_ORIENTATION);  // set compass's orientation on aircraft.
+  compass.set_offsets(0,0,0);  // set offsets to account for surrounding interference
+  compass.set_declination(ToRad(0.0));  // set local difference between magnetic north and true north
+
+  Serial.print("Compass auto-detected as: ");
+  switch( compass.product_id ) {
+      case AP_COMPASS_TYPE_HIL:
+	      Serial.println("HIL");
+		  break;
+      case AP_COMPASS_TYPE_HMC5843:
+	      Serial.println("HMC5843");
+		  break;
+      case AP_COMPASS_TYPE_HMC5883L:
+	      Serial.println("HMC5883L");
+		  break;
+      default:
+	      Serial.println("unknown");
+		  break;
+  }
+  
+  delay(3000);
+
+#endif
 	// initialise sonar
 	#if CONFIG_SONAR == ENABLED
 	init_sonar();
 	#endif
-#endif
+
 #endif
 	// Do GPS init
 	g_gps = &g_gps_driver;

APMrover2/test.pde

@@ -18,7 +18,9 @@ static int8_t	test_relay(uint8_t argc,	 	const Menu::arg *argv);
 static int8_t	test_wp(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_airspeed(uint8_t argc, 	const Menu::arg *argv);
 static int8_t	test_pressure(uint8_t argc, 	const Menu::arg *argv);
-static int8_t	test_vario(uint8_t argc, 	const Menu::arg *argv);
+#if CONFIG_SONAR == ENABLED
+static int8_t	test_sonar(uint8_t argc, 	const Menu::arg *argv);
+#endif
 static int8_t	test_mag(uint8_t argc, 			const Menu::arg *argv);
 static int8_t	test_xbee(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_eedump(uint8_t argc, 		const Menu::arg *argv);
@@ -59,7 +61,9 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
 	{"imu",			test_imu},
 	{"airspeed",	test_airspeed},
 	{"airpressure",	test_pressure},
-	{"vario",	test_vario},
+#if CONFIG_SONAR == ENABLED
+	{"sonartest",	test_sonar},
+#endif
 	{"compass",		test_mag},
 #elif HIL_MODE == HIL_MODE_SENSORS
 	{"adc", 		test_adc},
@@ -662,12 +666,6 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
 
 }
 
-static int8_t
-test_vario(uint8_t argc, const Menu::arg *argv)
-{
-
-}
-
 static int8_t
 test_rawgps(uint8_t argc, const Menu::arg *argv)
 {
@@ -690,6 +688,30 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
 		}
   }
 }
+
+#if CONFIG_SONAR == ENABLED
+static int8_t
+test_sonar(uint8_t argc, const Menu::arg *argv)
+{
+  print_hit_enter();
+	delay(1000);
+	init_sonar();
+	delay(1000);
+
+	while(1){
+	  delay(20);
+	  if(g.sonar_enabled){
+		sonar_dist = sonar.read();
+	  }
+    	  Serial.printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist);
+
+          if(Serial.available() > 0){
+  		break;
+	    }
+  }
+  return (0);
+}
+#endif // SONAR == ENABLED
 #endif // HIL_MODE == HIL_MODE_DISABLED
 
 #endif // CLI_ENABLED