Rover: fixed EKF usage for rover

added barometer object, and use shared home object from AHRS. This
gives basic EKF operation.

APMrover2/APMrover2.pde

@@ -406,7 +406,6 @@ static uint8_t 	ground_start_count	= 5;
 // on the ground or in the air.  Used to decide if a ground start is appropriate if we
 // booted with an air start.
 static int16_t     ground_start_avg;
-static int32_t          gps_base_alt;		
 
 ////////////////////////////////////////////////////////////////////////////////
 // Location & Navigation
@@ -510,7 +509,7 @@ static int16_t 		condition_rate;
 // Location structure defined in AP_Common
 ////////////////////////////////////////////////////////////////////////////////
 // The home location used for RTL.  The location is set when we first get stable GPS lock
-static struct 	Location home;
+static const struct	Location &home = ahrs.get_home();
 // Flag for if we have g_gps lock and have set the home location
 static bool	home_is_set;
 // The location of the previous waypoint.  Used for track following and altitude ramp calculations
@@ -572,6 +571,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { set_servos,             1,   1500 },
     { update_GPS_50Hz,        1,   2500 },
     { update_GPS_10Hz,        5,   2500 },
+    { update_alt,             5,   3400 },
     { navigate,               5,   1600 },
     { update_compass,         5,   2000 },
     { update_commands,        5,   1000 },
@@ -684,6 +684,14 @@ static void mount_update(void)
 #endif
 }
 
+static void update_alt()
+{
+    barometer.read();
+    if (should_log(MASK_LOG_IMU)) {
+        Log_Write_Baro();
+    }
+}
+
 /*
   check for GCS failsafe - 10Hz
  */

APMrover2/Log.pde

@@ -519,6 +519,11 @@ static void Log_Write_RC(void)
     DataFlash.Log_Write_RCOUT();
 }
 
+static void Log_Write_Baro(void)
+{
+    DataFlash.Log_Write_Baro(barometer);
+}
+
 static const struct LogStructure log_structure[] PROGMEM = {
     LOG_COMMON_STRUCTURES,
     { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       

APMrover2/Parameters.h

@@ -168,6 +168,7 @@ public:
         k_param_rcmap,
         k_param_L1_controller,
         k_param_steerController,
+        k_param_barometer,
 
         // 254,255: reserved
         };

APMrover2/Parameters.pde

@@ -427,6 +427,12 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
     GOBJECT(scheduler, "SCHED_", AP_Scheduler),
 
+    // barometer ground calibration. The GND_ prefix is chosen for
+    // compatibility with previous releases of ArduPlane
+    // @Group: GND_
+    // @Path: ../libraries/AP_Baro/AP_Baro.cpp
+    GOBJECT(barometer, "GND_", AP_Baro),
+
     // @Group: RELAY_
     // @Path: ../libraries/AP_Relay/AP_Relay.cpp
     GOBJECT(relay,                  "RELAY_", AP_Relay),

APMrover2/commands.pde

@@ -151,12 +151,7 @@ void init_home()
 
 	gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
 
-	home.id 	= MAV_CMD_NAV_WAYPOINT;
+    ahrs.set_home(g_gps->latitude, g_gps->longitude, g_gps->altitude_cm);
-
-	home.lng 	= g_gps->longitude;				// Lon * 10**7
-	home.lat 	= g_gps->latitude;				// Lat * 10**7
-    gps_base_alt    = max(g_gps->altitude_cm, 0);
-    home.alt        = g_gps->altitude_cm;
 	home_is_set = true;
 
 	// Save Home to EEPROM - Command 0

APMrover2/commands_logic.pde

@@ -360,10 +360,7 @@ static void do_set_home()
 	if(next_nonnav_command.p1 == 1 && have_position) {
 		init_home();
 	} else {
-		home.id 	= MAV_CMD_NAV_WAYPOINT;
+        ahrs.set_home(next_nonnav_command.lat, next_nonnav_command.lng, next_nonnav_command.alt);
-		home.lng 	= next_nonnav_command.lng;				// Lon * 10**7
-		home.lat 	= next_nonnav_command.lat;				// Lat * 10**7
-		home.alt 	= max(next_nonnav_command.alt, 0);
 		home_is_set = true;
 	}
 }

APMrover2/config.h

@@ -60,31 +60,42 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
 # define CONFIG_INS_TYPE   CONFIG_INS_OILPAN
 # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+# define CONFIG_BARO     AP_BARO_BMP085
 # define BATTERY_PIN_1	  0
 # define CURRENT_PIN_1	  1
 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
 # define CONFIG_INS_TYPE   CONFIG_INS_MPU6000
 # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+# ifdef APM2_BETA_HARDWARE
+#  define CONFIG_BARO     AP_BARO_BMP085
+# else // APM2 Production Hardware (default)
+#  define CONFIG_BARO          AP_BARO_MS5611
+#  define CONFIG_MS5611_SERIAL AP_BARO_MS5611_SPI
+# endif
 # define BATTERY_PIN_1	  1
 # define CURRENT_PIN_1	  2
 #elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 # define CONFIG_INS_TYPE CONFIG_INS_HIL
 # define CONFIG_COMPASS  AP_COMPASS_HIL
+# define CONFIG_BARO     AP_BARO_HIL
 # define BATTERY_PIN_1	  1
 # define CURRENT_PIN_1	  2
 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
 # define CONFIG_INS_TYPE   CONFIG_INS_PX4
 # define CONFIG_COMPASS  AP_COMPASS_PX4
+# define CONFIG_BARO AP_BARO_PX4
 # define BATTERY_PIN_1	  -1
 # define CURRENT_PIN_1	  -1
 #elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
 # define CONFIG_INS_TYPE   CONFIG_INS_FLYMAPLE
 # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+# define CONFIG_BARO AP_BARO_BMP085
 # define BATTERY_PIN_1     20
 # define CURRENT_PIN_1	   19
 #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 # define CONFIG_INS_TYPE   CONFIG_INS_L3G4200D
 # define CONFIG_COMPASS  AP_COMPASS_HMC5843
+# define CONFIG_BARO     AP_BARO_BMP085
 # define BATTERY_PIN_1     -1
 # define CURRENT_PIN_1	   -1
 #endif

APMrover2/defines.h

@@ -173,6 +173,12 @@ enum mode {
 #define CONFIG_INS_FLYMAPLE 5
 #define CONFIG_INS_L3G4200D 6
 
+// barometer driver types
+#define AP_BARO_BMP085   1
+#define AP_BARO_MS5611   2
+#define AP_BARO_PX4      3
+#define AP_BARO_HIL      4
+
 // compass driver types
 #define AP_COMPASS_HMC5843   1
 #define AP_COMPASS_PX4       2

APMrover2/sensors.pde

@@ -1,5 +1,12 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+static void init_barometer(void)
+{
+    gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));    
+    barometer.calibrate();
+    gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
+}
+
 static void init_sonar(void)
 {
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1

APMrover2/system.pde

@@ -117,6 +117,9 @@ static void init_ardupilot()
     // used to detect in-flight resets
     g.num_resets.set_and_save(g.num_resets+1);
 
+    // init baro before we start the GCS, so that the CLI baro test works
+    barometer.init();
+
 	// init the GCS
 	gcs[0].init(hal.uartA);
 
@@ -174,6 +177,9 @@ static void init_ardupilot()
 	// initialise sonar
     init_sonar();
 
+    // and baro for EKF
+    init_barometer();
+
 	// Do GPS init
 	g_gps = &g_gps_driver;
     // GPS initialisation