fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

aerfofc_adc: move to PX4 WQ and cleanup

Browse Source
This commit is contained in:
Daniel Agar 2020-01-09 10:44:42 -05:00 committed by GitHub
parent d32a80df3a
commit 2bbf73097b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 229 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.cpp Normal file
View File

@ -0,0 +1,134 @@
/****************************************************************************
*
* Copyright (C) 2016 Intel Corporation. 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.
*
****************************************************************************/
#include "AEROFC_ADC.hpp"
using namespace time_literals;
AEROFC_ADC::AEROFC_ADC(uint8_t bus) :
I2C("AEROFC_ADC", ADC0_DEVICE_PATH, bus, SLAVE_ADDR, 400000),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample"))
{
_sample.am_channel = 1;
pthread_mutex_init(&_sample_mutex, nullptr);
}
AEROFC_ADC::~AEROFC_ADC()
{
ScheduleClear();
perf_free(_sample_perf);
}
int AEROFC_ADC::init()
{
int ret = I2C::init();
if (ret != PX4_OK) {
return ret;
}
ScheduleOnInterval(100_ms); // 10 Hz
return PX4_OK;
}
int AEROFC_ADC::probe()
{
uint8_t buffer[2];
int ret;
_retries = 3;
/* Enable ADC */
buffer[0] = ADC_ENABLE_REG;
buffer[1] = 0x01;
ret = transfer(buffer, 2, NULL, 0);
if (ret != PX4_OK) {
goto error;
}
usleep(10000);
/* Read ADC value */
buffer[0] = ADC_CHANNEL_REG;
ret = transfer(buffer, 1, buffer, 2);
if (ret != PX4_OK) {
goto error;
}
return PX4_OK;
error:
return -EIO;
}
ssize_t AEROFC_ADC::read(file *filp, char *buffer, size_t len)
{
if (len < sizeof(_sample)) {
return -ENOSPC;
}
if (len > sizeof(_sample)) {
len = sizeof(_sample);
}
pthread_mutex_lock(&_sample_mutex);
memcpy(buffer, &_sample, len);
pthread_mutex_unlock(&_sample_mutex);
return len;
}
void AEROFC_ADC::Run()
{
uint8_t buffer[2] {};
buffer[0] = ADC_CHANNEL_REG;
int ret = transfer(buffer, 1, buffer, sizeof(buffer));
if (ret != PX4_OK) {
PX4_ERR("Error reading sample");
return;
}
pthread_mutex_lock(&_sample_mutex);
_sample.am_data = (buffer[0] | (buffer[1] << 8));
pthread_mutex_unlock(&_sample_mutex);
}
uint32_t px4_arch_adc_dn_fullcount()
{
return 1 << 12; // 12 bit ADC
}

70
boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.hpp Normal file
View File

@ -0,0 +1,70 @@
/****************************************************************************
*
* Copyright (C) 2016 Intel Corporation. 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.
*
****************************************************************************/
#pragma once
#include <drivers/drv_adc.h>
#include <drivers/drv_hrt.h>
#include <lib/cdev/CDev.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_arch/adc.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_adc.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#define SLAVE_ADDR 0x50
#define ADC_ENABLE_REG 0x00
#define ADC_CHANNEL_REG 0x05
#define MAX_CHANNEL 5
class AEROFC_ADC : public device::I2C, public px4::ScheduledWorkItem
{
public:
AEROFC_ADC(uint8_t bus);
~AEROFC_ADC() override;
int init() override;
ssize_t read(file *filp, char *buffer, size_t len) override;
private:
int probe() override;;
void Run() override;
px4_adc_msg_t _sample{};
pthread_mutex_t _sample_mutex;
perf_counter_t _sample_perf;
};

10
boards/intel/aerofc-v1/aerofc_adc/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (C) 2016 Intel Corporation. All rights reserved.
# Copyright (C) 2016 Intel Corporation. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -31,11 +31,13 @@
#
############################################################################
px4_add_module(
MODULE drivers__aerofc_adc
MODULE aerofc_adc
MAIN aerofc_adc
COMPILE_FLAGS
SRCS
aerofc_adc.cpp
aerofc_adc_main.cpp
AEROFC_ADC.cpp
AEROFC_ADC.hpp
DEPENDS
drivers__device
px4_work_queue
)

191
boards/intel/aerofc-v1/aerofc_adc/aerofc_adc.cpp → boards/intel/aerofc-v1/aerofc_adc/aerofc_adc_main.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2016 Intel Corporation. All rights reserved.
* Copyright (C) 2016 Intel Corporation. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -32,35 +32,9 @@
****************************************************************************/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/getopt.h>
#include <cstring>
#include <arch/board/board.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <board_config.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_adc.h>
#include <systemlib/err.h>
#define SLAVE_ADDR 0x50
#define ADC_ENABLE_REG 0x00
#define ADC_CHANNEL_REG 0x05
#define MAX_CHANNEL 5
// 10Hz
#define CYCLE_TICKS_DELAY MSEC2TICK(100)
uint32_t px4_arch_adc_dn_fullcount()
{
return 1 << 12; // 12 bit ADC
}
#include "AEROFC_ADC.hpp"
enum AEROFC_ADC_BUS {
AEROFC_ADC_BUS_ALL = 0,
@ -85,42 +59,24 @@ static constexpr struct aerofc_adc_bus_option {
extern "C" { __EXPORT int aerofc_adc_main(int argc, char *argv[]); }
class AEROFC_ADC : public device::I2C
{
public:
AEROFC_ADC(uint8_t bus);
virtual ~AEROFC_ADC();
virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t read(file *filp, char *buffer, size_t len);
private:
virtual int probe();
void cycle();
static void cycle_trampoline(void *arg);
struct work_s _work;
px4_adc_msg_t _sample;
pthread_mutex_t _sample_mutex;
};
static AEROFC_ADC *instance = nullptr;
static void test()
static int test()
{
int fd = open(ADC0_DEVICE_PATH, O_RDONLY);
int fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "can't open ADC device");
PX4_ERR("can't open ADC device");
return PX4_ERROR;
}
px4_adc_msg_t data[MAX_CHANNEL];
ssize_t count = read(fd, data, sizeof(data));
ssize_t count = px4_read(fd, data, sizeof(data));
if (count < 0) {
errx(1, "read error");
PX4_ERR("read error");
px4_close(fd);
return PX4_ERROR;
}
unsigned channels = count / sizeof(data[0]);
@ -130,8 +86,9 @@ static void test()
}
printf("\n");
px4_close(fd);
exit(0);
return 0;
}
static void help()
@ -172,7 +129,7 @@ int aerofc_adc_main(int argc, char *argv[])
if (!strcmp(verb, "start")) {
if (instance) {
warn("AEROFC_ADC was already started");
PX4_WARN("AEROFC_ADC was already started");
return PX4_OK;
}
@ -184,7 +141,7 @@ int aerofc_adc_main(int argc, char *argv[])
instance = new AEROFC_ADC(bus_options[i].busnum);
if (!instance) {
warn("No memory to instance AEROFC_ADC");
PX4_WARN("No memory to instance AEROFC_ADC");
return PX4_ERROR;
}
@ -192,134 +149,24 @@ int aerofc_adc_main(int argc, char *argv[])
break;
}
warn("AEROFC_ADC not found on busnum=%u", bus_options[i].busnum);
PX4_WARN("AEROFC_ADC not found on busnum=%u", bus_options[i].busnum);
delete instance;
instance = nullptr;
}
if (!instance) {
warn("AEROFC_ADC not found");
PX4_WARN("AEROFC_ADC not found");
return PX4_ERROR;
}
} else if (!strcmp(verb, "test")) {
test();
return test();
} else {
warn("Action not supported");
PX4_WARN("Action not supported");
help();
return PX4_ERROR;
}
return PX4_OK;
}
AEROFC_ADC::AEROFC_ADC(uint8_t bus) :
I2C("AEROFC_ADC", ADC0_DEVICE_PATH, bus, SLAVE_ADDR, 400000),
_sample{}
{
_sample.am_channel = 1;
pthread_mutex_init(&_sample_mutex, NULL);
}
AEROFC_ADC::~AEROFC_ADC()
{
work_cancel(HPWORK, &_work);
}
int AEROFC_ADC::init()
{
int ret = I2C::init();
if (ret != PX4_OK) {
return ret;
}
work_queue(HPWORK, &_work, (worker_t)&AEROFC_ADC::cycle_trampoline, this,
CYCLE_TICKS_DELAY);
return PX4_OK;
}
int AEROFC_ADC::probe()
{
uint8_t buffer[2];
int ret;
_retries = 3;
/* Enable ADC */
buffer[0] = ADC_ENABLE_REG;
buffer[1] = 0x01;
ret = transfer(buffer, 2, NULL, 0);
if (ret != PX4_OK) {
goto error;
}
usleep(10000);
/* Read ADC value */
buffer[0] = ADC_CHANNEL_REG;
ret = transfer(buffer, 1, buffer, 2);
if (ret != PX4_OK) {
goto error;
}
return PX4_OK;
error:
return -EIO;
}
int AEROFC_ADC::ioctl(file *filp, int cmd, unsigned long arg)
{
return -ENOTTY;
}
ssize_t AEROFC_ADC::read(file *filp, char *buffer, size_t len)
{
if (len < sizeof(_sample)) {
return -ENOSPC;
}
if (len > sizeof(_sample)) {
len = sizeof(_sample);
}
pthread_mutex_lock(&_sample_mutex);
memcpy(buffer, &_sample, len);
pthread_mutex_unlock(&_sample_mutex);
return len;
}
void AEROFC_ADC::cycle_trampoline(void *arg)
{
AEROFC_ADC *dev = static_cast<AEROFC_ADC * >(arg);
dev->cycle();
}
void AEROFC_ADC::cycle()
{
uint8_t buffer[2];
int ret;
buffer[0] = ADC_CHANNEL_REG;
ret = transfer(buffer, 1, buffer, sizeof(buffer));
if (ret != PX4_OK) {
warn("Error reading sample");
return;
}
pthread_mutex_lock(&_sample_mutex);
_sample.am_data = (buffer[0] | (buffer[1] << 8));
pthread_mutex_unlock(&_sample_mutex);
work_queue(HPWORK, &_work, (worker_t)&AEROFC_ADC::cycle_trampoline, this,
CYCLE_TICKS_DELAY);
}