From 24f2ac20deb91a82fb6c9bffb992ee043c5ecab5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Jan 2014 16:28:50 +1100 Subject: [PATCH] Copter: removed HIL_MODE_ATTITUDE this really can't work any more, as AHRS can't support it --- ArduCopter/APM_Config_mavlink_hil.h | 8 ++------ ArduCopter/ArduCopter.pde | 28 +--------------------------- ArduCopter/GCS_Mavlink.pde | 10 ---------- ArduCopter/Parameters.pde | 2 -- ArduCopter/defines.h | 3 +-- ArduCopter/motors.pde | 2 -- ArduCopter/sensors.pde | 6 ------ ArduCopter/system.pde | 6 ------ ArduCopter/test.pde | 12 ++++++------ 9 files changed, 10 insertions(+), 67 deletions(-) diff --git a/ArduCopter/APM_Config_mavlink_hil.h b/ArduCopter/APM_Config_mavlink_hil.h index 3f6d781e63..3ba7fc38bf 100644 --- a/ArduCopter/APM_Config_mavlink_hil.h +++ b/ArduCopter/APM_Config_mavlink_hil.h @@ -3,9 +3,8 @@ // 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 +// 1. HIL_MODE_SENSORS: full sensor simulation +#define HIL_MODE HIL_MODE_SENSORS // HIL_PORT SELCTION // @@ -36,7 +35,4 @@ // Sensors // All sensors are supported in all modes. -// The magnetometer is not used in -// HIL_MODE_ATTITUDE but you may leave it -// enabled if you wish. #define MAGNETOMETER ENABLED diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 09fb8be83e..10c290cb36 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -318,7 +318,7 @@ AP_GPS_None g_gps_driver; static AP_AHRS_DCM ahrs(ins, g_gps); -#elif HIL_MODE == HIL_MODE_SENSORS +#elif HIL_MODE != HIL_MODE_DISABLED // sensor emulators static AP_ADC_HIL adc; static AP_Baro_HIL barometer; @@ -333,19 +333,6 @@ static AP_AHRS_DCM ahrs(ins, g_gps); static SITL sitl; #endif -#elif HIL_MODE == HIL_MODE_ATTITUDE -static AP_ADC_HIL adc; -static AP_InertialSensor_HIL ins; -static AP_AHRS_HIL ahrs(ins, g_gps); -static AP_GPS_HIL g_gps_driver; -static AP_Compass_HIL compass; // never used -static AP_Baro_HIL barometer; - -#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL - // When building for SITL we use the HIL barometer and compass drivers -static SITL sitl; -#endif - #else #error Unrecognised HIL_MODE setting. #endif // HIL MODE @@ -1067,7 +1054,6 @@ static void update_batt_compass(void) // read battery before compass because it may be used for motor interference compensation read_battery(); -#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode if(g.compass_enabled) { // update compass with throttle value - used for compassmot compass.set_throttle((float)g.rc_3.servo_out/1000.0f); @@ -1079,7 +1065,6 @@ static void update_batt_compass(void) Log_Write_Compass(); } } -#endif // record throttle output throttle_integrator += g.rc_3.servo_out; @@ -1364,20 +1349,11 @@ static void read_AHRS(void) // read baro and sonar altitude at 20hz static void update_altitude() { -#if HIL_MODE == HIL_MODE_ATTITUDE - // we are in the SIM, fake out the baro and Sonar - baro_alt = g_gps->altitude_cm; - - if(g.sonar_enabled) { - sonar_alt = baro_alt; - } -#else // read in baro altitude baro_alt = read_barometer(); // read in sonar altitude sonar_alt = read_sonar(); -#endif // HIL_MODE == HIL_MODE_ATTITUDE // write altitude info to dataflash logs if (g.log_bitmask & MASK_LOG_CTUN) { @@ -1518,7 +1494,6 @@ static void tuning(){ g.pid_optflow_pitch.kD(tuning_value); break; -#if HIL_MODE != HIL_MODE_ATTITUDE // do not allow modifying _kp or _kp_yaw gains in HIL mode case CH6_AHRS_YAW_KP: ahrs._kp_yaw.set(tuning_value); break; @@ -1526,7 +1501,6 @@ static void tuning(){ case CH6_AHRS_KP: ahrs._kp.set(tuning_value); break; -#endif case CH6_INAV_TC: // To-Do: allowing tuning TC for xy and z separately diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 2fc0a2e061..ab46c1a6b5 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1180,9 +1180,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) ahrs.set_trim(Vector3f(0,0,0)); // clear out saved trim } if (packet.param3 == 1) { -#if HIL_MODE != HIL_MODE_ATTITUDE init_barometer(false); // fast barometer calibratoin -#endif } if (packet.param4 == 1) { trim_radio(); @@ -1950,14 +1948,6 @@ mission_failed: barometer.setHIL(packet.alt*0.001f); compass.setHIL(packet.roll, packet.pitch, packet.yaw); - #if HIL_MODE == HIL_MODE_ATTITUDE - // set AHRS hil sensor - ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, - packet.pitchspeed,packet.yawspeed); - #endif - - - break; } #endif // HIL_MODE != HIL_MODE_DISABLED diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 3477811295..6143fc679c 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -946,9 +946,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Group: INS_ // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp -#if HIL_MODE != HIL_MODE_ATTITUDE GOBJECT(ins, "INS_", AP_InertialSensor), -#endif // @Group: INAV_ // @Path: ../libraries/AP_InertialNav/AP_InertialNav.cpp diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 2eb567b7c2..b7dcb0e3a5 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -89,8 +89,7 @@ // HIL enumerations #define HIL_MODE_DISABLED 0 -#define HIL_MODE_ATTITUDE 1 -#define HIL_MODE_SENSORS 2 +#define HIL_MODE_SENSORS 1 // Auto Pilot modes // ---------------- diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 584aa43d5e..1d8074a8ce 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -164,10 +164,8 @@ static void init_arm_motors() startup_ground(true); } -#if HIL_MODE != HIL_MODE_ATTITUDE // fast baro calibration to reset ground pressure init_barometer(false); -#endif // go back to normal AHRS gains ahrs.set_fast_gains(false); diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index d3f873d0f7..6ce828fac3 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -1,8 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// Sensors are not available in HIL_MODE_ATTITUDE -#if HIL_MODE != HIL_MODE_ATTITUDE - #if CONFIG_SONAR == ENABLED static void init_sonar(void) { @@ -65,9 +62,6 @@ static int16_t read_sonar(void) #endif } - -#endif // HIL_MODE != HIL_MODE_ATTITUDE - static void init_compass() { if (!compass.init() || !compass.read()) { diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index c245d97fc7..b774ad2588 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -161,9 +161,7 @@ static void init_ardupilot() rssi_analog_source = hal.analogin->channel(g.rssi_pin); -#if HIL_MODE != HIL_MODE_ATTITUDE barometer.init(); -#endif // init the GCS gcs[0].init(hal.uartA); @@ -217,12 +215,10 @@ static void init_ardupilot() */ hal.scheduler->register_timer_failsafe(failsafe_check, 1000); -#if HIL_MODE != HIL_MODE_ATTITUDE #if CONFIG_ADC == ENABLED // begin filtering the ADC Gyros adc.Init(); // APM ADC library initialization #endif // CONFIG_ADC -#endif // HIL_MODE // Do GPS init g_gps = &g_gps_driver; @@ -268,11 +264,9 @@ static void init_ardupilot() } #endif -#if HIL_MODE != HIL_MODE_ATTITUDE // read Baro pressure at ground //----------------------------- init_barometer(true); -#endif // initialise sonar #if CONFIG_SONAR == ENABLED diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 9524b5a027..5ab0980324 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -4,7 +4,7 @@ // These are function definitions so the Menu can be constructed before the functions // are defined below. Order matters to the compiler. -#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS +#if HIL_MODE == HIL_MODE_DISABLED static int8_t test_baro(uint8_t argc, const Menu::arg *argv); #endif static int8_t test_compass(uint8_t argc, const Menu::arg *argv); @@ -20,7 +20,7 @@ static int8_t test_relay(uint8_t argc, const Menu::arg *argv); #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 static int8_t test_shell(uint8_t argc, const Menu::arg *argv); #endif -#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS +#if HIL_MODE == HIL_MODE_DISABLED static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); #endif @@ -29,7 +29,7 @@ static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); // User enters the string in the console to call the functions on the right. // See class Menu in AP_Coommon for implementation details const struct Menu::command test_menu_commands[] PROGMEM = { -#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS +#if HIL_MODE == HIL_MODE_DISABLED {"baro", test_baro}, #endif {"compass", test_compass}, @@ -45,7 +45,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 {"shell", test_shell}, #endif -#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS +#if HIL_MODE == HIL_MODE_DISABLED {"sonar", test_sonar}, #endif }; @@ -60,7 +60,7 @@ test_mode(uint8_t argc, const Menu::arg *argv) return 0; } -#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS +#if HIL_MODE == HIL_MODE_DISABLED static int8_t test_baro(uint8_t argc, const Menu::arg *argv) { @@ -522,7 +522,7 @@ test_shell(uint8_t argc, const Menu::arg *argv) } #endif -#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS +#if HIL_MODE == HIL_MODE_DISABLED /* * test the sonar */