From feb539bade6ea3b3d7eab7e4f1eb5d1f1235640b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Jun 2013 11:57:59 +1000 Subject: [PATCH] Rover: fixed HIL operation only HIL sensors - removed HIL_MODE_ATTITUDE as it didn't exercise enough of the code --- APMrover2/APMrover2.pde | 22 +++++++---------- APMrover2/GCS_Mavlink.pde | 9 +------ APMrover2/Log.pde | 6 ----- APMrover2/Parameters.pde | 2 -- APMrover2/config.h | 50 --------------------------------------- APMrover2/defines.h | 3 --- APMrover2/sensors.pde | 7 ------ APMrover2/system.pde | 8 +------ APMrover2/test.pde | 39 ------------------------------ 9 files changed, 10 insertions(+), 136 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 0ec27f7e75..9da9f448cd 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -169,7 +169,7 @@ static GPS *g_gps; // flight modes convenience array static AP_Int8 *modes = &g.mode1; -#if CONFIG_ADC == ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 static AP_ADC_ADS7844 adc; #endif @@ -224,11 +224,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 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL SITL sitl; @@ -550,8 +546,8 @@ static float load; microseconds) */ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { - { update_GPS, 5, 1200 }, - { navigate, 5, 1000 }, + { update_GPS, 5, 2500 }, + { navigate, 5, 1600 }, { update_compass, 5, 2000 }, { update_commands, 5, 1000 }, { update_logging, 5, 1000 }, @@ -638,7 +634,7 @@ static void fast_loop() // some space available gcs_send_message(MSG_RETRY_DEFERRED); - #if HIL_MODE == HIL_MODE_SENSORS + #if HIL_MODE != HIL_MODE_DISABLED // update hil before dcm update gcs_update(); #endif @@ -650,13 +646,11 @@ static void fast_loop() // uses the yaw from the DCM to give more accurate turns calc_bearing_error(); - # if HIL_MODE == HIL_MODE_DISABLED - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) - Log_Write_Attitude(); + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) + Log_Write_Attitude(); - if (g.log_bitmask & MASK_LOG_IMU) - DataFlash.Log_Write_IMU(&ins); - #endif + if (g.log_bitmask & MASK_LOG_IMU) + DataFlash.Log_Write_IMU(&ins); // custom code/exceptions for flight modes // --------------------------------------- diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index bad619a8a8..d29af7e3c6 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -1700,14 +1700,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) ins.set_accel(accels); 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; + break; } #endif // HIL_MODE diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index 6dc137a445..6f3be7f596 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -175,7 +175,6 @@ struct PACKED log_Performance { }; // Write a performance monitoring packet. Total length : 19 bytes -#if HIL_MODE != HIL_MODE_ATTITUDE static void Log_Write_Performance() { struct log_Performance pkt = { @@ -194,7 +193,6 @@ static void Log_Write_Performance() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } -#endif struct PACKED log_Cmd { LOG_PACKET_HEADER; @@ -258,7 +256,6 @@ struct PACKED log_Control_Tuning { }; // Write a control tuning packet. Total length : 22 bytes -#if HIL_MODE != HIL_MODE_ATTITUDE static void Log_Write_Control_Tuning() { Vector3f accel = ins.get_accel(); @@ -272,7 +269,6 @@ static void Log_Write_Control_Tuning() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } -#endif struct PACKED log_Nav_Tuning { LOG_PACKET_HEADER; @@ -350,7 +346,6 @@ struct PACKED log_Sonar { }; // Write a sonar packet -#if HIL_MODE != HIL_MODE_ATTITUDE static void Log_Write_Sonar() { uint16_t turn_time = 0; @@ -370,7 +365,6 @@ static void Log_Write_Sonar() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } -#endif struct PACKED log_Current { LOG_PACKET_HEADER; diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 9845b9c079..b9b25f5dc8 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -435,11 +435,9 @@ const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp GOBJECT(sonar2, "SONAR2_", AP_RangeFinder_analog), -#if HIL_MODE == HIL_MODE_DISABLED // @Group: INS_ // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp GOBJECT(ins, "INS_", AP_InertialSensor), -#endif #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL // @Group: SIM_ diff --git a/APMrover2/config.h b/APMrover2/config.h index d2abfadd4f..4024769048 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -77,13 +77,11 @@ # define CONFIG_RELAY ENABLED # define BATTERY_PIN_1 0 # define CURRENT_PIN_1 1 -# 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 CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN # define A_LED_PIN 27 # define B_LED_PIN 26 # define C_LED_PIN 25 @@ -100,7 +98,6 @@ # define CONFIG_COMPASS AP_COMPASS_HIL # define CONFIG_PUSHBUTTON DISABLED # define CONFIG_RELAY DISABLED -# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN # define A_LED_PIN 27 # define B_LED_PIN 26 # define C_LED_PIN 25 @@ -117,7 +114,6 @@ # define CONFIG_COMPASS AP_COMPASS_PX4 # define CONFIG_PUSHBUTTON DISABLED # define CONFIG_RELAY DISABLED -# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN # define A_LED_PIN 27 # define B_LED_PIN 26 # define C_LED_PIN 25 @@ -138,50 +134,6 @@ # define CONFIG_INS_TYPE CONFIG_INS_OILPAN #endif -////////////////////////////////////////////////////////////////////////////// -// ADC Enable - used to eliminate for systems which don't have ADC. -// -#ifndef CONFIG_ADC -# if CONFIG_INS_TYPE == CONFIG_INS_OILPAN -# define CONFIG_ADC ENABLED -# else -# define CONFIG_ADC DISABLED -# endif -#endif - -#ifndef SONAR_TYPE -# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL, -#endif - -////////////////////////////////////////////////////////////////////////////// -// Sonar -// - -#ifndef CONFIG_SONAR_SOURCE -# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ADC -#endif - -#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC && CONFIG_ADC == DISABLED -# warning Cannot use ADC for CONFIG_SONAR_SOURCE, becaude CONFIG_ADC is DISABLED -# warning Defaulting CONFIG_SONAR_SOURCE to ANALOG_PIN -# undef CONFIG_SONAR_SOURCE -# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN -#endif - -#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC -# ifndef CONFIG_SONAR_SOURCE_ADC_CHANNEL -# define CONFIG_SONAR_SOURCE_ADC_CHANNEL 7 -# endif -#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN -# ifndef CONFIG_SONAR_SOURCE_ANALOG_PIN -# define CONFIG_SONAR_SOURCE_ANALOG_PIN 0 -# endif -#else -# warning Invalid value for CONFIG_SONAR_SOURCE, disabling sonar -# undef SONAR_ENABLED -# define SONAR_ENABLED DISABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // HIL_MODE OPTIONAL @@ -194,8 +146,6 @@ #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 diff --git a/APMrover2/defines.h b/APMrover2/defines.h index a3cb3f86f4..2e91892ace 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -19,9 +19,6 @@ #define SONAR 0 #define BARO 1 -#define SONAR_SOURCE_ADC 1 -#define SONAR_SOURCE_ANALOG_PIN 2 - // CH 7 control enum ch7_option { CH7_DO_NOTHING=0, diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index 9af97f941b..b37821677b 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -11,13 +11,6 @@ static void init_sonar(void) #endif } -// Sensors are not available in HIL_MODE_ATTITUDE -#if HIL_MODE != HIL_MODE_ATTITUDE - -void ReadSCP1000(void) {} - -#endif // HIL_MODE != HIL_MODE_ATTITUDE - /* read and update the battery */ diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 22da95b768..567e82fb39 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -166,9 +166,7 @@ static void init_ardupilot() } #endif -#if HIL_MODE != HIL_MODE_ATTITUDE - -#if CONFIG_ADC == ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 adc.Init(); // APM ADC library initialization #endif @@ -185,7 +183,6 @@ static void init_ardupilot() // initialise sonar init_sonar(); -#endif // Do GPS init g_gps = &g_gps_driver; // GPS initialisation @@ -386,7 +383,6 @@ static void failsafe_trigger(uint8_t failsafe_type, bool on) static void startup_INS_ground(bool force_accel_level) { -#if HIL_MODE != HIL_MODE_ATTITUDE gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); mavlink_delay(500); @@ -410,8 +406,6 @@ static void startup_INS_ground(bool force_accel_level) } ahrs.reset(); -#endif // HIL_MODE_ATTITUDE - 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/APMrover2/test.pde b/APMrover2/test.pde index fc11bfa1b5..1898de9ca3 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -9,9 +9,6 @@ static int8_t test_radio(uint8_t argc, const Menu::arg *argv); static int8_t test_passthru(uint8_t argc, const Menu::arg *argv); static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); static int8_t test_gps(uint8_t argc, const Menu::arg *argv); -#if CONFIG_ADC == ENABLED -static int8_t test_adc(uint8_t argc, const Menu::arg *argv); -#endif static int8_t test_ins(uint8_t argc, const Menu::arg *argv); static int8_t test_battery(uint8_t argc, const Menu::arg *argv); static int8_t test_relay(uint8_t argc, const Menu::arg *argv); @@ -40,21 +37,10 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { // Tests below here are for hardware sensors only present // when real sensors are attached or they are emulated -#if HIL_MODE == HIL_MODE_DISABLED -#if CONFIG_ADC == ENABLED - {"adc", test_adc}, -#endif {"gps", test_gps}, {"ins", test_ins}, {"sonartest", test_sonar}, {"compass", test_mag}, -#elif HIL_MODE == HIL_MODE_SENSORS - {"adc", test_adc}, - {"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 {"shell", test_shell}, @@ -355,29 +341,6 @@ test_logging(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) -{ - print_hit_enter(); - adc.Init(); - delay(1000); - cliSerial->printf_P(PSTR("ADC\n")); - delay(1000); - - while(1){ - for (int i=0;i<9;i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i)); - cliSerial->println(); - delay(100); - if(cliSerial->available() > 0){ - return (0); - } - } -} -#endif // CONFIG_ADC - static int8_t test_gps(uint8_t argc, const Menu::arg *argv) { @@ -544,8 +507,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