diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 2bcc235deb..d7d387fdb9 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -287,29 +287,11 @@ static AP_Compass_HIL compass; #error Unrecognized CONFIG_COMPASS setting #endif -#if CONFIG_INS_TYPE == HAL_INS_OILPAN || CONFIG_HAL_BOARD == HAL_BOARD_APM1 +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 AP_ADC_ADS7844 apm1_adc; #endif -#if CONFIG_INS_TYPE == HAL_INS_MPU6000 -AP_InertialSensor_MPU6000 ins; -#elif CONFIG_INS_TYPE == HAL_INS_PX4 -AP_InertialSensor_PX4 ins; -#elif CONFIG_INS_TYPE == HAL_INS_VRBRAIN -AP_InertialSensor_VRBRAIN ins; -#elif CONFIG_INS_TYPE == HAL_INS_HIL -AP_InertialSensor_HIL ins; -#elif CONFIG_INS_TYPE == HAL_INS_OILPAN -AP_InertialSensor_Oilpan ins( &apm1_adc ); -#elif CONFIG_INS_TYPE == HAL_INS_FLYMAPLE -AP_InertialSensor_Flymaple ins; -#elif CONFIG_INS_TYPE == HAL_INS_L3G4200D -AP_InertialSensor_L3G4200D ins; -#elif CONFIG_INS_TYPE == HAL_INS_MPU9250 -AP_InertialSensor_MPU9250 ins; -#else - #error Unrecognised CONFIG_INS_TYPE setting. -#endif // CONFIG_INS_TYPE +AP_InertialSensor ins; // Inertial Navigation EKF #if AP_AHRS_NAVEKF_AVAILABLE @@ -969,10 +951,8 @@ static void perf_update(void) void loop() { // wait for an INS sample - if (!ins.wait_for_sample(1000)) { - Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_MAIN_INS_DELAY); - return; - } + ins.wait_for_sample(); + uint32_t timer = micros(); // check loop time diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 624112e45d..09b7b725fe 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -55,7 +55,6 @@ ////////////////////////////////////////////////////////////////////////////// // sensor types -#define CONFIG_INS_TYPE HAL_INS_DEFAULT #define CONFIG_BARO HAL_BARO_DEFAULT #define CONFIG_COMPASS HAL_COMPASS_DEFAULT @@ -73,8 +72,6 @@ #if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode #undef CONFIG_BARO #define CONFIG_BARO HAL_BARO_HIL - #undef CONFIG_INS_TYPE - #define CONFIG_INS_TYPE HAL_INS_HIL #undef CONFIG_COMPASS #define CONFIG_COMPASS HAL_COMPASS_HIL #endif @@ -162,7 +159,7 @@ // ADC Enable - used to eliminate for systems which don't have ADC. // #ifndef CONFIG_ADC - # if CONFIG_INS_TYPE == HAL_INS_OILPAN || CONFIG_HAL_BOARD == HAL_BOARD_APM1 + # if CONFIG_HAL_BOARD == HAL_BOARD_APM1 # define CONFIG_ADC ENABLED # else # define CONFIG_ADC DISABLED diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index d391c5cd30..523a507039 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -249,6 +249,9 @@ static void init_ardupilot() gcs_send_text_P(SEVERITY_LOW, PSTR("Waiting for first HIL_STATE message")); delay(1000); } + + // set INS to HIL mode + ins.set_hil_mode(); #endif // read Baro pressure at ground