From 96e77275ac30a58667f4bf161952fb7d4f9700d4 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 18 Mar 2024 19:34:53 -0700 Subject: [PATCH] Add drivers DAC_DACx050 and ADC_ADS1115 --- Tools/AP_Periph/AP_Periph.cpp | 12 +++ Tools/AP_Periph/AP_Periph.h | 13 ++++ Tools/AP_Periph/Parameters.cpp | 12 +++ Tools/AP_Periph/Parameters.h | 2 + Tools/AP_Periph/adc.cpp | 108 ++++++++++++++++++++++++++ Tools/AP_Periph/adc.h | 36 +++++++++ Tools/AP_Periph/wscript | 2 + libraries/AP_ADC/AP_ADC_ADS1115.cpp | 14 +++- libraries/AP_ADC/AP_ADC_ADS1115.h | 10 +++ libraries/AP_DAC/AP_DAC_DACx0501.cpp | 109 +++++++++++++++++++++++++++ libraries/AP_DAC/AP_DAC_DACx0501.h | 41 ++++++++++ 11 files changed, 357 insertions(+), 2 deletions(-) create mode 100644 Tools/AP_Periph/adc.cpp create mode 100644 Tools/AP_Periph/adc.h create mode 100644 libraries/AP_DAC/AP_DAC_DACx0501.cpp create mode 100644 libraries/AP_DAC/AP_DAC_DACx0501.h diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index b663855304b19e..a85d8c90bcead1 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -180,6 +180,10 @@ void AP_Periph_FW::init() rcout_init(); #endif +#ifdef HAL_PERIPH_ENABLE_ADC + adc.init(); +#endif + #ifdef HAL_PERIPH_ENABLE_ADSB adsb_init(); #endif @@ -195,6 +199,10 @@ void AP_Periph_FW::init() } #endif +#if AP_DAC_DACx0501_ENABLED + dac.init(); +#endif + #if AP_KDECAN_ENABLED kdecan.init(); #endif @@ -493,6 +501,10 @@ void AP_Periph_FW::update() temperature_sensor.update(); #endif +#ifdef HAL_PERIPH_ENABLE_ADC + adc.update(); +#endif + #ifdef HAL_PERIPH_ENABLE_RPM if (now - rpm_last_update_ms >= 100) { rpm_last_update_ms = now; diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 95d32af7936fff..0c5de8048bd5e1 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +66,10 @@ #error "AP_NMEA_Output requires Serial/GCS and either AHRS or GPS. Needs HAL_GCS_ENABLED and HAL_PERIPH_ENABLE_GPS" #endif +#ifdef HAL_PERIPH_ENABLE_ADC +#include "adc.h" +#endif + #if HAL_GCS_ENABLED #include "GCS_MAVLink.h" #endif @@ -228,6 +233,10 @@ class AP_Periph_FW { uint32_t rpm_last_update_ms; #endif +#ifdef HAL_PERIPH_ENABLE_ADC + Periph_ADC adc; +#endif + #ifdef HAL_PERIPH_ENABLE_BATTERY void handle_battery_failsafe(const char* type_str, const int8_t action) { } AP_BattMonitor battery_lib{0, FUNCTOR_BIND_MEMBER(&AP_Periph_FW::handle_battery_failsafe, void, const char*, const int8_t), nullptr}; @@ -309,6 +318,10 @@ class AP_Periph_FW { uint32_t efi_update_ms; #endif +#if AP_DAC_DACx0501_ENABLED + AP_DAC_DACx0501 dac; +#endif + #if AP_KDECAN_ENABLED AP_KDECAN kdecan; #endif diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 3b00ce8bafc85e..7caef7e0002fb3 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -550,6 +550,18 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(nmea, "NMEA_", AP_NMEA_Output), #endif +#if AP_DAC_DACx0501_ENABLED + // @Group: DAC_ + // @Path: ../libraries/AP_DAC/AP_DAC_DACx0501.cpp + GOBJECT(dac, "DAC_", AP_DAC_DACx0501), +#endif + +#ifdef HAL_PERIPH_ENABLE_ADC + // @Group: ADC + // @Path: adc.cpp + GOBJECT(adc, "ADC_", Periph_ADC), +#endif + #if AP_KDECAN_ENABLED // @Group: KDE_ // @Path: ../libraries/AP_KDECAN/AP_KDECAN.cpp diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 94ac49b39ac05c..711e3cb0a784a4 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -91,6 +91,8 @@ class Parameters { k_param_serial_options, k_param_relay, k_param_temperature_msg_rate, + k_param_adc, + k_param_dac, }; AP_Int16 format_version; diff --git a/Tools/AP_Periph/adc.cpp b/Tools/AP_Periph/adc.cpp new file mode 100644 index 00000000000000..6f10529e949c16 --- /dev/null +++ b/Tools/AP_Periph/adc.cpp @@ -0,0 +1,108 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + + +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_ADC + +#include + +extern const AP_HAL::HAL &hal; + +const AP_Param::GroupInfo Periph_ADC::var_info[] { + + // @Param: ADC_RATE + // @DisplayName: ADC Send Rate Interval + // @Description: ADC Send Rate Interval + // @Range: -1 2000 + // @Increment: 1 + // @Units: ms + AP_GROUPINFO("ADC_RATE", 1, Periph_ADC, params.send_rate, 1000), + + AP_GROUPEND +}; + +void Periph_ADC::init() +{ + lib.init(); +} + +void Periph_ADC::update() +{ + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _last_update_ms >= 10) { + _last_update_ms = now_ms; + IGNORE_RETURN(lib.read(_samples, ARRAY_SIZE(_samples))); + } + + if (params.send_rate >= 1 || params.send_rate == -1) { + const uint32_t last_sample_timestamp_ms = lib.get_last_sample_timestamp_ms(); + const bool new_sample_available = (last_sample_timestamp_ms != 0) && (last_sample_timestamp_ms != _last_sample_timestamp_ms); + + if (params.send_rate == -1) { + if (now_ms - _last_debug_ms >= 1000) { + _last_debug_ms = now_ms; + _last_sample_timestamp_ms = last_sample_timestamp_ms; + + can_printf("ADC%s %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", + new_sample_available ? "" : " STALE", + _samples[0].data, + _samples[1].data, + _samples[2].data, + _samples[3].data, + _samples[4].data, + _samples[5].data); + } + + } else if (new_sample_available && now_ms - _last_send_ms >= params.send_rate) { + _last_sample_timestamp_ms = last_sample_timestamp_ms; + _last_send_ms = now_ms; + + ardupilot_equipment_power_BatteryCells pkt1 {}; + pkt1.voltages.len = MIN(ARRAY_SIZE(_samples),ARRAY_SIZE(pkt1.voltages.data)); + for (uint8_t i=0; i + +class Periph_ADC { +public: + friend class AP_Periph_FW; + + Periph_ADC() { + AP_Param::setup_object_defaults(this, var_info); + } + + void init(); + void update(); + + static const struct AP_Param::GroupInfo var_info[]; + +private: + + struct { + AP_Int32 send_rate; + } params; + + AP_ADC_ADS1115 lib; + adc_report_s _samples[ADS1115_CHANNELS_COUNT]; + uint32_t _last_update_ms; + uint32_t _last_send_ms; + uint32_t _last_sample_timestamp_ms; + uint32_t _last_debug_ms; + +}; + +#endif // HAL_PERIPH_ENABLE_ADC + diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index a3b92b0150a111..ffbddeb35ae4f2 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -66,6 +66,8 @@ def build(bld): 'AP_Logger', 'AC_PID', 'AP_BLHeli', + 'AP_ADC', + 'AP_DAC', 'AP_ESC_Telem', 'AP_Stats', 'AP_EFI', diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.cpp b/libraries/AP_ADC/AP_ADC_ADS1115.cpp index 21f124a88d1816..026cb1db0ad266 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS1115.cpp @@ -2,14 +2,20 @@ #include #include "AP_ADC_ADS1115.h" +#if AP_ADC_ADS1115_ENABLED #define ADS1115_ADDRESS_ADDR_GND 0x48 // address pin low (GND) #define ADS1115_ADDRESS_ADDR_VDD 0x49 // address pin high (VCC) #define ADS1115_ADDRESS_ADDR_SDA 0x4A // address pin tied to SDA pin #define ADS1115_ADDRESS_ADDR_SCL 0x4B // address pin tied to SCL pin +#ifndef ADS1115_I2C_ADDR #define ADS1115_I2C_ADDR ADS1115_ADDRESS_ADDR_GND +#endif + +#ifndef ADS1115_I2C_BUS #define ADS1115_I2C_BUS 1 +#endif #define ADS1115_RA_CONVERSION 0x00 #define ADS1115_RA_CONFIG 0x01 @@ -93,7 +99,6 @@ extern const AP_HAL::HAL &hal; -#define ADS1115_CHANNELS_COUNT 6 const uint8_t AP_ADC_ADS1115::_channels_number = ADS1115_CHANNELS_COUNT; @@ -130,6 +135,8 @@ bool AP_ADC_ADS1115::init() _gain = ADS1115_PGA_4P096; + _dev->set_retries(3); + _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_ADC_ADS1115::_update, void)); return true; @@ -205,7 +212,7 @@ void AP_ADC_ADS1115::_update() uint8_t config[2]; be16_t val; - if (!_dev->read_registers(ADS1115_RA_CONFIG, config, sizeof(config))) { + if (!_dev->read_registers(ADS1115_RA_CONFIG, config, ARRAY_SIZE(config))) { error("_dev->read_registers failed in ADS1115"); return; } @@ -227,4 +234,7 @@ void AP_ADC_ADS1115::_update() /* select next channel */ _channel_to_read = (_channel_to_read + 1) % _channels_number; _start_conversion(_channel_to_read); + + _last_sample_timestamp_ms = AP_HAL::millis(); } +#endif // AP_ADC_ADS1115_ENABLED diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.h b/libraries/AP_ADC/AP_ADC_ADS1115.h index 6816280b685f5d..d04f0273e2cf7e 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.h +++ b/libraries/AP_ADC/AP_ADC_ADS1115.h @@ -4,6 +4,13 @@ #include #include +#ifndef AP_ADC_ADS1115_ENABLED +#define AP_ADC_ADS1115_ENABLED 0 +#endif + +#define ADS1115_CHANNELS_COUNT 6 + +#if AP_ADC_ADS1115_ENABLED struct adc_report_s { uint8_t id; @@ -18,6 +25,7 @@ class AP_ADC_ADS1115 bool init(); size_t read(adc_report_s *report, size_t length) const; + uint32_t get_last_sample_timestamp_ms() const { return _last_sample_timestamp_ms;} uint8_t get_channels_number() const { @@ -32,9 +40,11 @@ class AP_ADC_ADS1115 uint16_t _gain; int _channel_to_read; adc_report_s *_samples; + uint32_t _last_sample_timestamp_ms; void _update(); bool _start_conversion(uint8_t channel); float _convert_register_data_to_mv(int16_t word) const; }; +#endif // AP_ADC_ADS1115_ENABLED diff --git a/libraries/AP_DAC/AP_DAC_DACx0501.cpp b/libraries/AP_DAC/AP_DAC_DACx0501.cpp new file mode 100644 index 00000000000000..dcb5be6a79262b --- /dev/null +++ b/libraries/AP_DAC/AP_DAC_DACx0501.cpp @@ -0,0 +1,109 @@ + +#include "AP_DAC_DACx0501.h" + +#if AP_DAC_DACx0501_ENABLED +#include +#include + +extern const AP_HAL::HAL &hal; + +#define DACx0501_ADDRESS_ADDR_GND 0x48 // address pin low (GND) +#define DACx0501_ADDRESS_ADDR_VDD 0x49 // address pin high (VDD) +#define DACx0501_ADDRESS_ADDR_SDA 0x4A // address pin tied to SDA pin +#define DACx0501_ADDRESS_ADDR_SCL 0x4B // address pin tied to SCL pin + +#ifndef DACx0501_I2C_ADDR +#define DACx0501_I2C_ADDR DACx0501_ADDRESS_ADDR_GND +#endif + +#ifndef DACx0501_I2C_BUS +#define DACx0501_I2C_BUS 0 +#endif + +// Registers +#define DACx0501_REG_NOOP 0x00 // no operation, this should never be used intentionally +#define DACx0501_REG_DEVID 0x01 // same as WHO_AM_I +#define DACx0501_REG_SYNC 0x02 +#define DACx0501_REG_CONFIG 0x03 +#define DACx0501_REG_GAIN 0x04 +#define DACx0501_REG_TRIGGER 0x05 +//#define DACx0501_REG_UNKNOWN 0x06 +#define DACx0501_REG_STATUS 0x07 +#define DACx0501_REG_DAC_DATA 0x08 + +#define DACx0501_DEVID 0x0015 // base ID. More bit sget set for other flavors of the chip + +// table of user settable CAN bus parameters +const AP_Param::GroupInfo AP_DAC_DACx0501::var_info[] = { + + // @Param: ADDR + // @DisplayName: I2C address + // @Description: I2C address + AP_GROUPINFO("ADDR", 1, AP_DAC_DACx0501, params.i2c_address, DACx0501_I2C_ADDR), + + // @Param: BUS + // @DisplayName: I2C bus + // @Description: I2C bus + AP_GROUPINFO("BUS", 2, AP_DAC_DACx0501, params.i2c_bus, DACx0501_I2C_BUS), + + // @Param: SRV_INDEX + // @DisplayName: SRV_INDEX + // @Description: SRV_INDEX + AP_GROUPINFO("SRV_INDEX", 3, AP_DAC_DACx0501, params.servo_index, -1), + + // @Param: BIT_RES + // @DisplayName: BIT_RES + // @Description: BIT_RESolution of the DAC. + // @Values: 12:12-bit, 14:14-bit, 16:16-bit + AP_GROUPINFO("BIT_RES", 4, AP_DAC_DACx0501, params.bit_resolution, 12), + + // @Param: GAIN + // @DisplayName: GAIN + // @Description: GAIN + // @Range: 0.01 100 + // @Incremenet: 0.01 + AP_GROUPINFO("GAIN", 5, AP_DAC_DACx0501, params.gain, 1), + + AP_GROUPEND +}; + +void AP_DAC_DACx0501::init() +{ + _dev = hal.i2c_mgr->get_device(params.i2c_bus, params.i2c_address); + if (!_dev) { + return; + } + + _dev->set_retries(10); + // Set gain to unity + uint8_t buf[3]; + buf[0] = DACx0501_REG_GAIN; + buf[1] = 0; + buf[2] = 0; + UNUSED_RESULT(_dev->transfer(buf, sizeof(buf), nullptr, 0)); + _dev->set_retries(3); + + _dev->register_periodic_callback(20 * 1000, FUNCTOR_BIND_MEMBER(&AP_DAC_DACx0501::thread, void)); +} + +void AP_DAC_DACx0501::thread() +{ + const SRV_Channel *c = SRV_Channels::srv_channel(params.servo_index); + if (c == nullptr || !c->valid_function()) { + return; + } + + const uint16_t bit_resolution = constrain_int16(params.bit_resolution.get(), 12, 16); + const uint16_t max_output = (1U << bit_resolution) - 1; + + uint16_t value = linear_interpolate(0, max_output, c->get_output_pwm(), c->get_output_min(), c->get_output_max()); + value *= MAX(0.01f, params.gain.get()); + + uint8_t buf[3]; + buf[0] = DACx0501_REG_DAC_DATA; + buf[1] = HIGHBYTE(value); + buf[2] = LOWBYTE(value); + + UNUSED_RESULT(_dev->transfer(buf, sizeof(buf), nullptr, 0)); +} +#endif // AP_DAC_DACx0501_ENABLED diff --git a/libraries/AP_DAC/AP_DAC_DACx0501.h b/libraries/AP_DAC/AP_DAC_DACx0501.h new file mode 100644 index 00000000000000..50cffa83e852d2 --- /dev/null +++ b/libraries/AP_DAC/AP_DAC_DACx0501.h @@ -0,0 +1,41 @@ +#pragma once + +#include + +#ifndef AP_DAC_DACx0501_ENABLED +#define AP_DAC_DACx0501_ENABLED 0 +#endif + +#if AP_DAC_DACx0501_ENABLED +#include +#include + +class AP_DAC_DACx0501 +{ +public: + AP_DAC_DACx0501() { + AP_Param::setup_object_defaults(this, var_info); + } + + /* Do not allow copies */ + CLASS_NO_COPY(AP_DAC_DACx0501); + + static const struct AP_Param::GroupInfo var_info[]; + + void init(); + +private: + AP_HAL::OwnPtr _dev; + + struct { + AP_Int8 i2c_address; + AP_Int8 i2c_bus; + AP_Int16 servo_index; + AP_Int16 bit_resolution; + AP_Float gain; + } params; + + void thread(); +}; + +#endif // AP_DAC_DACx0501_ENABLED