45 #include <px4_arch/adc.h> 46 #include <px4_platform_common/px4_config.h> 47 #include <px4_platform_common/log.h> 48 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> 57 #error "board needs to define ADC_CHANNELS to use this driver" 60 #define ADC_TOTAL_CHANNELS 32 66 ADC(uint32_t base_address, uint32_t channels);
71 virtual int ioctl(
file *filp,
int cmd,
unsigned long arg);
72 virtual ssize_t
read(
file *filp,
char *buffer,
size_t len);
75 virtual int open_first(
struct file *filp);
76 virtual int close_last(
struct file *filp);
87 uint32_t
sample(
unsigned channel);
97 unsigned _channel_count{0};
105 ADC::ADC(uint32_t base_address, uint32_t channels) :
107 ScheduledWorkItem(MODULE_NAME,
px4::wq_configurations::hp_default),
109 _base_address(base_address)
116 if (channels & (1 << i)) {
132 if (channels & (1 << i)) {
157 PX4_DEBUG(
"sample timeout");
181 irqstate_t flags = px4_enter_critical_section();
183 px4_leave_critical_section(flags);
234 for (
unsigned i = 0; i < max_num; i++) {
246 #if defined (BOARD_ADC_USB_CONNECTED) 253 # if defined(ADC_SCALED_V5_SENSE) && defined(ADC_SCALED_V3V3_SENSORS_SENSE) 258 # if defined(ADC_SCALED_V5_SENSE) 260 if (
_samples[i].am_channel == ADC_SCALED_V5_SENSE) {
267 # if defined(ADC_SCALED_V3V3_SENSORS_SENSE) 269 if (
_samples[i].am_channel == ADC_SCALED_V3V3_SENSORS_SENSE) {
272 system_power.v3v3_valid = 1;
290 system_power.usb_connected = BOARD_ADC_USB_CONNECTED;
292 #if defined(BOARD_ADC_USB_VALID) 293 system_power.usb_valid = BOARD_ADC_USB_VALID;
296 system_power.usb_valid = system_power.usb_connected;
300 #if !defined(BOARD_NUMBER_DIGITAL_BRICKS) 301 bool valid_chan[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
302 system_power.brick_valid = 0;
304 for (
int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
305 system_power.brick_valid |= valid_chan[b] ? 1 << b : 0;
310 system_power.servo_valid = BOARD_ADC_SERVO_VALID;
312 #ifdef BOARD_ADC_PERIPH_5V_OC 314 system_power.periph_5v_oc = BOARD_ADC_PERIPH_5V_OC;
317 #ifdef BOARD_ADC_HIPOWER_5V_OC 318 system_power.hipower_5v_oc = BOARD_ADC_HIPOWER_5V_OC;
324 #endif // BOARD_ADC_USB_CONNECTED 333 if (result == UINT32_MAX) {
334 PX4_ERR(
"sample timeout");
357 PX4_ERR(
"can't open ADC device %d", errno);
361 for (
unsigned i = 0; i < 20; i++) {
363 ssize_t count =
read(fd, data,
sizeof(data));
366 PX4_ERR(
"read error");
370 unsigned channels = count /
sizeof(data[0]);
372 for (
unsigned j = 0; j < channels; j++) {
373 printf(
"%d: %u ", data[j].am_channel, data[j].am_data);
387 if (g_adc ==
nullptr) {
388 g_adc =
new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS);
390 if (g_adc ==
nullptr) {
391 PX4_ERR(
"couldn't allocate the ADC driver");
395 if (g_adc->init() !=
OK) {
397 PX4_ERR(
"ADC init failed");
403 if (!strcmp(argv[1],
"test")) {
virtual ssize_t read(file *filp, char *buffer, size_t len)
uORB::Publication< system_power_s > _to_system_power
virtual int open(file_t *filep)
Handle an open of the device.
measure the time elapsed performing an event
void px4_arch_adc_uninit(uint32_t base_address)
Uninitialize ADC hardware.
#define PX4_MAX_ADC_CHANNELS
static const hrt_abstime kINTERVAL
100Hz base rate
uint32_t px4_arch_adc_dn_fullcount(void)
Get the adc digital number full count.
High-resolution timer with callouts and timekeeping.
void update_system_power(hrt_abstime now)
px4_adc_msg_t * _samples
sample buffer
#define ADC_TOTAL_CHANNELS
static void read(bootloader_app_shared_t *pshared)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
Read a sample from the ADC.
uORB::Publication< adc_report_s > _to_adc_report
bool publish(const T &data)
Publish the struct.
Abstract class for any character device.
virtual int open_first(struct file *filp)
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
__BEGIN_DECLS int px4_arch_adc_init(uint32_t base_address)
Initialize ADC hardware.
__EXPORT int adc_main(int argc, char *argv[])
void perf_end(perf_counter_t handle)
End a performance event.
void test(enum LPS25H_BUS busid)
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
void update_adc_report(hrt_abstime now)
ADC(uint32_t base_address, uint32_t channels)
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
uint32_t sample(unsigned channel)
Sample a single channel and return the measured value.
struct @83::@85::@87 file
virtual int ioctl(file *filp, int cmd, unsigned long arg)
perf_counter_t _sample_perf
uint32_t px4_arch_adc_temp_sensor_mask(void)
Get the temperature sensor channel bitmask.
const uint32_t _base_address
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
virtual int close_last(struct file *filp)
Performance measuring tools.