Plane: use the new airspeed driver interface

the choice of analog source is now in the airspeed driver
This commit is contained in:
Andrew Tridgell 2013-06-03 11:40:44 +10:00
parent 68adeb041d
commit f7697574db
5 changed files with 15 additions and 55 deletions

View File

@ -142,8 +142,8 @@ static GPS *g_gps;
// flight modes convenience array // flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1; static AP_Int8 *flight_modes = &g.flight_mode1;
#if CONFIG_ADC == ENABLED #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
static AP_ADC_ADS7844 adc; AP_ADC_ADS7844 apm1_adc;
#endif #endif
#if CONFIG_BARO == AP_BARO_BMP085 #if CONFIG_BARO == AP_BARO_BMP085
@ -210,7 +210,7 @@ AP_InertialSensor_PX4 ins;
#elif CONFIG_INS_TYPE == CONFIG_INS_STUB #elif CONFIG_INS_TYPE == CONFIG_INS_STUB
AP_InertialSensor_Stub ins; AP_InertialSensor_Stub ins;
#elif CONFIG_INS_TYPE == CONFIG_INS_OILPAN #elif CONFIG_INS_TYPE == CONFIG_INS_OILPAN
AP_InertialSensor_Oilpan ins( &adc ); AP_InertialSensor_Oilpan ins( &apm1_adc );
#else #else
#error Unrecognised CONFIG_INS_TYPE setting. #error Unrecognised CONFIG_INS_TYPE setting.
#endif // CONFIG_INS_TYPE #endif // CONFIG_INS_TYPE
@ -240,8 +240,6 @@ static AP_Navigation *nav_controller = &L1_controller;
// Analog Inputs // Analog Inputs
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
static AP_HAL::AnalogSource *pitot_analog_source;
// a pin for reading the receiver RSSI voltage. // a pin for reading the receiver RSSI voltage.
static AP_HAL::AnalogSource *rssi_analog_source; static AP_HAL::AnalogSource *rssi_analog_source;
@ -661,19 +659,11 @@ void setup() {
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE); 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); vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
batt_volt_pin = hal.analogin->channel(g.battery_volt_pin); batt_volt_pin = hal.analogin->channel(g.battery_volt_pin);
batt_curr_pin = hal.analogin->channel(g.battery_curr_pin); batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
airspeed.init(pitot_analog_source);
init_ardupilot(); init_ardupilot();
} }

View File

@ -1919,6 +1919,7 @@ mission_failed:
barometer.setHIL(packet.alt*0.001f); barometer.setHIL(packet.alt*0.001f);
compass.setHIL(packet.roll, packet.pitch, packet.yaw); compass.setHIL(packet.roll, packet.pitch, packet.yaw);
airspeed.disable();
break; break;
} }
#endif // HIL_MODE #endif // HIL_MODE

View File

@ -98,8 +98,6 @@
# define BATTERY_VOLT_PIN 0 // Battery voltage on A0 # define BATTERY_VOLT_PIN 0 // Battery voltage on A0
# define BATTERY_CURR_PIN 1 // Battery current on A1 # define BATTERY_CURR_PIN 1 // Battery current on A1
# define CONFIG_INS_TYPE CONFIG_INS_OILPAN # 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_BARO AP_BARO_BMP085
# define CONFIG_COMPASS AP_COMPASS_HMC5843 # define CONFIG_COMPASS AP_COMPASS_HMC5843
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
@ -116,8 +114,6 @@
# define BATTERY_VOLT_PIN 1 // Battery voltage on A1 # define BATTERY_VOLT_PIN 1 // Battery voltage on A1
# define BATTERY_CURR_PIN 2 // Battery current on A2 # define BATTERY_CURR_PIN 2 // Battery current on A2
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000 # 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 # ifdef APM2_BETA_HARDWARE
# define CONFIG_BARO AP_BARO_BMP085 # define CONFIG_BARO AP_BARO_BMP085
# else // APM2 Production Hardware (default) # else // APM2 Production Hardware (default)
@ -134,8 +130,6 @@
# define BATTERY_VOLT_PIN 1 // Battery voltage on A1 # define BATTERY_VOLT_PIN 1 // Battery voltage on A1
# define BATTERY_CURR_PIN 2 // Battery current on A2 # define BATTERY_CURR_PIN 2 // Battery current on A2
# define CONFIG_INS_TYPE CONFIG_INS_STUB # 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_BARO AP_BARO_HIL
# define CONFIG_COMPASS AP_COMPASS_HIL # define CONFIG_COMPASS AP_COMPASS_HIL
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
@ -148,25 +142,12 @@
# define BATTERY_VOLT_PIN -1 # define BATTERY_VOLT_PIN -1
# define BATTERY_CURR_PIN -1 # define BATTERY_CURR_PIN -1
# define CONFIG_INS_TYPE CONFIG_INS_PX4 # 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_BARO AP_BARO_PX4
# define CONFIG_COMPASS AP_COMPASS_PX4 # define CONFIG_COMPASS AP_COMPASS_PX4
# define SERIAL0_BAUD 115200 # define SERIAL0_BAUD 115200
#endif #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 #ifndef CONFIG_BARO
# error "CONFIG_BARO not set" # error "CONFIG_BARO not set"
#endif #endif
@ -175,10 +156,6 @@
# error "CONFIG_COMPASS not set" # error "CONFIG_COMPASS not set"
#endif #endif
#ifndef CONFIG_PITOT_SOURCE
# error "CONFIG_PITOT_SOURCE not set"
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// HIL_MODE OPTIONAL // HIL_MODE OPTIONAL
@ -193,12 +170,6 @@
#define CONFIG_BARO AP_BARO_HIL #define CONFIG_BARO AP_BARO_HIL
#undef CONFIG_INS_TYPE #undef CONFIG_INS_TYPE
#define CONFIG_INS_TYPE CONFIG_INS_STUB #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 #undef CONFIG_COMPASS
#define CONFIG_COMPASS AP_COMPASS_HIL #define CONFIG_COMPASS AP_COMPASS_HIL
#endif #endif

View File

@ -156,10 +156,13 @@ static void init_ardupilot()
} }
#endif #endif
#if CONFIG_ADC == ENABLED #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
adc.Init(); // APM ADC library initialization apm1_adc.Init(); // APM ADC library initialization
#endif #endif
// initialise airspeed sensor
airspeed.init();
if (g.compass_enabled==true) { if (g.compass_enabled==true) {
if (!compass.init() || !compass.read()) { if (!compass.init() || !compass.read()) {
cliSerial->println_P(PSTR("Compass initialisation failed!")); cliSerial->println_P(PSTR("Compass initialisation failed!"));

View File

@ -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_passthru(uint8_t argc, const Menu::arg *argv);
static int8_t test_failsafe(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); 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); static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
#endif #endif
static int8_t test_ins(uint8_t argc, const Menu::arg *argv); 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 // Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated // when real sensors are attached or they are emulated
#if HIL_MODE == HIL_MODE_DISABLED #if HIL_MODE == HIL_MODE_DISABLED
#if CONFIG_ADC == ENABLED #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
{"adc", test_adc}, {"adc", test_adc},
#endif #endif
{"gps", test_gps}, {"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 // 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 static int8_t
test_adc(uint8_t argc, const Menu::arg *argv) test_adc(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
adc.Init(); apm1_adc.Init();
delay(1000); delay(1000);
cliSerial->printf_P(PSTR("ADC\n")); cliSerial->printf_P(PSTR("ADC\n"));
delay(1000); delay(1000);
while(1) { 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(); cliSerial->println();
delay(100); delay(100);
if(cliSerial->available() > 0) { 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 static int8_t
test_gps(uint8_t argc, const Menu::arg *argv) 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 static int8_t
test_airspeed(uint8_t argc, const Menu::arg *argv) 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()) { if (!airspeed.enabled()) {
cliSerial->printf_P(PSTR("airspeed: ")); cliSerial->printf_P(PSTR("airspeed: "));
print_enabled(false); print_enabled(false);
return (0); return (0);
}else{ }else{
print_hit_enter(); print_hit_enter();
zero_airspeed(); zero_airspeed();