Rover: fixed PX4 MPU6000 driver for rover

APMrover2/APMrover2.pde

@@ -153,41 +153,44 @@ static GPS         *g_gps;
 // flight modes convenience array
 static AP_Int8		*modes = &g.mode1;
 
-#if HIL_MODE == HIL_MODE_DISABLED
-
-// real sensors
 #if CONFIG_ADC == ENABLED
 static AP_ADC_ADS7844 adc;
 #endif
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+#if CONFIG_COMPASS == AP_COMPASS_PX4
-static AP_Compass_HIL compass;
+static AP_Compass_PX4 compass;
-static SITL sitl;
+#elif CONFIG_COMPASS == AP_COMPASS_HMC5843
-#else
 static AP_Compass_HMC5843 compass;
+#elif CONFIG_COMPASS == AP_COMPASS_HIL
+static AP_Compass_HIL compass;
+#else
+ #error Unrecognized CONFIG_COMPASS setting
 #endif
 
-// real GPS selection
+// GPS selection
 #if   GPS_PROTOCOL == GPS_PROTOCOL_AUTO
 AP_GPS_Auto     g_gps_driver(&g_gps);
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
-AP_GPS_NMEA     g_gps_driver();
+AP_GPS_NMEA     g_gps_driver;
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
-AP_GPS_SIRF     g_gps_driver();
+AP_GPS_SIRF     g_gps_driver;
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
-AP_GPS_UBLOX    g_gps_driver();
+AP_GPS_UBLOX    g_gps_driver;
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
-AP_GPS_MTK      g_gps_driver();
+AP_GPS_MTK      g_gps_driver;
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK19
-AP_GPS_MTK19    g_gps_driver();
+AP_GPS_MTK19    g_gps_driver;
 
 #elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
-AP_GPS_None     g_gps_driver();
+AP_GPS_None     g_gps_driver;
+
+#elif GPS_PROTOCOL == GPS_PROTOCOL_HIL
+AP_GPS_HIL      g_gps_driver;
 
 #else
   #error Unrecognised GPS_PROTOCOL setting.
@@ -195,30 +198,25 @@ AP_GPS_None     g_gps_driver();
 
 #if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
 AP_InertialSensor_MPU6000 ins;
-# elif CONFIG_INS_TYPE == CONFIG_INS_SITL
+#elif CONFIG_INS_TYPE == CONFIG_INS_PX4
+AP_InertialSensor_PX4 ins;
+#elif CONFIG_INS_TYPE == CONFIG_INS_STUB
 AP_InertialSensor_Stub ins;
-#else
+#elif CONFIG_INS_TYPE == CONFIG_INS_OILPAN
 AP_InertialSensor_Oilpan ins( &adc );
+#else
+  #error Unrecognised CONFIG_INS_TYPE setting.
 #endif // CONFIG_INS_TYPE
 
-AP_AHRS_DCM  ahrs(&ins, g_gps);
+#if HIL_MODE == HIL_MODE_ATTITUDE
-
-#elif HIL_MODE == HIL_MODE_SENSORS
-// sensor emulators
-AP_ADC_HIL              adc;
-AP_Compass_HIL          compass;
-AP_GPS_HIL              g_gps_driver(NULL);
-AP_InertialSensor_Oilpan ins( &adc );
-AP_AHRS_DCM  ahrs(&ins, g_gps);
-
-#elif HIL_MODE == HIL_MODE_ATTITUDE
-AP_ADC_HIL              adc;
 AP_AHRS_HIL ahrs(&ins, g_gps);
-AP_GPS_HIL              g_gps_driver(NULL);
-AP_Compass_HIL          compass; // never used
 #else
- #error Unrecognised HIL_MODE setting.
+AP_AHRS_DCM ahrs(&ins, g_gps);
-#endif // HIL MODE
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
+SITL sitl;
+#endif
 
 ////////////////////////////////////////////////////////////////////////////////
 // GCS selection

APMrover2/config.h

@@ -65,6 +65,7 @@
 //
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
 # define CONFIG_INS_TYPE   CONFIG_INS_OILPAN
+# define CONFIG_COMPASS  AP_COMPASS_HMC5843
 # define A_LED_PIN        37
 # define B_LED_PIN        36
 # define C_LED_PIN        35
@@ -79,6 +80,7 @@
 # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ADC
 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
 # define CONFIG_INS_TYPE   CONFIG_INS_MPU6000
+# define CONFIG_COMPASS  AP_COMPASS_HMC5843
 # define CONFIG_PUSHBUTTON DISABLED
 # define CONFIG_RELAY      DISABLED
 # define MAG_ORIENTATION   AP_COMPASS_APM2_SHIELD
@@ -95,7 +97,8 @@
 # define BATTERY_PIN_1	  1
 # define CURRENT_PIN_1	  2
 #elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
-# define CONFIG_INS_TYPE   CONFIG_INS_SITL
+# define CONFIG_INS_TYPE CONFIG_INS_STUB
+# define CONFIG_COMPASS  AP_COMPASS_HIL
 # define CONFIG_PUSHBUTTON DISABLED
 # define CONFIG_RELAY      DISABLED
 # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
@@ -112,7 +115,8 @@
 # define CURRENT_PIN_1	  2
 # define MAG_ORIENTATION  AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
-# define CONFIG_INS_TYPE   CONFIG_INS_SITL
+# define CONFIG_INS_TYPE   CONFIG_INS_PX4
+# define CONFIG_COMPASS  AP_COMPASS_PX4
 # define CONFIG_PUSHBUTTON DISABLED
 # define CONFIG_RELAY      DISABLED
 # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
@@ -188,6 +192,17 @@
 #define HIL_MODE	HIL_MODE_DISABLED
 #endif
 
+#if HIL_MODE != HIL_MODE_DISABLED       // we are in HIL mode
+ #undef GPS_PROTOCOL
+ #define GPS_PROTOCOL GPS_PROTOCOL_HIL
+ #undef CONFIG_INS_TYPE
+ #define CONFIG_INS_TYPE CONFIG_INS_STUB
+ #undef CONFIG_ADC
+ #define CONFIG_ADC DISABLED
+ #undef  CONFIG_COMPASS
+ #define CONFIG_COMPASS  AP_COMPASS_HIL
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // GPS_PROTOCOL
 //

APMrover2/defines.h

@@ -208,10 +208,15 @@ enum gcs_severity {
 // mark a function as not to be inlined
 #define NOINLINE __attribute__((noinline))
 
+// InertialSensor driver types
 #define CONFIG_INS_OILPAN  1
 #define CONFIG_INS_MPU6000 2
+#define CONFIG_INS_STUB    3
+#define CONFIG_INS_PX4     4
 
-#define AP_BARO_BMP085   1
+// compass driver types
-#define AP_BARO_MS5611   2
+#define AP_COMPASS_HMC5843   1
+#define AP_COMPASS_PX4       2
+#define AP_COMPASS_HIL       3
 
 #endif // _DEFINES_H