diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index e470d74eac..3a121d003c 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -17,9 +17,7 @@ * // HIL_MODE SELECTION * // * // Mavlink supports - * // 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude * // 2. HIL_MODE_SENSORS: full sensor simulation - * //#define HIL_MODE HIL_MODE_ATTITUDE * */ diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 6a21499430..12bba92ab9 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -213,11 +213,7 @@ AP_InertialSensor_Oilpan ins( &adc ); #error Unrecognised CONFIG_INS_TYPE setting. #endif // CONFIG_INS_TYPE -#if HIL_MODE == HIL_MODE_ATTITUDE -AP_AHRS_HIL ahrs(&ins, g_gps); -#else AP_AHRS_DCM ahrs(&ins, g_gps); -#endif static AP_L1_Control L1_controller(&ahrs); @@ -740,7 +736,7 @@ static void fast_loop() // ------------------------------------ check_short_failsafe(); -#if HIL_MODE == HIL_MODE_SENSORS +#if HIL_MODE != HIL_MODE_DISABLED // update hil before AHRS update gcs_update(); #endif @@ -802,14 +798,12 @@ static void medium_loop() update_GPS(); calc_gndspeed_undershoot(); -#if HIL_MODE != HIL_MODE_ATTITUDE if (g.compass_enabled && compass.read()) { ahrs.set_compass(&compass); compass.null_offsets(); } else { ahrs.set_compass(NULL); } -#endif break; @@ -835,11 +829,9 @@ static void medium_loop() // Read Airspeed // ------------- -#if HIL_MODE != HIL_MODE_ATTITUDE if (airspeed.enabled()) { read_airspeed(); } -#endif read_receiver_rssi(); @@ -908,12 +900,11 @@ static void slow_loop() slow_loopCounter++; check_long_failsafe(); superslow_loopCounter++; - if(superslow_loopCounter >=200) { // 200 = Execute every minute -#if HIL_MODE != HIL_MODE_ATTITUDE + if(superslow_loopCounter >=200) { + // 200 = Execute every minute if(g.compass_enabled) { compass.save_offsets(); } -#endif superslow_loopCounter = 0; } @@ -1249,9 +1240,6 @@ static void update_navigation() static void update_alt() { -#if HIL_MODE == HIL_MODE_ATTITUDE - current_loc.alt = g_gps->altitude; -#else // this function is in place to potentially add a sonar sensor in the future //altitude_sensor = BARO; @@ -1261,7 +1249,6 @@ static void update_alt() } else if (g_gps->status() >= GPS::GPS_OK_FIX_3D) { current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100) } -#endif geofence_check(true); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 97074db223..96dddf74c7 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1911,15 +1911,6 @@ mission_failed: y *= 95446.0; barometer.setHIL(Temp, y); - - #if HIL_MODE == HIL_MODE_ATTITUDE - // set AHRS hil sensor. We don't do this in sensors mode, as - // in that case the attitude is computed via DCM - ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, - packet.pitchspeed,packet.yawspeed); - - #endif - break; } #endif // HIL_MODE diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index ac2d4474c8..e7e3313e1d 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -54,7 +54,9 @@ #define CH_RUDDER CH_4 #define CH_YAW CH_4 -// HIL enumerations +// HIL enumerations. Note that HIL_MODE_ATTITUDE and HIL_MODE_SENSORS +// are now the same thing, and are sensors based. The old define is +// kept to allow old APM_Config.h headers to keep working #define HIL_MODE_DISABLED 0 #define HIL_MODE_ATTITUDE 1 #define HIL_MODE_SENSORS 2 diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index b1824ad569..c9ac9c9561 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -153,8 +153,6 @@ static void init_ardupilot() } #endif -#if HIL_MODE != HIL_MODE_ATTITUDE - #if CONFIG_ADC == ENABLED adc.Init(); // APM ADC library initialization #endif @@ -170,7 +168,6 @@ static void init_ardupilot() ahrs.set_compass(&compass); } } -#endif // give AHRS the airspeed sensor ahrs.set_airspeed(&airspeed); @@ -226,7 +223,6 @@ static void init_ardupilot() // Get necessary data from EEPROM //---------------- //read_EEPROM_airstart_critical(); -#if HIL_MODE != HIL_MODE_ATTITUDE ahrs.init(); ahrs.set_fly_forward(true); @@ -234,8 +230,6 @@ static void init_ardupilot() ins_sample_rate, flash_leds); -#endif - // This delay is important for the APM_RC library to work. // We need some time for the comm between the 328 and 1280 to be established. int old_pulse = 0; @@ -474,7 +468,6 @@ static void startup_INS_ground(bool force_accel_level) #endif ahrs.reset(); -#if HIL_MODE != HIL_MODE_ATTITUDE // read Baro pressure at ground //----------------------------- init_barometer(); @@ -487,7 +480,6 @@ static void startup_INS_ground(bool force_accel_level) gcs_send_text_P(SEVERITY_LOW,PSTR("NO airspeed")); } -#endif digitalWrite(B_LED_PIN, LED_ON); // Set LED B high to indicate INS ready digitalWrite(A_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_OFF); diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 31d4f39b2a..c8539fa85b 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -56,11 +56,10 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { {"airspeed", test_airspeed}, {"airpressure", test_pressure}, {"compass", test_mag}, -#elif HIL_MODE == HIL_MODE_SENSORS +#else {"gps", test_gps}, {"ins", test_ins}, {"compass", test_mag}, -#elif HIL_MODE == HIL_MODE_ATTITUDE #endif {"logging", test_logging}, #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 @@ -413,8 +412,6 @@ test_shell(uint8_t argc, const Menu::arg *argv) //------------------------------------------------------------------------------------------- // tests in this section are for real sensors or sensors that have been simulated -#if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS - #if CONFIG_ADC == ENABLED static int8_t test_adc(uint8_t argc, const Menu::arg *argv) @@ -602,8 +599,6 @@ test_mag(uint8_t argc, const Menu::arg *argv) return (0); } -#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS - //------------------------------------------------------------------------------------------- // real sensors that have not been simulated yet go here