From f7697574db37d2d5fe42d7ae31ee7ccdb2e25747 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 3 Jun 2013 11:40:44 +1000 Subject: [PATCH] Plane: use the new airspeed driver interface the choice of analog source is now in the airspeed driver --- ArduPlane/ArduPlane.pde | 16 +++------------- ArduPlane/GCS_Mavlink.pde | 1 + ArduPlane/config.h | 29 ----------------------------- ArduPlane/system.pde | 7 +++++-- ArduPlane/test.pde | 17 ++++++----------- 5 files changed, 15 insertions(+), 55 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index f34e4da33e..5365f629a0 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -142,8 +142,8 @@ static GPS *g_gps; // flight modes convenience array static AP_Int8 *flight_modes = &g.flight_mode1; -#if CONFIG_ADC == ENABLED -static AP_ADC_ADS7844 adc; +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 +AP_ADC_ADS7844 apm1_adc; #endif #if CONFIG_BARO == AP_BARO_BMP085 @@ -210,7 +210,7 @@ AP_InertialSensor_PX4 ins; #elif CONFIG_INS_TYPE == CONFIG_INS_STUB AP_InertialSensor_Stub ins; #elif CONFIG_INS_TYPE == CONFIG_INS_OILPAN -AP_InertialSensor_Oilpan ins( &adc ); +AP_InertialSensor_Oilpan ins( &apm1_adc ); #else #error Unrecognised CONFIG_INS_TYPE setting. #endif // CONFIG_INS_TYPE @@ -240,8 +240,6 @@ static AP_Navigation *nav_controller = &L1_controller; // Analog Inputs //////////////////////////////////////////////////////////////////////////////// -static AP_HAL::AnalogSource *pitot_analog_source; - // a pin for reading the receiver RSSI voltage. static AP_HAL::AnalogSource *rssi_analog_source; @@ -661,19 +659,11 @@ void setup() { rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE); -#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC - pitot_analog_source = new AP_ADC_AnalogSource( &adc, - CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0f); -#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN - pitot_analog_source = hal.analogin->channel(CONFIG_PITOT_SOURCE_ANALOG_PIN); - hal.gpio->write(hal.gpio->analogPinToDigitalPin(CONFIG_PITOT_SOURCE_ANALOG_PIN), 0); -#endif vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC); batt_volt_pin = hal.analogin->channel(g.battery_volt_pin); batt_curr_pin = hal.analogin->channel(g.battery_curr_pin); - airspeed.init(pitot_analog_source); init_ardupilot(); } diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 96c5a78400..9ad98898ed 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1919,6 +1919,7 @@ mission_failed: barometer.setHIL(packet.alt*0.001f); compass.setHIL(packet.roll, packet.pitch, packet.yaw); + airspeed.disable(); break; } #endif // HIL_MODE diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 766aa5c1cf..04b044e483 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -98,8 +98,6 @@ # define BATTERY_VOLT_PIN 0 // Battery voltage on A0 # define BATTERY_CURR_PIN 1 // Battery current on A1 # define CONFIG_INS_TYPE CONFIG_INS_OILPAN - # define CONFIG_PITOT_SOURCE PITOT_SOURCE_ADC - # define CONFIG_PITOT_SOURCE_ADC_CHANNEL 7 # define CONFIG_BARO AP_BARO_BMP085 # define CONFIG_COMPASS AP_COMPASS_HMC5843 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 @@ -116,8 +114,6 @@ # define BATTERY_VOLT_PIN 1 // Battery voltage on A1 # define BATTERY_CURR_PIN 2 // Battery current on A2 # define CONFIG_INS_TYPE CONFIG_INS_MPU6000 - # define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN - # define CONFIG_PITOT_SOURCE_ANALOG_PIN 0 # ifdef APM2_BETA_HARDWARE # define CONFIG_BARO AP_BARO_BMP085 # else // APM2 Production Hardware (default) @@ -134,8 +130,6 @@ # define BATTERY_VOLT_PIN 1 // Battery voltage on A1 # define BATTERY_CURR_PIN 2 // Battery current on A2 # define CONFIG_INS_TYPE CONFIG_INS_STUB - # define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN - # define CONFIG_PITOT_SOURCE_ANALOG_PIN 0 # define CONFIG_BARO AP_BARO_HIL # define CONFIG_COMPASS AP_COMPASS_HIL #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 @@ -148,25 +142,12 @@ # define BATTERY_VOLT_PIN -1 # define BATTERY_CURR_PIN -1 # define CONFIG_INS_TYPE CONFIG_INS_PX4 - # define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN - # define CONFIG_PITOT_SOURCE_ANALOG_PIN 11 # define CONFIG_BARO AP_BARO_PX4 # define CONFIG_COMPASS AP_COMPASS_PX4 # define SERIAL0_BAUD 115200 #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 CONFIG_BARO # error "CONFIG_BARO not set" #endif @@ -175,10 +156,6 @@ # error "CONFIG_COMPASS not set" #endif -#ifndef CONFIG_PITOT_SOURCE - # error "CONFIG_PITOT_SOURCE not set" -#endif - ////////////////////////////////////////////////////////////////////////////// // HIL_MODE OPTIONAL @@ -193,12 +170,6 @@ #define CONFIG_BARO AP_BARO_HIL #undef CONFIG_INS_TYPE #define CONFIG_INS_TYPE CONFIG_INS_STUB - #undef CONFIG_ADC - #define CONFIG_ADC DISABLED - #undef CONFIG_PITOT_SOURCE - #define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN - #undef CONFIG_PITOT_SOURCE_ANALOG_PIN - #define CONFIG_PITOT_SOURCE_ANALOG_PIN -1 #undef CONFIG_COMPASS #define CONFIG_COMPASS AP_COMPASS_HIL #endif diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index a5c4d32670..f1881c88ca 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -156,10 +156,13 @@ static void init_ardupilot() } #endif - #if CONFIG_ADC == ENABLED - adc.Init(); // APM ADC library initialization + #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 + apm1_adc.Init(); // APM ADC library initialization #endif + // initialise airspeed sensor + airspeed.init(); + if (g.compass_enabled==true) { if (!compass.init() || !compass.read()) { cliSerial->println_P(PSTR("Compass initialisation failed!")); diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index ebb933e137..ca110c7da1 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -9,7 +9,7 @@ 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 +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 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); @@ -47,7 +47,7 @@ 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 + #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 {"adc", test_adc}, #endif {"gps", test_gps}, @@ -414,18 +414,18 @@ test_shell(uint8_t argc, const Menu::arg *argv) //------------------------------------------------------------------------------------------- // tests in this section are for real sensors or sensors that have been simulated - #if CONFIG_ADC == ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 static int8_t test_adc(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); - adc.Init(); + apm1_adc.Init(); delay(1000); cliSerial->printf_P(PSTR("ADC\n")); delay(1000); while(1) { - for (int8_t i=0; i<9; i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i)); + for (int8_t i=0; i<9; i++) cliSerial->printf_P(PSTR("%.1f\t"),apm1_adc.Ch(i)); cliSerial->println(); delay(100); if(cliSerial->available() > 0) { @@ -433,7 +433,7 @@ test_adc(uint8_t argc, const Menu::arg *argv) } } } - #endif // CONFIG_ADC +#endif // CONFIG_HAL_BOARD == HAL_BOARD_APM1 static int8_t test_gps(uint8_t argc, const Menu::arg *argv) @@ -608,15 +608,10 @@ test_mag(uint8_t argc, const Menu::arg *argv) static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv) { - float airspeed_ch = pitot_analog_source->voltage_average(); - // cliSerial->println(pitot_analog_source.read()); - cliSerial->printf_P(PSTR("airspeed_ch: %.3f\n"), airspeed_ch); - if (!airspeed.enabled()) { cliSerial->printf_P(PSTR("airspeed: ")); print_enabled(false); return (0); - }else{ print_hit_enter(); zero_airspeed();