forked from Archive/PX4-Autopilot
PX4 System:Expunge the nuttx adc structure from the system
This PR is preliminary ground work for FMUv5. PX4 does not use the NuttX adc driver. But used the same format for the data returned by the nuttx ADC driver. There was a fixme:in src/platforms/px4_adc.h "this needs to be a px4_adc_msg_s type" With this PR the need for src/platforms/px4_adc.h goes away as the driver drv_adc.h now describes the px4_adc_msg_t.
This commit is contained in:
parent
d92377a6e6
commit
4349f49610
|
@ -1,2 +1,2 @@
|
||||||
int16[8] channel_id # ADC channel IDs, negative for non-existent
|
int16[12] channel_id # ADC channel IDs, negative for non-existent
|
||||||
float32[8] channel_value # ADC channel value in volt, valid if channel ID is positive
|
float32[12] channel_value # ADC channel value in volt, valid if channel ID is positive
|
||||||
|
|
|
@ -73,7 +73,7 @@ private:
|
||||||
static void cycle_trampoline(void *arg);
|
static void cycle_trampoline(void *arg);
|
||||||
|
|
||||||
struct work_s _work;
|
struct work_s _work;
|
||||||
adc_msg_s _sample;
|
px4_adc_msg_t _sample;
|
||||||
pthread_mutex_t _sample_mutex;
|
pthread_mutex_t _sample_mutex;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -87,7 +87,7 @@ static void test()
|
||||||
err(1, "can't open ADC device");
|
err(1, "can't open ADC device");
|
||||||
}
|
}
|
||||||
|
|
||||||
adc_msg_s data[MAX_CHANNEL];
|
px4_adc_msg_t data[MAX_CHANNEL];
|
||||||
ssize_t count = read(fd, data, sizeof(data));
|
ssize_t count = read(fd, data, sizeof(data));
|
||||||
|
|
||||||
if (count < 0) {
|
if (count < 0) {
|
||||||
|
|
|
@ -45,6 +45,19 @@
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <sys/ioctl.h>
|
#include <sys/ioctl.h>
|
||||||
|
|
||||||
|
/* Define the PX4 low level format ADC and the maximum
|
||||||
|
* number of channels that can be returned by a lowlevel
|
||||||
|
* ADC driver. Drivers may return less than PX4_MAX_ADC_CHANNELS
|
||||||
|
* but no more than PX4_MAX_ADC_CHANNELS.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
#define PX4_MAX_ADC_CHANNELS 12
|
||||||
|
typedef struct __attribute__((packed)) px4_adc_msg_t {
|
||||||
|
uint8_t am_channel; /* The 8-bit ADC Channel */
|
||||||
|
int32_t am_data; /* ADC convert result (4 bytes) */
|
||||||
|
} px4_adc_msg_t;
|
||||||
|
|
||||||
|
|
||||||
#define ADC0_DEVICE_PATH "/dev/adc0"
|
#define ADC0_DEVICE_PATH "/dev/adc0"
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -44,7 +44,6 @@
|
||||||
#include <px4_config.h>
|
#include <px4_config.h>
|
||||||
#include <px4_tasks.h>
|
#include <px4_tasks.h>
|
||||||
#include <px4_posix.h>
|
#include <px4_posix.h>
|
||||||
#include <px4_adc.h>
|
|
||||||
#include <drivers/drv_adc.h>
|
#include <drivers/drv_adc.h>
|
||||||
|
|
||||||
#include <VirtDevObj.hpp>
|
#include <VirtDevObj.hpp>
|
||||||
|
@ -90,11 +89,11 @@ protected:
|
||||||
virtual void _measure() override;
|
virtual void _measure() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
int read_channel(struct adc_msg_s *adc_msg, int channel);
|
int read_channel(px4_adc_msg_t *adc_msg, int channel);
|
||||||
|
|
||||||
pthread_mutex_t _samples_lock;
|
pthread_mutex_t _samples_lock;
|
||||||
int _fd[ADC_MAX_CHAN];
|
int _fd[ADC_MAX_CHAN];
|
||||||
adc_msg_s _samples[ADC_MAX_CHAN];
|
px4_adc_msg_t _samples[ADC_MAX_CHAN];
|
||||||
};
|
};
|
||||||
|
|
||||||
NavioADC::NavioADC()
|
NavioADC::NavioADC()
|
||||||
|
@ -120,7 +119,7 @@ NavioADC::~NavioADC()
|
||||||
|
|
||||||
void NavioADC::_measure()
|
void NavioADC::_measure()
|
||||||
{
|
{
|
||||||
adc_msg_s tmp_samples[ADC_MAX_CHAN];
|
px4_adc_msg_t tmp_samples[ADC_MAX_CHAN];
|
||||||
|
|
||||||
for (int i = 0; i < ADC_MAX_CHAN; ++i) {
|
for (int i = 0; i < ADC_MAX_CHAN; ++i) {
|
||||||
int ret;
|
int ret;
|
||||||
|
@ -216,7 +215,7 @@ ssize_t NavioADC::devRead(void *buf, size_t count)
|
||||||
return count;
|
return count;
|
||||||
}
|
}
|
||||||
|
|
||||||
int NavioADC::read_channel(struct adc_msg_s *adc_msg, int channel)
|
int NavioADC::read_channel(px4_adc_msg_t *adc_msg, int channel)
|
||||||
{
|
{
|
||||||
char buffer[11]; /* 32bit max INT has maximum 10 chars */
|
char buffer[11]; /* 32bit max INT has maximum 10 chars */
|
||||||
int ret;
|
int ret;
|
||||||
|
@ -304,7 +303,7 @@ int navio_adc_main(int argc, char *argv[])
|
||||||
return PX4_ERROR;
|
return PX4_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct adc_msg_s adc_msgs[ADC_MAX_CHAN];
|
px4_adc_msg_t adc_msgs[ADC_MAX_CHAN];
|
||||||
|
|
||||||
ret = instance->devRead((char *)&adc_msgs, sizeof(adc_msgs));
|
ret = instance->devRead((char *)&adc_msgs, sizeof(adc_msgs));
|
||||||
|
|
||||||
|
|
|
@ -43,7 +43,6 @@
|
||||||
#include <px4_config.h>
|
#include <px4_config.h>
|
||||||
#include <px4_tasks.h>
|
#include <px4_tasks.h>
|
||||||
#include <px4_posix.h>
|
#include <px4_posix.h>
|
||||||
#include <px4_adc.h>
|
|
||||||
#include <drivers/drv_adc.h>
|
#include <drivers/drv_adc.h>
|
||||||
|
|
||||||
#include <VirtDevObj.hpp>
|
#include <VirtDevObj.hpp>
|
||||||
|
@ -75,10 +74,10 @@ protected:
|
||||||
virtual void _measure() override;
|
virtual void _measure() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
int read(struct adc_msg_s(*buf)[12], unsigned int len);
|
int read(px4_adc_msg_t(*buf)[PX4_MAX_ADC_CHANNELS], unsigned int len);
|
||||||
|
|
||||||
pthread_mutex_t _samples_lock;
|
pthread_mutex_t _samples_lock;
|
||||||
adc_msg_s _samples;
|
px4_adc_msg_t _samples;
|
||||||
};
|
};
|
||||||
|
|
||||||
OcpocADC::OcpocADC()
|
OcpocADC::OcpocADC()
|
||||||
|
@ -94,7 +93,7 @@ OcpocADC::~OcpocADC()
|
||||||
|
|
||||||
void OcpocADC::_measure()
|
void OcpocADC::_measure()
|
||||||
{
|
{
|
||||||
struct adc_msg_s tmp_samples[12];
|
px4_adc_msg_t tmp_samples[PX4_MAX_ADC_CHANNELS];
|
||||||
|
|
||||||
int ret = read(&tmp_samples, sizeof(tmp_samples));
|
int ret = read(&tmp_samples, sizeof(tmp_samples));
|
||||||
|
|
||||||
|
@ -147,7 +146,7 @@ ssize_t OcpocADC::devRead(void *buf, size_t count)
|
||||||
return count;
|
return count;
|
||||||
}
|
}
|
||||||
|
|
||||||
int OcpocADC::read(struct adc_msg_s(*buf)[12], unsigned int len)
|
int OcpocADC::read(px4_adc_msg_t(*buf)[PX4_MAX_ADC_CHANNELS], unsigned int len)
|
||||||
{
|
{
|
||||||
uint32_t buff[1];
|
uint32_t buff[1];
|
||||||
int ret = 0;
|
int ret = 0;
|
||||||
|
@ -219,7 +218,7 @@ int ocpoc_adc_main(int argc, char *argv[])
|
||||||
return PX4_ERROR;
|
return PX4_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct adc_msg_s adc_msgs[12];
|
px4_adc_msg_t adc_msgs[PX4_MAX_ADC_CHANNELS];
|
||||||
|
|
||||||
ret = instance->devRead((char *)&adc_msgs, sizeof(adc_msgs));
|
ret = instance->devRead((char *)&adc_msgs, sizeof(adc_msgs));
|
||||||
|
|
||||||
|
|
|
@ -122,7 +122,7 @@ private:
|
||||||
perf_counter_t _sample_perf;
|
perf_counter_t _sample_perf;
|
||||||
|
|
||||||
unsigned _channel_count;
|
unsigned _channel_count;
|
||||||
adc_msg_s *_samples; /**< sample buffer */
|
px4_adc_msg_t *_samples; /**< sample buffer */
|
||||||
|
|
||||||
orb_advert_t _to_system_power;
|
orb_advert_t _to_system_power;
|
||||||
orb_advert_t _to_adc_report;
|
orb_advert_t _to_adc_report;
|
||||||
|
@ -168,7 +168,11 @@ ADC::ADC(uint32_t channels) :
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
_samples = new adc_msg_s[_channel_count];
|
if (_channel_count > PX4_MAX_ADC_CHANNELS) {
|
||||||
|
PX4_ERR("PX4_MAX_ADC_CHANNELS is too small:is %d needed:%d", PX4_MAX_ADC_CHANNELS, _channel_count);
|
||||||
|
}
|
||||||
|
|
||||||
|
_samples = new px4_adc_msg_t[_channel_count];
|
||||||
|
|
||||||
/* prefill the channel numbers in the sample array */
|
/* prefill the channel numbers in the sample array */
|
||||||
if (_samples != nullptr) {
|
if (_samples != nullptr) {
|
||||||
|
@ -265,7 +269,7 @@ ADC::ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
ssize_t
|
ssize_t
|
||||||
ADC::read(file *filp, char *buffer, size_t len)
|
ADC::read(file *filp, char *buffer, size_t len)
|
||||||
{
|
{
|
||||||
const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
|
const size_t maxsize = sizeof(px4_adc_msg_t) * _channel_count;
|
||||||
|
|
||||||
if (len > maxsize) {
|
if (len > maxsize) {
|
||||||
len = maxsize;
|
len = maxsize;
|
||||||
|
@ -441,7 +445,7 @@ test(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
for (unsigned i = 0; i < 50; i++) {
|
for (unsigned i = 0; i < 50; i++) {
|
||||||
adc_msg_s data[12];
|
px4_adc_msg_t data[PX4_MAX_ADC_CHANNELS];
|
||||||
ssize_t count = read(fd, data, sizeof(data));
|
ssize_t count = read(fd, data, sizeof(data));
|
||||||
|
|
||||||
if (count < 0) {
|
if (count < 0) {
|
||||||
|
|
|
@ -43,7 +43,6 @@
|
||||||
|
|
||||||
#include <board_config.h>
|
#include <board_config.h>
|
||||||
|
|
||||||
#include <px4_adc.h>
|
|
||||||
#include <px4_config.h>
|
#include <px4_config.h>
|
||||||
#include <px4_module.h>
|
#include <px4_module.h>
|
||||||
#include <px4_getopt.h>
|
#include <px4_getopt.h>
|
||||||
|
@ -422,7 +421,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
||||||
/* rate limit to 100 Hz */
|
/* rate limit to 100 Hz */
|
||||||
if (t - _last_adc >= 10000) {
|
if (t - _last_adc >= 10000) {
|
||||||
/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
|
/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
|
||||||
struct adc_msg_s buf_adc[12];
|
px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS];
|
||||||
/* read all channels available */
|
/* read all channels available */
|
||||||
int ret = _h_adc.read(&buf_adc, sizeof(buf_adc));
|
int ret = _h_adc.read(&buf_adc, sizeof(buf_adc));
|
||||||
|
|
||||||
|
|
|
@ -42,7 +42,6 @@
|
||||||
|
|
||||||
#include <px4_config.h>
|
#include <px4_config.h>
|
||||||
#include <px4_time.h>
|
#include <px4_time.h>
|
||||||
#include <px4_adc.h>
|
|
||||||
#include <board_config.h>
|
#include <board_config.h>
|
||||||
#include <drivers/device/device.h>
|
#include <drivers/device/device.h>
|
||||||
|
|
||||||
|
@ -84,7 +83,7 @@ private:
|
||||||
perf_counter_t _sample_perf;
|
perf_counter_t _sample_perf;
|
||||||
|
|
||||||
unsigned _channel_count;
|
unsigned _channel_count;
|
||||||
adc_msg_s *_samples; /**< sample buffer */
|
px4_adc_msg_t *_samples; /**< sample buffer */
|
||||||
|
|
||||||
/** worker function */
|
/** worker function */
|
||||||
virtual void _measure();
|
virtual void _measure();
|
||||||
|
@ -119,7 +118,7 @@ ADCSIM::ADCSIM(uint32_t channels) :
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
_samples = new adc_msg_s[_channel_count];
|
_samples = new px4_adc_msg_t[_channel_count];
|
||||||
|
|
||||||
/* prefill the channel numbers in the sample array */
|
/* prefill the channel numbers in the sample array */
|
||||||
if (_samples != nullptr) {
|
if (_samples != nullptr) {
|
||||||
|
@ -145,7 +144,7 @@ ADCSIM::~ADCSIM()
|
||||||
ssize_t
|
ssize_t
|
||||||
ADCSIM::devRead(void *buffer, size_t len)
|
ADCSIM::devRead(void *buffer, size_t len)
|
||||||
{
|
{
|
||||||
const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
|
const size_t maxsize = sizeof(px4_adc_msg_t) * _channel_count;
|
||||||
|
|
||||||
if (len > maxsize) {
|
if (len > maxsize) {
|
||||||
len = maxsize;
|
len = maxsize;
|
||||||
|
@ -204,7 +203,7 @@ test()
|
||||||
}
|
}
|
||||||
|
|
||||||
for (unsigned i = 0; i < 50; i++) {
|
for (unsigned i = 0; i < 50; i++) {
|
||||||
adc_msg_s data[12];
|
px4_adc_msg_t data[PX4_MAX_ADC_CHANNELS];
|
||||||
ssize_t count = h.read(data, sizeof(data));
|
ssize_t count = h.read(data, sizeof(data));
|
||||||
|
|
||||||
if (count < 0) {
|
if (count < 0) {
|
||||||
|
|
|
@ -39,6 +39,11 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
|
||||||
|
#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1))
|
||||||
|
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
|
||||||
|
#define ADCSIM0_DEVICE_PATH "/dev/adc0"
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* I2C busses
|
* I2C busses
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -1,67 +0,0 @@
|
||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* 1. Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* 2. Redistributions in binary form must reproduce the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer in
|
|
||||||
* the documentation and/or other materials provided with the
|
|
||||||
* distribution.
|
|
||||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
||||||
* used to endorse or promote products derived from this software
|
|
||||||
* without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
||||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
||||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file px4_adc.h
|
|
||||||
*
|
|
||||||
* ADC header depending on the build target
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
#if defined(__PX4_ROS)
|
|
||||||
#error "ADC not supported in ROS"
|
|
||||||
#elif defined(__PX4_NUTTX)
|
|
||||||
/*
|
|
||||||
* Building for NuttX
|
|
||||||
*/
|
|
||||||
#include <nuttx/analog/adc.h>
|
|
||||||
#elif defined(__PX4_POSIX)
|
|
||||||
|
|
||||||
// FIXME - this needs to be a px4_adc_msg_s type
|
|
||||||
// Curently copied from NuttX
|
|
||||||
struct adc_msg_s {
|
|
||||||
uint8_t am_channel; /* The 8-bit ADC Channel */
|
|
||||||
int32_t am_data; /* ADC convert result (4 bytes) */
|
|
||||||
} __attribute__((packed));
|
|
||||||
|
|
||||||
// Example settings
|
|
||||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
|
|
||||||
#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1))
|
|
||||||
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
|
|
||||||
#define ADCSIM0_DEVICE_PATH "/dev/adc0"
|
|
||||||
#else
|
|
||||||
#error "No target platform defined"
|
|
||||||
#endif
|
|
|
@ -37,7 +37,6 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <px4_config.h>
|
#include <px4_config.h>
|
||||||
#include <px4_adc.h>
|
|
||||||
#include <px4_posix.h>
|
#include <px4_posix.h>
|
||||||
#include <px4_log.h>
|
#include <px4_log.h>
|
||||||
|
|
||||||
|
@ -63,8 +62,8 @@ int test_adc(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
for (unsigned i = 0; i < 5; i++) {
|
for (unsigned i = 0; i < 5; i++) {
|
||||||
/* make space for a maximum of twelve channels */
|
/* make space for a maximum number of channels */
|
||||||
struct adc_msg_s data[12];
|
px4_adc_msg_t data[PX4_MAX_ADC_CHANNELS];
|
||||||
/* read all channels available */
|
/* read all channels available */
|
||||||
ssize_t count = px4_read(fd, data, sizeof(data));
|
ssize_t count = px4_read(fd, data, sizeof(data));
|
||||||
|
|
||||||
|
|
|
@ -47,7 +47,6 @@
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <errno.h>
|
#include <errno.h>
|
||||||
|
|
||||||
#include <px4_adc.h>
|
|
||||||
|
|
||||||
#include "tests_main.h"
|
#include "tests_main.h"
|
||||||
|
|
||||||
|
|
|
@ -48,7 +48,6 @@
|
||||||
|
|
||||||
#include "tests_main.h"
|
#include "tests_main.h"
|
||||||
|
|
||||||
#include <px4_adc.h>
|
|
||||||
#include <drivers/drv_adc.h>
|
#include <drivers/drv_adc.h>
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
|
||||||
|
@ -95,8 +94,9 @@ int test_jig_voltages(int argc, char *argv[])
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* make space for a maximum of eight channels */
|
/* make space for the maximum channels */
|
||||||
struct adc_msg_s data[8];
|
px4_adc_msg_t data[PX4_MAX_ADC_CHANNELS];
|
||||||
|
|
||||||
/* read all channels available */
|
/* read all channels available */
|
||||||
ssize_t count = read(fd, data, sizeof(data));
|
ssize_t count = read(fd, data, sizeof(data));
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue