mirror of https://github.com/ArduPilot/ardupilot
Plane: use the new airspeed driver interface
the choice of analog source is now in the airspeed driver
This commit is contained in:
parent
68adeb041d
commit
f7697574db
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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!"));
|
||||
|
|
|
@ -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();
|
||||
|
|
Loading…
Reference in New Issue