AP_IOMCU: fix ADC scaling on IOMCU

This commit is contained in:
TunaLobster 2021-09-28 11:40:08 -05:00 committed by Andrew Tridgell
parent 11f98f7421
commit db6383eee7
2 changed files with 6 additions and 6 deletions

View File

@ -74,14 +74,14 @@ public:
}
/*
get servo rail voltage
get servo rail voltage adc counts
*/
float get_vservo(void) const { return reg_status.vservo * 0.001; }
uint16_t get_vservo_adc_count(void) const { return reg_status.vservo; }
/*
get rssi voltage
get rssi voltage adc counts
*/
float get_vrssi(void) const { return reg_status.vrssi * 0.001; }
uint16_t get_vrssi_adc_count(void) const { return reg_status.vrssi; }
// set target for IMU heater
void set_heater_duty_cycle(uint8_t duty_cycle);

View File

@ -75,7 +75,7 @@ static uint16_t adc_sample_channel(uint32_t channel)
uint16_t adc_sample_vservo(void)
{
const uint32_t channel = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN4);
return adc_sample_channel(channel) * 9900 / 4096;
return adc_sample_channel(channel);
}
/*
@ -84,5 +84,5 @@ uint16_t adc_sample_vservo(void)
uint16_t adc_sample_vrssi(void)
{
const uint32_t channel = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN5);
return adc_sample_channel(channel) * 9900 / 4096;
return adc_sample_channel(channel);
}