diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp index 2bd608a367..539acb7665 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp @@ -36,7 +36,17 @@ extern AP_IOMCU iomcu; #define ANLOGIN_DEBUGGING 0 // base voltage scaling for 12 bit 3.3V ADC -#define VOLTAGE_SCALING (3.3f/(1<<12)) +#define VOLTAGE_SCALING (3.3f / ((1 << 12) - 1)) + +// voltage divider is usually 1/(10/(20+10)) +#ifndef HAL_IOMCU_VSERVO_SCALAR + #define HAL_IOMCU_VSERVO_SCALAR 3 +#endif + +// voltage divider is usually not present +#ifndef HAL_IOMCU_VRSSI_SCALAR + #define HAL_IOMCU_VRSSI_SCALAR 1 +#endif #if ANLOGIN_DEBUGGING # define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) @@ -462,8 +472,8 @@ void AnalogIn::_timer_tick(void) #if HAL_WITH_IO_MCU // now handle special inputs from IOMCU - _servorail_voltage = iomcu.get_vservo(); - _rssi_voltage = iomcu.get_vrssi(); + _servorail_voltage = iomcu.get_vservo_adc_count() * (VOLTAGE_SCALING * HAL_IOMCU_VSERVO_SCALAR); + _rssi_voltage = iomcu.get_vrssi_adc_count() * (VOLTAGE_SCALING * HAL_IOMCU_VRSSI_SCALAR); #endif for (uint8_t i=0; i