From dc8e775d8fa3cb2b25afe5ef1ba94bedb5eaf4a8 Mon Sep 17 00:00:00 2001 From: SalimTerryLi Date: Fri, 20 Mar 2020 18:23:32 +0800 Subject: [PATCH] ADC: replace ioctl with uorb message (#14087) --- .../init.d/airframes/4016_holybro_px4vision | 2 - .../intel/aerofc-v1/aerofc_adc/AEROFC_ADC.cpp | 59 ++++---- .../intel/aerofc-v1/aerofc_adc/AEROFC_ADC.hpp | 10 +- .../aerofc-v1/aerofc_adc/aerofc_adc_main.cpp | 25 +--- boards/px4/raspberrypi/default.cmake | 2 +- boards/px4/raspberrypi/src/board_config.h | 6 + msg/adc_report.msg | 7 +- .../px4_platform_common/board_common.h | 8 +- posix-configs/bbblue/px4.config | 5 +- posix-configs/bbblue/px4_fw.config | 5 +- posix-configs/rpi/px4.config | 2 - posix-configs/rpi/px4_fw.config | 2 - posix-configs/rpi/px4_test.config | 2 - src/drivers/adc/ADC.cpp | 79 ++++------- src/drivers/adc/ADC.hpp | 6 +- src/drivers/drv_adc.h | 6 +- src/drivers/rc_input/RCInput.cpp | 11 +- src/drivers/rc_input/RCInput.hpp | 1 + src/modules/battery_status/CMakeLists.txt | 1 - src/modules/battery_status/analog_battery.cpp | 56 +++----- src/modules/battery_status/analog_battery.h | 18 ++- .../analog_battery_params_common.c | 24 ---- src/modules/battery_status/battery_status.cpp | 88 +++++------- src/modules/battery_status/module.yaml | 21 ++- src/modules/sensors/sensors.cpp | 54 +++---- src/systemcmds/tests/CMakeLists.txt | 4 +- .../tests/{test_adc.c => test_adc.cpp} | 56 +++----- src/systemcmds/tests/test_jig_voltages.c | 133 ------------------ src/systemcmds/tests/test_jig_voltages.cpp | 120 ++++++++++++++++ src/systemcmds/tests/tests_main.c | 2 +- 30 files changed, 337 insertions(+), 478 deletions(-) rename src/systemcmds/tests/{test_adc.c => test_adc.cpp} (69%) delete mode 100644 src/systemcmds/tests/test_jig_voltages.c create mode 100644 src/systemcmds/tests/test_jig_voltages.cpp diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index d2f8a61c12..b75deab3a6 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -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 diff --git a/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.cpp b/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.cpp index 7c0ac152fa..e99f0fd1d4 100644 --- a/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.cpp +++ b/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.cpp @@ -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); +} \ No newline at end of file diff --git a/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.hpp b/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.hpp index 2d6bdc8cbd..c148c08946 100644 --- a/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.hpp +++ b/boards/intel/aerofc-v1/aerofc_adc/AEROFC_ADC.hpp @@ -37,18 +37,20 @@ #include #include #include -#include #include #include #include #include #include #include +#include +#include #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 _to_adc_report{ORB_ID(adc_report)}; perf_counter_t _sample_perf; }; diff --git a/boards/intel/aerofc-v1/aerofc_adc/aerofc_adc_main.cpp b/boards/intel/aerofc-v1/aerofc_adc/aerofc_adc_main.cpp index d619e9c6f7..db4640ec6e 100644 --- a/boards/intel/aerofc-v1/aerofc_adc/aerofc_adc_main.cpp +++ b/boards/intel/aerofc-v1/aerofc_adc/aerofc_adc_main.cpp @@ -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; } diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index 64f5d34c0d..279a8438c6 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -32,7 +32,7 @@ px4_add_board( MODULES airspeed_selector attitude_estimator_q - #battery_status + battery_status camera_feedback commander dataman diff --git a/boards/px4/raspberrypi/src/board_config.h b/boards/px4/raspberrypi/src/board_config.h index 9f2e3bc25d..3cc4658b21 100644 --- a/boards/px4/raspberrypi/src/board_config.h +++ b/boards/px4/raspberrypi/src/board_config.h @@ -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 #include diff --git a/msg/adc_report.msg b/msg/adc_report.msg index b4eca08b43..1ae72b6d6c 100644 --- a/msg/adc_report.msg +++ b/msg/adc_report.msg @@ -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) diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index b301c4c8ef..bc8fa8ed89 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -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 */ diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index 2d8da06a11..cf83992c1a 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -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 diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 8bf3eafaef..2a50222b6f 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -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 diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 752c69d050..e14cb29fb4 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -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 diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 7ed8c4083e..3bdad55e8b 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -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 diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index b07ca9e5e8..1d896db9a2 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -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 diff --git a/src/drivers/adc/ADC.cpp b/src/drivers/adc/ADC.cpp index 93a429fbf8..d5befd80b3 100644 --- a/src/drivers/adc/ADC.cpp +++ b/src/drivers/adc/ADC.cpp @@ -31,10 +31,11 @@ * ****************************************************************************/ +#include + #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); - return 1; - } + px4_usleep(20000); // sleep 20ms and wait for adc report - 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 (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); - if (count < 0) { - PX4_ERR("read error"); - return 1; + for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) { + PX4_INFO_RAW("%d: %d ", adc.channel_id[i], adc.raw_data[i]); } - unsigned channels = count / sizeof(data[0]); - - for (unsigned j = 0; j < channels; j++) { - printf("%d: %u ", data[j].am_channel, data[j].am_data); - } + PX4_INFO_RAW("\n"); - printf("\n"); - px4_usleep(100000); - } + PX4_INFO_RAW("\t ADC test successful.\n"); - px4_close(fd); + return 0; - return 0; + } else { + return 1; + } } int ADC::custom_command(int argc, char *argv[]) diff --git a/src/drivers/adc/ADC.hpp b/src/drivers/adc/ADC.hpp index 380d0f5c59..659fa40a75 100644 --- a/src/drivers/adc/ADC.hpp +++ b/src/drivers/adc/ADC.hpp @@ -60,7 +60,7 @@ using namespace time_literals; #define ADC_TOTAL_CHANNELS 32 -class ADC : public ModuleBase, public cdev::CDev, public px4::ScheduledWorkItem +class ADC : public ModuleBase, 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. * diff --git a/src/drivers/drv_adc.h b/src/drivers/drv_adc.h index f0accdf9e5..15d372621a 100644 --- a/src/drivers/drv_adc.h +++ b/src/drivers/drv_adc.h @@ -42,6 +42,8 @@ #include #include +#include +#include /* 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 diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 106f6decd4..00657e1ba4 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -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) { diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index 966e7f3d28..52c0a83f5b 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include diff --git a/src/modules/battery_status/CMakeLists.txt b/src/modules/battery_status/CMakeLists.txt index 0162819cab..df7c9967d4 100644 --- a/src/modules/battery_status/CMakeLists.txt +++ b/src/modules/battery_status/CMakeLists.txt @@ -40,7 +40,6 @@ px4_add_module( MODULE_CONFIG module.yaml DEPENDS - arch_adc battery conversion drivers__device diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index 41bb5756b9..927bb00c43 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -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(_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(_analog_param_handles.adc_channel_old, _analog_param_handles.adc_channel, - &_analog_params.adc_channel_old, &_analog_params.adc_channel, _first_parameter_update); + migrateParam(_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; diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index 3dbc1cb4c2..e7ae037495 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -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; diff --git a/src/modules/battery_status/analog_battery_params_common.c b/src/modules/battery_status/analog_battery_params_common.c index 7fbea4cb05..ed33d09c1b 100644 --- a/src/modules/battery_status/analog_battery_params_common.c +++ b/src/modules/battery_status/analog_battery_params_common.c @@ -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. * diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index e54065a278..87e5f2700f 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -60,7 +60,7 @@ #include #include #include -#include +#include #include #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; - /* 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++) { + if (_adc_report_sub.update(&adc_report)) { - /* Once we have subscriptions, Do this once for the lowest (highest priority - * supply on power controller) that is valid. + /* Read add channels we got */ + 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. + */ + 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; - - } - - /* 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; - } + selected_source = b; + + } + + /* 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(); diff --git a/src/modules/battery_status/module.yaml b/src/modules/battery_status/module.yaml index bf79791028..91359c2947 100644 --- a/src/modules/battery_status/module.yaml +++ b/src/modules/battery_status/module.yaml @@ -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] \ No newline at end of file diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cfe413967a..a4c5ac4b6f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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 _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); } diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index bd3abb967a..2482d2a030 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -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 diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.cpp similarity index 69% rename from src/systemcmds/tests/test_adc.c rename to src/systemcmds/tests/test_adc.cpp index 605440fb11..1f6489350f 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.cpp @@ -37,56 +37,36 @@ */ #include -#include -#include #include - -#include - -#include -#include -#include -#include -#include - #include "tests_main.h" - #include +#include +#include + int test_adc(int argc, char *argv[]) { - int fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("ERROR: can't open ADC device"); - 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)); + uORB::Subscription _adc_sub{ORB_ID(adc_report)}; + adc_report_s adc; - if (count < 0) { - goto errout_with_dev; - } + px4_usleep(50000); // sleep 50ms and wait for adc report - unsigned channels = count / sizeof(data[0]); + 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 (unsigned j = 0; j < channels; j++) { - printf("%d: %u ", data[j].am_channel, data[j].am_data); + for (int i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) { + PX4_INFO_RAW("%d: %d ", adc.channel_id[i], adc.raw_data[i]); } - printf("\n"); - px4_usleep(150000); - } + PX4_INFO_RAW("\n"); - printf("\t ADC test successful.\n"); + PX4_INFO_RAW("\t ADC test successful.\n"); -errout_with_dev: + return OK; - if (fd != 0) { px4_close(fd); } - - return OK; + } else { + return 1; + } } diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c deleted file mode 100644 index 1aa7f96e21..0000000000 --- a/src/systemcmds/tests/test_jig_voltages.c +++ /dev/null @@ -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 -#include - -#include - -#include -#include -#include -#include -#include - -#include "tests_main.h" - -#include -#include - -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; -} diff --git a/src/systemcmds/tests/test_jig_voltages.cpp b/src/systemcmds/tests/test_jig_voltages.cpp new file mode 100644 index 0000000000..e4462c9483 --- /dev/null +++ b/src/systemcmds/tests/test_jig_voltages.cpp @@ -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 +#include +#include "tests_main.h" +#include + +#include +#include + +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; + } +} diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 787de451e3..b7f633c0c3 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -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},