Plane: removed scaling of pitot source

This commit is contained in:
Andrew Tridgell 2013-05-13 15:15:16 +10:00
parent 97b6aaac77
commit 001d18b51d
3 changed files with 5 additions and 9 deletions

View File

@ -665,9 +665,10 @@ void setup() {
#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC #if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC
pitot_analog_source = new AP_ADC_AnalogSource( &adc, pitot_analog_source = new AP_ADC_AnalogSource( &adc,
CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0); CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0f);
#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN #elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN
pitot_analog_source = hal.analogin->channel(CONFIG_PITOT_SOURCE_ANALOG_PIN, CONFIG_PITOT_SCALING); pitot_analog_source = hal.analogin->channel(CONFIG_PITOT_SOURCE_ANALOG_PIN);
hal.gpio->write(hal.gpio->analogPinToDigitalPin(CONFIG_PITOT_SOURCE_ANALOG_PIN), 0);
#endif #endif
vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC); vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);

View File

@ -118,7 +118,6 @@
# 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 PITOT_SOURCE_ANALOG_PIN
# define CONFIG_PITOT_SOURCE_ANALOG_PIN 0 # define CONFIG_PITOT_SOURCE_ANALOG_PIN 0
# define CONFIG_PITOT_SCALING 4.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)
@ -137,7 +136,6 @@
# 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 PITOT_SOURCE_ANALOG_PIN
# define CONFIG_PITOT_SOURCE_ANALOG_PIN 0 # define CONFIG_PITOT_SOURCE_ANALOG_PIN 0
# define CONFIG_PITOT_SCALING 4.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
@ -152,7 +150,6 @@
# 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 PITOT_SOURCE_ANALOG_PIN
# define CONFIG_PITOT_SOURCE_ANALOG_PIN 11 # define CONFIG_PITOT_SOURCE_ANALOG_PIN 11
# define CONFIG_PITOT_SCALING (4.0*5.0/3.3)
# 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
@ -202,8 +199,6 @@
#define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN #define CONFIG_PITOT_SOURCE PITOT_SOURCE_ANALOG_PIN
#undef CONFIG_PITOT_SOURCE_ANALOG_PIN #undef CONFIG_PITOT_SOURCE_ANALOG_PIN
#define CONFIG_PITOT_SOURCE_ANALOG_PIN -1 #define CONFIG_PITOT_SOURCE_ANALOG_PIN -1
#undef CONFIG_PITOT_SCALING
#define CONFIG_PITOT_SCALING 4.0
#undef CONFIG_COMPASS #undef CONFIG_COMPASS
#define CONFIG_COMPASS AP_COMPASS_HIL #define CONFIG_COMPASS AP_COMPASS_HIL
#endif #endif

View File

@ -606,9 +606,9 @@ 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->read_average(); float airspeed_ch = pitot_analog_source->voltage_average();
// cliSerial->println(pitot_analog_source.read()); // cliSerial->println(pitot_analog_source.read());
cliSerial->printf_P(PSTR("airspeed_ch: %.1f\n"), airspeed_ch); 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: "));