mirror of https://github.com/ArduPilot/ardupilot
HAL_Chibios: optional ADC debugging
this debugging capability is very useful on board bringup. It outputs the first 6 analog channels as an AP_ADC MAVLink message
This commit is contained in:
parent
c863681849
commit
ba3cdf74b5
|
@ -23,6 +23,13 @@
|
||||||
extern AP_IOMCU iomcu;
|
extern AP_IOMCU iomcu;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef CHIBIOS_ADC_MAVLINK_DEBUG
|
||||||
|
// this allows the first 6 analog channels to be reported by mavlink for debugging purposes
|
||||||
|
#define CHIBIOS_ADC_MAVLINK_DEBUG 0
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||||
|
|
||||||
#define ANLOGIN_DEBUGGING 0
|
#define ANLOGIN_DEBUGGING 0
|
||||||
|
|
||||||
// base voltage scaling for 12 bit 3.3V ADC
|
// base voltage scaling for 12 bit 3.3V ADC
|
||||||
|
@ -274,6 +281,18 @@ void AnalogIn::_timer_tick(void)
|
||||||
// now handle special inputs from IOMCU
|
// now handle special inputs from IOMCU
|
||||||
_servorail_voltage = iomcu.get_vservo();
|
_servorail_voltage = iomcu.get_vservo();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if CHIBIOS_ADC_MAVLINK_DEBUG
|
||||||
|
static uint8_t count;
|
||||||
|
if (AP_HAL::millis() > 5000 && count++ == 10) {
|
||||||
|
count = 0;
|
||||||
|
uint16_t adc[6] {};
|
||||||
|
for (uint8_t i=0; i < ADC_GRP1_NUM_CHANNELS; i++) {
|
||||||
|
adc[i] = buf_adc[i];
|
||||||
|
}
|
||||||
|
mavlink_msg_ap_adc_send(MAVLINK_COMM_0, adc[0], adc[1], adc[2], adc[3], adc[4], adc[5]);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
AP_HAL::AnalogSource* AnalogIn::channel(int16_t pin)
|
AP_HAL::AnalogSource* AnalogIn::channel(int16_t pin)
|
||||||
|
|
Loading…
Reference in New Issue