From 632e099f3a5ef25794934d1d717358f5f93821c9 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Thu, 14 Mar 2024 14:40:13 -0700 Subject: [PATCH] DAC WIP --- Tools/AP_Periph/AP_Periph.cpp | 4 + Tools/AP_Periph/AP_Periph.h | 5 + Tools/AP_Periph/Parameters.cpp | 6 ++ Tools/AP_Periph/Parameters.h | 1 + Tools/AP_Periph/wscript | 1 + libraries/AP_ADC/AP_ADC_ADS1115.cpp | 2 + libraries/AP_DAC/AP_DAC_DACx0501.cpp | 100 ++++++++++++++++++ libraries/AP_DAC/AP_DAC_DACx0501.h | 41 +++++++ .../hwdef/CubeOrange-periph/hwdef.dat | 3 + 9 files changed, 163 insertions(+) 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 32be57bb3e236..8226894827267 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -200,6 +200,10 @@ void AP_Periph_FW::init() } #endif +#if AP_DAC_DACx0501_ENABLED + dac.init(); +#endif + #if AP_KDECAN_ENABLED kdecan.init(); #endif diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index a3b7205a623f0..02a9938bf545b 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 @@ -323,6 +324,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 ec48dd7606614..04794364f83a2 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -560,6 +560,12 @@ 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 + #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 c30245b5a90b9..457ff17a2e73f 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -92,6 +92,7 @@ class Parameters { k_param_relay, k_param_temperature_msg_rate, k_param_adc_send_rate, + k_param_dac, }; AP_Int16 format_version; diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index 74244e3e19322..ffbddeb35ae4f 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -67,6 +67,7 @@ def build(bld): '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 86e1ca4c99c0d..4962ad44d4eba 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS1115.cpp @@ -140,6 +140,8 @@ bool AP_ADC_ADS1115::init() _samples[i].data = -1.0f; } + _dev->set_retries(3); + _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_ADC_ADS1115::_update, void)); return true; diff --git a/libraries/AP_DAC/AP_DAC_DACx0501.cpp b/libraries/AP_DAC/AP_DAC_DACx0501.cpp new file mode 100644 index 0000000000000..810dda845f160 --- /dev/null +++ b/libraries/AP_DAC/AP_DAC_DACx0501.cpp @@ -0,0 +1,100 @@ + +#include "AP_DAC_DACx0501.h" + +#if AP_DAC_DACx0501_ENABLED +#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(3); + _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, 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 0000000000000..50cffa83e852d --- /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 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat index 6f092875c0e78..425d456279b55 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat @@ -65,3 +65,6 @@ define AP_ADC_ADS1115_ENABLED 1 define ADS1115_I2C_BUS 0 define HAL_I2C_MAX_CLOCK 400000 +define AP_DAC_DACx0501_ENABLED 1 +define DACx0501_I2C_ADDR 0x49 # address pin high (VDD) +define DACx0501_I2C_BUS 0