forked from Archive/PX4-Autopilot
ADC: replace ioctl with uorb message (#14087)
This commit is contained in:
SalimTerryLi
GitHub
parent
40af5b0fbe
commit
dc8e775d8f
|
|
@ -130,8 +130,6 @@ then
|
|||
# Power Parameters
|
||||
param set BAT_N_CELLS 4
|
||||
param set BAT_A_PER_V 36.3675
|
||||
param set BAT_CNT_V_CURR 0.0008
|
||||
param set BAT_CNT_V_VOLT 0.0008
|
||||
param set BAT_V_DIV 18.40697289
|
||||
|
||||
# Circuit breakers
|
||||
|
|
|
|||
|
|
@ -36,12 +36,11 @@
|
|||
using namespace time_literals;
|
||||
|
||||
AEROFC_ADC::AEROFC_ADC(uint8_t bus) :
|
||||
I2C("AEROFC_ADC", ADC0_DEVICE_PATH, bus, SLAVE_ADDR, 400000),
|
||||
I2C("AEROFC_ADC", AEROFC_ADC_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()
|
||||
|
|
@ -95,26 +94,16 @@ 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] {};
|
||||
/*
|
||||
* https://github.com/intel-aero/intel-aero-fpga/blob/master/aero_sample/adc/adc.html
|
||||
* https://github.com/intel-aero/meta-intel-aero/wiki/95-(References)-FPGA
|
||||
* https://github.com/intel-aero/intel-aero-fpga/blob/master/aero_rtf_kit/RTL/adc.v
|
||||
*/
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
uint8_t buffer[10] {};
|
||||
buffer[0] = ADC_CHANNEL_REG;
|
||||
int ret = transfer(buffer, 1, buffer, sizeof(buffer));
|
||||
|
||||
|
|
@ -123,12 +112,24 @@ void AEROFC_ADC::Run()
|
|||
return;
|
||||
}
|
||||
|
||||
pthread_mutex_lock(&_sample_mutex);
|
||||
_sample.am_data = (buffer[0] | (buffer[1] << 8));
|
||||
pthread_mutex_unlock(&_sample_mutex);
|
||||
}
|
||||
adc_report_s adc_report;
|
||||
adc_report.device_id = this->get_device_id();
|
||||
adc_report.timestamp = hrt_absolute_time();
|
||||
adc_report.v_ref = 3.0f;
|
||||
adc_report.resolution = 1 << 12;
|
||||
|
||||
uint32_t px4_arch_adc_dn_fullcount()
|
||||
{
|
||||
return 1 << 12; // 12 bit ADC
|
||||
}
|
||||
unsigned i;
|
||||
|
||||
for (i = 0; i < MAX_CHANNEL; ++i) {
|
||||
adc_report.channel_id[i] = i;
|
||||
adc_report.raw_data[i] = (buffer[i * 2] | (buffer[i * 2 + 1] << 8));
|
||||
}
|
||||
|
||||
for (; i < PX4_MAX_ADC_CHANNELS; ++i) { // set unused channel id to -1
|
||||
adc_report.channel_id[i] = -1;
|
||||
}
|
||||
|
||||
_to_adc_report.publish(adc_report);
|
||||
|
||||
perf_end(_sample_perf);
|
||||
}
|
||||
|
|
@ -37,18 +37,20 @@
|
|||
#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>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/adc_report.h>
|
||||
|
||||
#define SLAVE_ADDR 0x50
|
||||
#define ADC_ENABLE_REG 0x00
|
||||
#define ADC_CHANNEL_REG 0x05
|
||||
#define ADC_CHANNEL_REG 0x03
|
||||
#define MAX_CHANNEL 5
|
||||
#define AEROFC_ADC_DEVICE_PATH "/dev/aerofc_adc"
|
||||
|
||||
class AEROFC_ADC : public device::I2C, public px4::ScheduledWorkItem
|
||||
{
|
||||
|
|
@ -57,14 +59,12 @@ public:
|
|||
~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;
|
||||
uORB::Publication<adc_report_s> _to_adc_report{ORB_ID(adc_report)};
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -63,30 +63,7 @@ static AEROFC_ADC *instance = nullptr;
|
|||
|
||||
static int test()
|
||||
{
|
||||
int fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
PX4_ERR("can't open ADC device");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
px4_adc_msg_t data[MAX_CHANNEL];
|
||||
ssize_t count = px4_read(fd, data, sizeof(data));
|
||||
|
||||
if (count < 0) {
|
||||
PX4_ERR("read error");
|
||||
px4_close(fd);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
unsigned channels = count / sizeof(data[0]);
|
||||
|
||||
for (unsigned j = 0; j < channels; j++) {
|
||||
printf("%d: %u ", data[j].am_channel, data[j].am_data);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
px4_close(fd);
|
||||
PX4_INFO("test is currently unavailable");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ px4_add_board(
|
|||
MODULES
|
||||
airspeed_selector
|
||||
attitude_estimator_q
|
||||
#battery_status
|
||||
battery_status
|
||||
camera_feedback
|
||||
commander
|
||||
dataman
|
||||
|
|
|
|||
|
|
@ -57,5 +57,11 @@
|
|||
#define PX4_SPI_BUS_SENSORS 0
|
||||
#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(0, 0) // spidev0.0
|
||||
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 0
|
||||
#define ADC_BATTERY_CURRENT_CHANNEL -1
|
||||
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 2
|
||||
|
||||
#define ADC_DP_V_DIV 1.0f
|
||||
|
||||
#include <system_config.h>
|
||||
#include <px4_platform_common/board_common.h>
|
||||
|
|
|
|||
|
|
@ -1,3 +1,6 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
int16[12] channel_id # ADC channel IDs, negative for non-existent
|
||||
float32[12] channel_value # ADC channel value in volt, valid if channel ID is positive
|
||||
uint32 device_id # unique device ID for the sensor that does not change between power cycles
|
||||
int16[12] channel_id # ADC channel IDs, negative for non-existent, TODO: should be kept same as array index
|
||||
int32[12] raw_data # ADC channel raw value, accept negative value, valid if channel ID is positive
|
||||
uint32 resolution # ADC channel resolution
|
||||
float32 v_ref # ADC channel voltage reference, use to calculate LSB voltage(lsb=scale/resolution)
|
||||
|
|
|
|||
|
|
@ -157,12 +157,8 @@
|
|||
#define BOARD_ADC_POS_REF_V (3.3f) // Default reference voltage for every channels
|
||||
#endif
|
||||
|
||||
#ifndef BOARD_ADC_POS_REF_V_FOR_CURRENT_CHAN
|
||||
#define BOARD_ADC_POS_REF_V_FOR_CURRENT_CHAN (3.3f) // Reference voltage for reading out the current channel
|
||||
#endif
|
||||
|
||||
#ifndef BOARD_ADC_POS_REF_V_FOR_VOLTAGE_CHAN
|
||||
#define BOARD_ADC_POS_REF_V_FOR_VOLTAGE_CHAN (3.3f) // Reference voltage for reading out the voltage channel
|
||||
#if !defined(ADC_DP_V_DIV) // Analog differential pressure (analog airspeed sensor)
|
||||
#define ADC_DP_V_DIV (2.0f) // The scale factor defined by HW's resistive divider (Rt+Rb)/ Rb
|
||||
#endif
|
||||
|
||||
/* Provide define for Bricks and Battery */
|
||||
|
|
|
|||
|
|
@ -38,10 +38,7 @@ param set MAV_TYPE 2
|
|||
# 2. onboard 9-18V DC Jack, which is connect to ADC channel 5. This is the board default.
|
||||
# 3. other power source (e.g., LiPo battery more than 4S/18V).
|
||||
# Scale the voltage to below 1.8V and connect it to ADC channel # 0, 1, 2 or 3.
|
||||
param set BAT_ADC_CHANNEL 5
|
||||
|
||||
# 12-bit 1.8V ADC, 1.8V / 2^12 = 0.000439453125 V/CNT
|
||||
param set BAT_CNT_V_VOLT 0.0004394531
|
||||
param set BAT1_V_CHANNEL 5
|
||||
|
||||
# Battery voltage scale factor, from BBBlue schematics: (4.7K + 47K) / 4.7K = 11
|
||||
param set BAT_V_DIV 11.0
|
||||
|
|
|
|||
|
|
@ -38,10 +38,7 @@ param set MAV_TYPE 2
|
|||
# 2. onboard 9-18V DC Jack, which is connect to ADC channel 5. This is the board default.
|
||||
# 3. other power source (e.g., LiPo battery more than 4S/18V).
|
||||
# Scale the voltage to below 1.8V and connect it to ADC channel # 0, 1, 2 or 3.
|
||||
param set BAT_ADC_CHANNEL 5
|
||||
|
||||
# 12-bit 1.8V ADC, 1.8V / 2^12 = 0.000439453125 V/CNT
|
||||
param set BAT_CNT_V_VOLT 0.0004394531
|
||||
param set BAT1_V_CHANNEL 5
|
||||
|
||||
# Battery voltage scale factor, from BBBlue schematics: (4.7K + 47K) / 4.7K = 11
|
||||
param set BAT_V_DIV 11.0
|
||||
|
|
|
|||
|
|
@ -14,8 +14,6 @@ fi
|
|||
param set SYS_AUTOSTART 4001
|
||||
param set MAV_BROADCAST 1
|
||||
param set MAV_TYPE 2
|
||||
param set BAT_CNT_V_VOLT 0.001
|
||||
param set BAT_CNT_V_CURR 0.001
|
||||
|
||||
dataman start
|
||||
|
||||
|
|
|
|||
|
|
@ -14,8 +14,6 @@ fi
|
|||
param set MAV_BROADCAST 1
|
||||
param set SYS_AUTOSTART 2100
|
||||
param set MAV_TYPE 1
|
||||
param set BAT_CNT_V_VOLT 0.001
|
||||
param set BAT_CNT_V_CURR 0.001
|
||||
|
||||
dataman start
|
||||
|
||||
|
|
|
|||
|
|
@ -14,8 +14,6 @@ fi
|
|||
param set SYS_AUTOSTART 4001
|
||||
param set MAV_BROADCAST 1
|
||||
param set MAV_TYPE 2
|
||||
param set BAT_CNT_V_VOLT 0.001
|
||||
param set BAT_CNT_V_CURR 0.001
|
||||
|
||||
dataman start
|
||||
|
||||
|
|
|
|||
|
|
@ -31,10 +31,11 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
#include "ADC.hpp"
|
||||
|
||||
ADC::ADC(uint32_t base_address, uint32_t channels) :
|
||||
CDev(ADC0_DEVICE_PATH),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
|
||||
_base_address(base_address)
|
||||
|
|
@ -90,39 +91,14 @@ int ADC::init()
|
|||
return ret_init;
|
||||
}
|
||||
|
||||
/* create the device node */
|
||||
int ret_cdev = CDev::init();
|
||||
|
||||
if (ret_cdev != PX4_OK) {
|
||||
PX4_ERR("CDev init failed");
|
||||
return ret_cdev;
|
||||
}
|
||||
|
||||
// schedule regular updates
|
||||
ScheduleOnInterval(kINTERVAL, kINTERVAL);
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
ssize_t ADC::read(cdev::file_t *filp, char *buffer, size_t len)
|
||||
{
|
||||
const size_t maxsize = sizeof(px4_adc_msg_t) * _channel_count;
|
||||
|
||||
if (len > maxsize) {
|
||||
len = maxsize;
|
||||
}
|
||||
|
||||
/* block interrupts while copying samples to avoid racing with an update */
|
||||
lock();
|
||||
memcpy(buffer, _samples, len);
|
||||
unlock();
|
||||
|
||||
return len;
|
||||
}
|
||||
|
||||
void ADC::Run()
|
||||
{
|
||||
lock();
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
/* scan the channel set and sample each */
|
||||
|
|
@ -132,13 +108,13 @@ void ADC::Run()
|
|||
|
||||
update_adc_report(now);
|
||||
update_system_power(now);
|
||||
unlock();
|
||||
}
|
||||
|
||||
void ADC::update_adc_report(hrt_abstime now)
|
||||
{
|
||||
adc_report_s adc = {};
|
||||
adc.timestamp = now;
|
||||
adc.device_id = BUILTIN_ADC_DEVID;
|
||||
|
||||
unsigned max_num = _channel_count;
|
||||
|
||||
|
|
@ -146,11 +122,20 @@ void ADC::update_adc_report(hrt_abstime now)
|
|||
max_num = (sizeof(adc.channel_id) / sizeof(adc.channel_id[0]));
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < max_num; i++) {
|
||||
unsigned i;
|
||||
|
||||
for (i = 0; i < max_num; i++) {
|
||||
adc.channel_id[i] = _samples[i].am_channel;
|
||||
adc.channel_value[i] = _samples[i].am_data * 3.3f / px4_arch_adc_dn_fullcount();
|
||||
adc.raw_data[i] = _samples[i].am_data;
|
||||
}
|
||||
|
||||
for (; i < PX4_MAX_ADC_CHANNELS; ++i) { // set unused channel id to -1
|
||||
adc.channel_id[i] = -1;
|
||||
}
|
||||
|
||||
adc.v_ref = px4_arch_adc_reference_v();
|
||||
adc.resolution = px4_arch_adc_dn_fullcount();
|
||||
|
||||
_to_adc_report.publish(adc);
|
||||
}
|
||||
|
||||
|
|
@ -252,35 +237,29 @@ uint32_t ADC::sample(unsigned channel)
|
|||
|
||||
int ADC::test()
|
||||
{
|
||||
int fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY);
|
||||
uORB::Subscription adc_sub_test{ORB_ID(adc_report)};
|
||||
adc_report_s adc;
|
||||
|
||||
if (fd < 0) {
|
||||
PX4_ERR("can't open ADC device %d", errno);
|
||||
px4_usleep(20000); // sleep 20ms and wait for adc report
|
||||
|
||||
if (adc_sub_test.update(&adc)) {
|
||||
PX4_INFO_RAW("DeviceID: %d\n", adc.device_id);
|
||||
PX4_INFO_RAW("Resolution: %d\n", adc.resolution);
|
||||
PX4_INFO_RAW("Voltage Reference: %f\n", (double)adc.v_ref);
|
||||
|
||||
for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
|
||||
PX4_INFO_RAW("%d: %d ", adc.channel_id[i], adc.raw_data[i]);
|
||||
}
|
||||
|
||||
PX4_INFO_RAW("\n");
|
||||
|
||||
PX4_INFO_RAW("\t ADC test successful.\n");
|
||||
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 20; i++) {
|
||||
px4_adc_msg_t data[ADC_TOTAL_CHANNELS];
|
||||
ssize_t count = px4_read(fd, data, sizeof(data));
|
||||
|
||||
if (count < 0) {
|
||||
PX4_ERR("read error");
|
||||
return 1;
|
||||
}
|
||||
|
||||
unsigned channels = count / sizeof(data[0]);
|
||||
|
||||
for (unsigned j = 0; j < channels; j++) {
|
||||
printf("%d: %u ", data[j].am_channel, data[j].am_data);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
px4_usleep(100000);
|
||||
}
|
||||
|
||||
px4_close(fd);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ADC::custom_command(int argc, char *argv[])
|
||||
|
|
|
|||
|
|
@ -60,7 +60,7 @@ using namespace time_literals;
|
|||
|
||||
#define ADC_TOTAL_CHANNELS 32
|
||||
|
||||
class ADC : public ModuleBase<ADC>, public cdev::CDev, public px4::ScheduledWorkItem
|
||||
class ADC : public ModuleBase<ADC>, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS);
|
||||
|
|
@ -76,7 +76,7 @@ public:
|
|||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
int init() override;
|
||||
int init();
|
||||
|
||||
int test();
|
||||
|
||||
|
|
@ -84,8 +84,6 @@ private:
|
|||
|
||||
void Run() override;
|
||||
|
||||
ssize_t read(cdev::file_t *filp, char *buffer, size_t len) override;
|
||||
|
||||
/**
|
||||
* Sample a single channel and return the measured value.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -42,6 +42,8 @@
|
|||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
#include <uORB/topics/adc_report.h>
|
||||
|
||||
/* Define the PX4 low level format ADC and the maximum
|
||||
* number of channels that can be returned by a lowlevel
|
||||
|
|
@ -49,14 +51,14 @@
|
|||
* but no more than PX4_MAX_ADC_CHANNELS.
|
||||
*
|
||||
*/
|
||||
#define PX4_MAX_ADC_CHANNELS 12
|
||||
#define PX4_MAX_ADC_CHANNELS arraySize(adc_report_s::channel_id)
|
||||
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 BUILTIN_ADC_DEVID 0xffffffff // TODO: integrate into existing ID management
|
||||
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
|
|
|||
|
|
@ -308,16 +308,17 @@ void RCInput::Run()
|
|||
adc_report_s adc;
|
||||
|
||||
if (_adc_sub.update(&adc)) {
|
||||
const unsigned adc_chans = sizeof(adc.channel_id) / sizeof(adc.channel_id[0]);
|
||||
|
||||
for (unsigned i = 0; i < adc_chans; i++) {
|
||||
for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
|
||||
if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
|
||||
float adc_volt = adc.raw_data[i] *
|
||||
adc.v_ref /
|
||||
adc.resolution;
|
||||
|
||||
if (_analog_rc_rssi_volt < 0.0f) {
|
||||
_analog_rc_rssi_volt = adc.channel_value[i];
|
||||
_analog_rc_rssi_volt = adc_volt;
|
||||
}
|
||||
|
||||
_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc.channel_value[i] * 0.005f;
|
||||
_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc_volt * 0.005f;
|
||||
|
||||
/* only allow this to be used if we see a high RSSI once */
|
||||
if (_analog_rc_rssi_volt > 2.5f) {
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@
|
|||
#include <float.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#include <drivers/drv_adc.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
|
|
|||
|
|
@ -40,7 +40,6 @@ px4_add_module(
|
|||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
arch_adc
|
||||
battery
|
||||
conversion
|
||||
drivers__device
|
||||
|
|
|
|||
|
|
@ -15,17 +15,11 @@ static constexpr int DEFAULT_V_CHANNEL[1] = {0};
|
|||
static constexpr int DEFAULT_I_CHANNEL[1] = {0};
|
||||
#endif
|
||||
|
||||
static constexpr float DEFAULT_VOLTS_PER_COUNT = 3.3f / 4096.0f;
|
||||
|
||||
AnalogBattery::AnalogBattery(int index, ModuleParams *parent) :
|
||||
Battery(index, parent)
|
||||
{
|
||||
char param_name[17];
|
||||
|
||||
_analog_param_handles.cnt_v_volt = param_find("BAT_CNT_V_VOLT");
|
||||
|
||||
_analog_param_handles.cnt_v_curr = param_find("BAT_CNT_V_CURR");
|
||||
|
||||
_analog_param_handles.v_offs_cur = param_find("BAT_V_OFFS_CURR");
|
||||
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_V_DIV", index);
|
||||
|
|
@ -34,8 +28,11 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent) :
|
|||
snprintf(param_name, sizeof(param_name), "BAT%d_A_PER_V", index);
|
||||
_analog_param_handles.a_per_v = param_find(param_name);
|
||||
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_ADC_CHANNEL", index);
|
||||
_analog_param_handles.adc_channel = param_find(param_name);
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_V_CHANNEL", index);
|
||||
_analog_param_handles.v_channel = param_find(param_name);
|
||||
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_I_CHANNEL", index);
|
||||
_analog_param_handles.i_channel = param_find(param_name);
|
||||
|
||||
_analog_param_handles.v_div_old = param_find("BAT_V_DIV");
|
||||
_analog_param_handles.a_per_v_old = param_find("BAT_A_PER_V");
|
||||
|
|
@ -43,11 +40,11 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent) :
|
|||
}
|
||||
|
||||
void
|
||||
AnalogBattery::updateBatteryStatusRawADC(hrt_abstime timestamp, int32_t voltage_raw, int32_t current_raw,
|
||||
bool selected_source, int priority, float throttle_normalized)
|
||||
AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
|
||||
bool selected_source, int priority, float throttle_normalized)
|
||||
{
|
||||
float voltage_v = (voltage_raw * _analog_params.cnt_v_volt) * _analog_params.v_div;
|
||||
float current_a = ((current_raw * _analog_params.cnt_v_curr) - _analog_params.v_offs_cur) * _analog_params.a_per_v;
|
||||
float voltage_v = voltage_raw * _analog_params.v_div;
|
||||
float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v;
|
||||
|
||||
bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V &&
|
||||
(BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid());
|
||||
|
|
@ -70,8 +67,8 @@ bool AnalogBattery::is_valid()
|
|||
|
||||
int AnalogBattery::get_voltage_channel()
|
||||
{
|
||||
if (_analog_params.adc_channel >= 0) {
|
||||
return _analog_params.adc_channel;
|
||||
if (_analog_params.v_channel >= 0) {
|
||||
return _analog_params.v_channel;
|
||||
|
||||
} else {
|
||||
return DEFAULT_V_CHANNEL[_index - 1];
|
||||
|
|
@ -80,8 +77,12 @@ int AnalogBattery::get_voltage_channel()
|
|||
|
||||
int AnalogBattery::get_current_channel()
|
||||
{
|
||||
// TODO: Possibly implement parameter for current sense channel
|
||||
return DEFAULT_I_CHANNEL[_index - 1];
|
||||
if (_analog_params.i_channel >= 0) {
|
||||
return _analog_params.i_channel;
|
||||
|
||||
} else {
|
||||
return DEFAULT_I_CHANNEL[_index - 1];
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -92,33 +93,18 @@ AnalogBattery::updateParams()
|
|||
&_analog_params.v_div, _first_parameter_update);
|
||||
migrateParam<float>(_analog_param_handles.a_per_v_old, _analog_param_handles.a_per_v, &_analog_params.a_per_v_old,
|
||||
&_analog_params.a_per_v, _first_parameter_update);
|
||||
migrateParam<int>(_analog_param_handles.adc_channel_old, _analog_param_handles.adc_channel,
|
||||
&_analog_params.adc_channel_old, &_analog_params.adc_channel, _first_parameter_update);
|
||||
migrateParam<int>(_analog_param_handles.adc_channel_old, _analog_param_handles.v_channel,
|
||||
&_analog_params.adc_channel_old, &_analog_params.v_channel, _first_parameter_update);
|
||||
|
||||
} else {
|
||||
param_get(_analog_param_handles.v_div, &_analog_params.v_div);
|
||||
param_get(_analog_param_handles.a_per_v, &_analog_params.a_per_v);
|
||||
param_get(_analog_param_handles.adc_channel, &_analog_params.adc_channel);
|
||||
param_get(_analog_param_handles.v_channel, &_analog_params.v_channel);
|
||||
}
|
||||
|
||||
param_get(_analog_param_handles.cnt_v_volt, &_analog_params.cnt_v_volt);
|
||||
param_get(_analog_param_handles.cnt_v_curr, &_analog_params.cnt_v_curr);
|
||||
param_get(_analog_param_handles.i_channel, &_analog_params.i_channel);
|
||||
param_get(_analog_param_handles.v_offs_cur, &_analog_params.v_offs_cur);
|
||||
|
||||
/* scaling of ADC ticks to battery voltage */
|
||||
if (_analog_params.cnt_v_volt < 0.0f) {
|
||||
/* apply scaling according to defaults if set to default */
|
||||
_analog_params.cnt_v_volt = (BOARD_ADC_POS_REF_V_FOR_VOLTAGE_CHAN / px4_arch_adc_dn_fullcount());
|
||||
param_set_no_notification(_analog_param_handles.cnt_v_volt, &_analog_params.cnt_v_volt);
|
||||
}
|
||||
|
||||
/* scaling of ADC ticks to battery current */
|
||||
if (_analog_params.cnt_v_curr < 0.0f) {
|
||||
/* apply scaling according to defaults if set to default */
|
||||
_analog_params.cnt_v_curr = (BOARD_ADC_POS_REF_V_FOR_CURRENT_CHAN / px4_arch_adc_dn_fullcount());
|
||||
param_set_no_notification(_analog_param_handles.cnt_v_curr, &_analog_params.cnt_v_curr);
|
||||
}
|
||||
|
||||
if (_analog_params.v_div <= 0.0f) {
|
||||
/* apply scaling according to defaults if set to default */
|
||||
_analog_params.v_div = BOARD_BATTERY1_V_DIV;
|
||||
|
|
|
|||
|
|
@ -44,15 +44,15 @@ public:
|
|||
/**
|
||||
* Update current battery status message.
|
||||
*
|
||||
* @param voltage_raw Battery voltage read from ADC, in raw ADC counts
|
||||
* @param current_raw Voltage of current sense resistor, in raw ADC counts
|
||||
* @param voltage_raw Battery voltage read from ADC, volts
|
||||
* @param current_raw Voltage of current sense resistor, volts
|
||||
* @param timestamp Time at which the ADC was read (use hrt_absolute_time())
|
||||
* @param selected_source This battery is on the brick that the selected source for selected_source
|
||||
* @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
|
||||
* @param throttle_normalized Throttle of the vehicle, between 0 and 1
|
||||
*/
|
||||
void updateBatteryStatusRawADC(hrt_abstime timestamp, int32_t voltage_raw, int32_t current_raw,
|
||||
bool selected_source, int priority, float throttle_normalized);
|
||||
void updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
|
||||
bool selected_source, int priority, float throttle_normalized);
|
||||
|
||||
/**
|
||||
* Whether the ADC channel for the voltage of this battery is valid.
|
||||
|
|
@ -73,12 +73,11 @@ public:
|
|||
protected:
|
||||
|
||||
struct {
|
||||
param_t cnt_v_volt;
|
||||
param_t cnt_v_curr;
|
||||
param_t v_offs_cur;
|
||||
param_t v_div;
|
||||
param_t a_per_v;
|
||||
param_t adc_channel;
|
||||
param_t v_channel;
|
||||
param_t i_channel;
|
||||
|
||||
param_t v_div_old;
|
||||
param_t a_per_v_old;
|
||||
|
|
@ -86,12 +85,11 @@ protected:
|
|||
} _analog_param_handles;
|
||||
|
||||
struct {
|
||||
float cnt_v_volt;
|
||||
float cnt_v_curr;
|
||||
float v_offs_cur;
|
||||
float v_div;
|
||||
float a_per_v;
|
||||
int adc_channel;
|
||||
int v_channel;
|
||||
int i_channel;
|
||||
|
||||
float v_div_old;
|
||||
float a_per_v_old;
|
||||
|
|
|
|||
|
|
@ -31,30 +31,6 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Scaling from ADC counts to volt on the ADC input (battery voltage)
|
||||
*
|
||||
* This is not the battery voltage, but the intermediate ADC voltage.
|
||||
* A value of -1 signifies that the board defaults are used, which is
|
||||
* highly recommended.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_CNT_V_VOLT, -1.0f);
|
||||
|
||||
/**
|
||||
* Scaling from ADC counts to volt on the ADC input (battery current)
|
||||
*
|
||||
* This is not the battery current, but the intermediate ADC voltage.
|
||||
* A value of -1 signifies that the board defaults are used, which is
|
||||
* highly recommended.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0);
|
||||
|
||||
/**
|
||||
* Offset in volt as seen by the ADC input of the current sensor.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -60,7 +60,7 @@
|
|||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/adc_report.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include "analog_battery.h"
|
||||
|
|
@ -99,10 +99,9 @@ public:
|
|||
private:
|
||||
void Run() override;
|
||||
|
||||
int _adc_fd{-1}; /**< ADC file handle */
|
||||
|
||||
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
|
||||
uORB::Subscription _adc_report_sub{ORB_ID(adc_report)};
|
||||
|
||||
AnalogBattery _battery1;
|
||||
|
||||
|
|
@ -167,21 +166,15 @@ BatteryStatus::parameter_update_poll(bool forced)
|
|||
void
|
||||
BatteryStatus::adc_poll()
|
||||
{
|
||||
/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
|
||||
px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS];
|
||||
|
||||
/* read all channels available */
|
||||
int ret = px4_read(_adc_fd, &buf_adc, sizeof(buf_adc));
|
||||
|
||||
/* For legacy support we publish the battery_status for the Battery that is
|
||||
* associated with the Brick that is the selected source for VDD_5V_IN
|
||||
* Selection is done in HW ala a LTC4417 or similar, or may be hard coded
|
||||
* Like in the FMUv4
|
||||
*/
|
||||
* associated with the Brick that is the selected source for VDD_5V_IN
|
||||
* Selection is done in HW ala a LTC4417 or similar, or may be hard coded
|
||||
* Like in the FMUv4
|
||||
*/
|
||||
|
||||
/* Per Brick readings with default unread channels at 0 */
|
||||
int32_t bat_current_adc_readings[BOARD_NUMBER_BRICKS] {};
|
||||
int32_t bat_voltage_adc_readings[BOARD_NUMBER_BRICKS] {};
|
||||
float bat_current_adc_readings[BOARD_NUMBER_BRICKS] {};
|
||||
float bat_voltage_adc_readings[BOARD_NUMBER_BRICKS] {};
|
||||
|
||||
/* Based on the valid_chan, used to indicate the selected the lowest index
|
||||
* (highest priority) supply that is the source for the VDD_5V_IN
|
||||
|
|
@ -190,34 +183,40 @@ BatteryStatus::adc_poll()
|
|||
|
||||
int selected_source = -1;
|
||||
|
||||
if (ret >= (int)sizeof(buf_adc[0])) {
|
||||
adc_report_s adc_report;
|
||||
|
||||
if (_adc_report_sub.update(&adc_report)) {
|
||||
|
||||
/* Read add channels we got */
|
||||
for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) {
|
||||
{
|
||||
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
|
||||
for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
|
||||
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
|
||||
|
||||
/* Once we have subscriptions, Do this once for the lowest (highest priority
|
||||
* supply on power controller) that is valid.
|
||||
/* Once we have subscriptions, Do this once for the lowest (highest priority
|
||||
* supply on power controller) that is valid.
|
||||
*/
|
||||
if (selected_source < 0 && _analogBatteries[b]->is_valid()) {
|
||||
/* Indicate the lowest brick (highest priority supply on power controller)
|
||||
* that is valid as the one that is the selected source for the
|
||||
* VDD_5V_IN
|
||||
*/
|
||||
if (selected_source < 0 && _analogBatteries[b]->is_valid()) {
|
||||
/* Indicate the lowest brick (highest priority supply on power controller)
|
||||
* that is valid as the one that is the selected source for the
|
||||
* VDD_5V_IN
|
||||
*/
|
||||
selected_source = b;
|
||||
selected_source = b;
|
||||
|
||||
}
|
||||
|
||||
/* look for specific channels and process the raw voltage to measurement data */
|
||||
if (_analogBatteries[b]->get_voltage_channel() == buf_adc[i].am_channel) {
|
||||
/* Voltage in volts */
|
||||
bat_voltage_adc_readings[b] = buf_adc[i].am_data;
|
||||
|
||||
} else if (_analogBatteries[b]->get_current_channel() == buf_adc[i].am_channel) {
|
||||
bat_current_adc_readings[b] = buf_adc[i].am_data;
|
||||
}
|
||||
}
|
||||
|
||||
/* look for specific channels and process the raw voltage to measurement data */
|
||||
|
||||
if (adc_report.channel_id[i] == _analogBatteries[b]->get_voltage_channel()) {
|
||||
/* Voltage in volts */
|
||||
bat_voltage_adc_readings[b] = adc_report.raw_data[i] *
|
||||
adc_report.v_ref /
|
||||
adc_report.resolution;
|
||||
|
||||
} else if (adc_report.channel_id[i] == _analogBatteries[b]->get_current_channel()) {
|
||||
bat_current_adc_readings[b] = adc_report.raw_data[i] *
|
||||
adc_report.v_ref /
|
||||
adc_report.resolution;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -226,7 +225,7 @@ BatteryStatus::adc_poll()
|
|||
actuator_controls_s ctrl{};
|
||||
_actuator_ctrl_0_sub.copy(&ctrl);
|
||||
|
||||
_analogBatteries[b]->updateBatteryStatusRawADC(
|
||||
_analogBatteries[b]->updateBatteryStatusADC(
|
||||
hrt_absolute_time(),
|
||||
bat_voltage_adc_readings[b],
|
||||
bat_current_adc_readings[b],
|
||||
|
|
@ -249,15 +248,6 @@ BatteryStatus::Run()
|
|||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
if (_adc_fd < 0) {
|
||||
_adc_fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (_adc_fd < 0) {
|
||||
PX4_ERR("unable to open ADC: %s", ADC0_DEVICE_PATH);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* check parameters for updates */
|
||||
parameter_update_poll();
|
||||
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@ parameters:
|
|||
description:
|
||||
short: Battery ${i} voltage divider (V divider)
|
||||
long: |
|
||||
This is the divider from battery ${i} voltage to 3.3V ADC voltage.
|
||||
This is the divider from battery ${i} voltage to ADC voltage.
|
||||
If using e.g. Mauch power modules the value from the datasheet
|
||||
can be applied straight here. A value of -1 means to use
|
||||
the board default.
|
||||
|
|
@ -25,7 +25,7 @@ parameters:
|
|||
description:
|
||||
short: Battery ${i} current per volt (A/V)
|
||||
long: |
|
||||
The voltage seen by the 3.3V ADC multiplied by this factor
|
||||
The voltage seen by the ADC multiplied by this factor
|
||||
will determine the battery current. A value of -1 means to use
|
||||
the board default.
|
||||
|
||||
|
|
@ -36,9 +36,9 @@ parameters:
|
|||
instance_start: 1
|
||||
default: [-1.0, -1.0]
|
||||
|
||||
BAT${i}_ADC_CHANNEL:
|
||||
BAT${i}_V_CHANNEL:
|
||||
description:
|
||||
short: Battery ${i} ADC Channel
|
||||
short: Battery ${i} Voltage ADC Channel
|
||||
long: |
|
||||
This parameter specifies the ADC channel used to monitor voltage of main power battery.
|
||||
A value of -1 means to use the board default.
|
||||
|
|
@ -48,3 +48,16 @@ parameters:
|
|||
num_instances: *max_num_config_instances
|
||||
instance_start: 1
|
||||
default: [-1, -1]
|
||||
|
||||
BAT${i}_I_CHANNEL:
|
||||
description:
|
||||
short: Battery ${i} Current ADC Channel
|
||||
long: |
|
||||
This parameter specifies the ADC channel used to monitor current of main power battery.
|
||||
A value of -1 means to use the board default.
|
||||
|
||||
type: int32
|
||||
reboot_required: true
|
||||
num_instances: *max_num_config_instances
|
||||
instance_start: 1
|
||||
default: [-1, -1]
|
||||
|
|
@ -144,10 +144,10 @@ private:
|
|||
DataValidator _airspeed_validator; /**< data validator to monitor airspeed */
|
||||
|
||||
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
int _adc_fd {-1}; /**< ADC driver handle */
|
||||
|
||||
hrt_abstime _last_adc{0}; /**< last time we took input from the ADC */
|
||||
|
||||
uORB::Subscription _adc_report_sub{ORB_ID(adc_report)}; /**< adc_report sub */
|
||||
differential_pressure_s _diff_pres {};
|
||||
uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */
|
||||
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
|
||||
|
|
@ -183,11 +183,6 @@ private:
|
|||
*/
|
||||
void parameter_update_poll(bool forced = false);
|
||||
|
||||
/**
|
||||
* Do adc-related initialisation.
|
||||
*/
|
||||
int adc_init();
|
||||
|
||||
/**
|
||||
* Poll the ADC and update readings to suit.
|
||||
*
|
||||
|
|
@ -254,23 +249,6 @@ int Sensors::parameters_update()
|
|||
return PX4_OK;
|
||||
}
|
||||
|
||||
int Sensors::adc_init()
|
||||
{
|
||||
if (!_hil_enabled) {
|
||||
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
_adc_fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (_adc_fd == -1) {
|
||||
PX4_ERR("no ADC found: %s", ADC0_DEVICE_PATH);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void Sensors::diff_pres_poll()
|
||||
{
|
||||
differential_pressure_s diff_pres{};
|
||||
|
|
@ -365,37 +343,40 @@ Sensors::parameter_update_poll(bool forced)
|
|||
|
||||
void Sensors::adc_poll()
|
||||
{
|
||||
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
|
||||
/* only read if not in HIL mode */
|
||||
if (_hil_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
|
||||
if (_parameters.diff_pres_analog_scale > 0.0f) {
|
||||
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
/* rate limit to 100 Hz */
|
||||
if (t - _last_adc >= 10000) {
|
||||
/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
|
||||
px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS];
|
||||
/* read all channels available */
|
||||
int ret = px4_read(_adc_fd, &buf_adc, sizeof(buf_adc));
|
||||
|
||||
if (ret >= (int)sizeof(buf_adc[0])) {
|
||||
adc_report_s adc;
|
||||
|
||||
if (_adc_report_sub.update(&adc)) {
|
||||
/* Read add channels we got */
|
||||
for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) {
|
||||
if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
|
||||
for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) {
|
||||
if (adc.channel_id[i] == -1) {
|
||||
continue; // skip non-exist channels
|
||||
}
|
||||
|
||||
if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) {
|
||||
|
||||
/* calculate airspeed, raw is the difference from */
|
||||
const float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor)
|
||||
const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV;
|
||||
|
||||
/**
|
||||
* The voltage divider pulls the signal down, only act on
|
||||
* a valid voltage from a connected sensor. Also assume a non-
|
||||
* zero offset from the sensor if its connected.
|
||||
*
|
||||
* Notice: This won't work on devices which have PGA controlled
|
||||
* vref. Those devices require no divider at all.
|
||||
*/
|
||||
if (voltage > 0.4f) {
|
||||
const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
|
||||
|
|
@ -410,9 +391,9 @@ void Sensors::adc_poll()
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
_last_adc = t;
|
||||
}
|
||||
|
||||
_last_adc = t;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -465,7 +446,6 @@ void Sensors::Run()
|
|||
|
||||
// run once
|
||||
if (_last_config_update == 0) {
|
||||
adc_init();
|
||||
_voted_sensors_update.init(_sensor_combined);
|
||||
parameter_update_poll(true);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@
|
|||
############################################################################
|
||||
|
||||
set(srcs
|
||||
test_adc.c
|
||||
test_adc.cpp
|
||||
test_atomic_bitset.cpp
|
||||
test_autodeclination.cpp
|
||||
test_bezierQuad.cpp
|
||||
|
|
@ -48,7 +48,7 @@ set(srcs
|
|||
test_int.cpp
|
||||
test_i2c_spi_cli.cpp
|
||||
test_IntrusiveQueue.cpp
|
||||
test_jig_voltages.c
|
||||
test_jig_voltages.cpp
|
||||
test_led.c
|
||||
test_List.cpp
|
||||
test_mathlib.cpp
|
||||
|
|
|
|||
|
|
@ -37,56 +37,36 @@
|
|||
*/
|
||||
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "tests_main.h"
|
||||
|
||||
#include <drivers/drv_adc.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/adc_report.h>
|
||||
|
||||
int test_adc(int argc, char *argv[])
|
||||
{
|
||||
int fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY);
|
||||
uORB::Subscription _adc_sub{ORB_ID(adc_report)};
|
||||
adc_report_s adc;
|
||||
|
||||
if (fd < 0) {
|
||||
PX4_ERR("ERROR: can't open ADC device");
|
||||
px4_usleep(50000); // sleep 50ms and wait for adc report
|
||||
|
||||
if (_adc_sub.update(&adc)) {
|
||||
PX4_INFO_RAW("DeviceID: %d\n", adc.device_id);
|
||||
PX4_INFO_RAW("Resolution: %d\n", adc.resolution);
|
||||
PX4_INFO_RAW("Voltage Reference: %f\n", adc.v_ref);
|
||||
|
||||
for (int i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
|
||||
PX4_INFO_RAW("%d: %d ", adc.channel_id[i], adc.raw_data[i]);
|
||||
}
|
||||
|
||||
PX4_INFO_RAW("\n");
|
||||
|
||||
PX4_INFO_RAW("\t ADC test successful.\n");
|
||||
|
||||
return OK;
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < 5; i++) {
|
||||
/* make space for a maximum number of channels */
|
||||
px4_adc_msg_t data[PX4_MAX_ADC_CHANNELS];
|
||||
/* read all channels available */
|
||||
ssize_t count = px4_read(fd, data, sizeof(data));
|
||||
|
||||
if (count < 0) {
|
||||
goto errout_with_dev;
|
||||
}
|
||||
|
||||
unsigned channels = count / sizeof(data[0]);
|
||||
|
||||
for (unsigned j = 0; j < channels; j++) {
|
||||
printf("%d: %u ", data[j].am_channel, data[j].am_data);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
px4_usleep(150000);
|
||||
}
|
||||
|
||||
printf("\t ADC test successful.\n");
|
||||
|
||||
errout_with_dev:
|
||||
|
||||
if (fd != 0) { px4_close(fd); }
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -1,133 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2019 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 test_jig_voltages.c
|
||||
* Tests for jig voltages.
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "tests_main.h"
|
||||
|
||||
#include <drivers/drv_adc.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
int test_jig_voltages(int argc, char *argv[])
|
||||
{
|
||||
int fd = open(ADC0_DEVICE_PATH, O_RDONLY);
|
||||
int ret = OK;
|
||||
|
||||
if (fd < 0) {
|
||||
PX4_ERR("can't open ADC device");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* make space for the maximum channels */
|
||||
px4_adc_msg_t data[PX4_MAX_ADC_CHANNELS];
|
||||
|
||||
/* read all channels available */
|
||||
ssize_t count = read(fd, data, sizeof(data));
|
||||
|
||||
if (count < 0) {
|
||||
close(fd);
|
||||
PX4_ERR("can't read from ADC driver. Forgot 'adc start' command?");
|
||||
return 1;
|
||||
}
|
||||
|
||||
unsigned channels = count / sizeof(data[0]);
|
||||
|
||||
for (unsigned j = 0; j < channels; j++) {
|
||||
printf("%d: %u ", data[j].am_channel, data[j].am_data);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
PX4_INFO("\t ADC operational.\n");
|
||||
|
||||
/* Expected values */
|
||||
int16_t expected_min[] = {2800, 2800, 1800, 800};
|
||||
int16_t expected_max[] = {3100, 3100, 2100, 1100};
|
||||
char *check_res[channels];
|
||||
|
||||
if (channels < 4) {
|
||||
close(fd);
|
||||
PX4_ERR("not all four test channels available, aborting.");
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
/* Check values */
|
||||
check_res[0] = (expected_min[0] < data[0].am_data && expected_max[0] > data[0].am_data) ? "OK" : "FAIL";
|
||||
check_res[1] = (expected_min[1] < data[1].am_data && expected_max[1] > data[1].am_data) ? "OK" : "FAIL";
|
||||
check_res[2] = (expected_min[2] < data[2].am_data && expected_max[2] > data[2].am_data) ? "OK" : "FAIL";
|
||||
check_res[3] = (expected_min[3] < data[3].am_data && expected_max[3] > data[3].am_data) ? "OK" : "FAIL";
|
||||
|
||||
/* Accumulate result */
|
||||
ret += (expected_min[0] > data[0].am_data || expected_max[0] < data[0].am_data) ? 1 : 0;
|
||||
ret += (expected_min[1] > data[1].am_data || expected_max[1] < data[1].am_data) ? 1 : 0;
|
||||
ret += (expected_min[2] > data[2].am_data || expected_max[2] < data[2].am_data) ? 1 : 0;
|
||||
ret += (expected_min[3] > data[3].am_data || expected_max[3] < data[3].am_data) ? 1 : 0;
|
||||
|
||||
PX4_INFO("Sample:");
|
||||
PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
|
||||
data[0].am_channel, (int)(data[0].am_data), expected_min[0], expected_max[0], check_res[0]);
|
||||
PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
|
||||
data[1].am_channel, (int)(data[1].am_data), expected_min[1], expected_max[1], check_res[1]);
|
||||
PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
|
||||
data[2].am_channel, (int)(data[2].am_data), expected_min[2], expected_max[2], check_res[2]);
|
||||
PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
|
||||
data[3].am_channel, (int)(data[3].am_data), expected_min[3], expected_max[3], check_res[3]);
|
||||
|
||||
if (ret != OK) {
|
||||
PX4_ERR("\t JIG voltages test FAILED. Some channels where out of allowed range. Check supply voltages.");
|
||||
goto errout_with_dev;
|
||||
}
|
||||
}
|
||||
|
||||
PX4_INFO("JIG voltages test successful.");
|
||||
|
||||
errout_with_dev:
|
||||
|
||||
if (fd != 0) { close(fd); }
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -0,0 +1,120 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2019 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 test_jig_voltages.c
|
||||
* Tests for jig voltages.
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <unistd.h>
|
||||
#include "tests_main.h"
|
||||
#include <drivers/drv_adc.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/adc_report.h>
|
||||
|
||||
int test_jig_voltages(int argc, char *argv[])
|
||||
{
|
||||
uORB::Subscription _adc_sub{ORB_ID(adc_report)};
|
||||
adc_report_s adc;
|
||||
|
||||
px4_usleep(50000); // sleep 50ms and wait for adc report
|
||||
|
||||
if (_adc_sub.update(&adc)) {
|
||||
PX4_INFO_RAW("DeviceID: %d\n", adc.device_id);
|
||||
PX4_INFO_RAW("Resolution: %d\n", adc.resolution);
|
||||
PX4_INFO_RAW("Voltage Reference: %f\n", adc.v_ref);
|
||||
|
||||
unsigned channels = 0;
|
||||
|
||||
for (int i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
|
||||
PX4_INFO_RAW("%d: %d ", adc.channel_id[i], adc.raw_data[i]);
|
||||
|
||||
if (adc.channel_id[i] != -1) {
|
||||
++channels;
|
||||
}
|
||||
}
|
||||
|
||||
PX4_INFO_RAW("\n");
|
||||
|
||||
PX4_INFO("\t ADC operational.\n");
|
||||
|
||||
/* Expected values */
|
||||
int16_t expected_min[] = {2800, 2800, 1800, 800};
|
||||
int16_t expected_max[] = {3100, 3100, 2100, 1100};
|
||||
const char *check_res[channels];
|
||||
|
||||
if (channels < 4) {
|
||||
PX4_ERR("not all four test channels available, aborting.");
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
int ret = OK;
|
||||
|
||||
/* Check values */
|
||||
check_res[0] = (expected_min[0] < adc.raw_data[0] && expected_max[0] > adc.raw_data[0]) ? "OK" : "FAIL";
|
||||
check_res[1] = (expected_min[1] < adc.raw_data[1] && expected_max[1] > adc.raw_data[1]) ? "OK" : "FAIL";
|
||||
check_res[2] = (expected_min[2] < adc.raw_data[2] && expected_max[2] > adc.raw_data[2]) ? "OK" : "FAIL";
|
||||
check_res[3] = (expected_min[3] < adc.raw_data[3] && expected_max[3] > adc.raw_data[3]) ? "OK" : "FAIL";
|
||||
|
||||
/* Accumulate result */
|
||||
ret += (expected_min[0] > adc.raw_data[0] || expected_max[0] < adc.raw_data[0]) ? 1 : 0;
|
||||
ret += (expected_min[1] > adc.raw_data[1] || expected_max[1] < adc.raw_data[1]) ? 1 : 0;
|
||||
ret += (expected_min[2] > adc.raw_data[2] || expected_max[2] < adc.raw_data[2]) ? 1 : 0;
|
||||
ret += (expected_min[3] > adc.raw_data[3] || expected_max[3] < adc.raw_data[3]) ? 1 : 0;
|
||||
|
||||
PX4_INFO("Sample:");
|
||||
PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
|
||||
adc.channel_id[0], (int)(adc.raw_data[0]), expected_min[0], expected_max[0], check_res[0]);
|
||||
PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
|
||||
adc.channel_id[1], (int)(adc.raw_data[1]), expected_min[1], expected_max[1], check_res[1]);
|
||||
PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
|
||||
adc.channel_id[2], (int)(adc.raw_data[2]), expected_min[2], expected_max[2], check_res[2]);
|
||||
PX4_INFO("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s",
|
||||
adc.channel_id[3], (int)(adc.raw_data[3]), expected_min[3], expected_max[3], check_res[3]);
|
||||
|
||||
if (ret != OK) {
|
||||
PX4_ERR("\t JIG voltages test FAILED. Some channels where out of allowed range. Check supply voltages.");
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
PX4_INFO("JIG voltages test successful.");
|
||||
|
||||
return OK;
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -67,7 +67,6 @@ const struct {
|
|||
{"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
{"adc", test_adc, OPT_NOJIGTEST},
|
||||
{"file", test_file, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"led", test_led, 0},
|
||||
{"time", test_time, OPT_NOJIGTEST},
|
||||
|
|
@ -78,6 +77,7 @@ const struct {
|
|||
{"rc", rc_tests_main, 0},
|
||||
#endif /* __PX4_NUTTX */
|
||||
|
||||
{"adc", test_adc, OPT_NOJIGTEST},
|
||||
{"atomic_bitset", test_atomic_bitset, 0},
|
||||
{"autodeclination", test_autodeclination, 0},
|
||||
{"bezier", test_bezierQuad, 0},
|
||||
|
|
|
|||
Loading…
Reference in New Issue