From f6b98003a552caed8034ebeda2021c4c34dfe159 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 13 Mar 2024 18:41:16 -0700 Subject: [PATCH] Add support for ADS1115 ADC in Periph --- Tools/AP_Periph/AP_Periph.cpp | 45 +++++++++++++++++++ Tools/AP_Periph/AP_Periph.h | 11 +++++ Tools/AP_Periph/Parameters.cpp | 10 +++++ Tools/AP_Periph/Parameters.h | 5 +++ libraries/AP_ADC/AP_ADC_ADS1115.cpp | 2 + libraries/AP_ADC/AP_ADC_ADS1115.h | 6 +++ libraries/AP_DroneCAN/dsdl/rb/31000.uavcan | 6 +++ .../hwdef/CubeOrange-periph/hwdef.dat | 2 +- 8 files changed, 86 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_DroneCAN/dsdl/rb/31000.uavcan diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index b663855304b19..5fa62dc071bd3 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -24,6 +24,7 @@ #include #include "AP_Periph.h" #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include @@ -180,6 +181,10 @@ void AP_Periph_FW::init() rcout_init(); #endif +#if AP_ADC_ADS1115_ENABLED + adc.lib.init(); +#endif + #ifdef HAL_PERIPH_ENABLE_ADSB adsb_init(); #endif @@ -493,6 +498,46 @@ void AP_Periph_FW::update() temperature_sensor.update(); #endif +#if AP_ADC_ADS1115_ENABLED + if (now - adc.last_update_ms >= 10) { + adc.last_update_ms = now; + IGNORE_RETURN(adc.lib.read(adc.samples, ARRAY_SIZE(adc.samples))); + } + if (g.adc_send_rate > 0 && now - adc.last_send_ms >= g.adc_send_rate) { + adc.last_send_ms = now; + + ardupilot_equipment_power_BatteryCells pkt1 {}; + pkt1.voltages.len = MIN(ARRAY_SIZE(adc.samples),ARRAY_SIZE(pkt1.voltages.data)); + for (uint8_t i=0; i= 100) { rpm_last_update_ms = now; diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index d14885a9de124..ba10888758834 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -56,6 +56,8 @@ #endif #endif +#include + #if defined(HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE) && !AP_TEMPERATURE_SENSOR_ENABLED #error "HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE requires AP_TEMPERATURE_SENSOR_ENABLED" #endif @@ -229,6 +231,15 @@ class AP_Periph_FW { uint32_t rpm_last_update_ms; #endif +#if AP_ADC_ADS1115_ENABLED + struct { + AP_ADC_ADS1115 lib; + adc_report_s samples[ADS1115_CHANNELS_COUNT]; + uint32_t last_update_ms; + uint32_t last_send_ms; + } 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}; diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 3b00ce8bafc85..532e40ecc14fb 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -510,6 +510,16 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(efi, "EFI", AP_EFI), #endif +#if AP_ADC_ADS1115_ENABLED + // @Param: ADC_RATE + // @DisplayName: ADC Send Rate Interval + // @Description: ADC Send Rate Interval + // @Range: 0 2000 + // @Increment: 1 + // @Units: ms + GSCALAR(adc_send_rate, "ADC_RATE", 1), +#endif + #ifdef HAL_PERIPH_ENABLE_PROXIMITY // @Param: PRX_BAUDRATE // @DisplayName: Proximity Sensor serial baudrate diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 94ac49b39ac05..c30245b5a90b9 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -91,6 +91,7 @@ class Parameters { k_param_serial_options, k_param_relay, k_param_temperature_msg_rate, + k_param_adc_send_rate, }; AP_Int16 format_version; @@ -201,6 +202,10 @@ class Parameters { AP_Int8 efi_port; #endif +#if AP_ADC_ADS1115_ENABLED + AP_Int32 adc_send_rate; +#endif + #if HAL_PERIPH_CAN_MIRROR AP_Int8 can_mirror_ports; #endif // HAL_PERIPH_CAN_MIRROR diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.cpp b/libraries/AP_ADC/AP_ADC_ADS1115.cpp index 47ceae7bfacc6..faece20badd8d 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS1115.cpp @@ -2,6 +2,7 @@ #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) @@ -227,3 +228,4 @@ void AP_ADC_ADS1115::_update() _channel_to_read = (_channel_to_read + 1) % _channels_number; _start_conversion(_channel_to_read); } +#endif // AP_ADC_ADS1115_ENABLED diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.h b/libraries/AP_ADC/AP_ADC_ADS1115.h index f978861451be0..3913723517478 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.h +++ b/libraries/AP_ADC/AP_ADC_ADS1115.h @@ -4,8 +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; @@ -40,3 +45,4 @@ class AP_ADC_ADS1115 float _convert_register_data_to_mv(int16_t word) const; }; +#endif // AP_ADC_ADS1115_ENABLED diff --git a/libraries/AP_DroneCAN/dsdl/rb/31000.uavcan b/libraries/AP_DroneCAN/dsdl/rb/31000.uavcan new file mode 100644 index 0000000000000..ea10a4decbf05 --- /dev/null +++ b/libraries/AP_DroneCAN/dsdl/rb/31000.uavcan @@ -0,0 +1,6 @@ +# +# ADC voltages +# + +float32[<=10] voltages # [Volt] + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat index 8aef94756f872..836813045eb95 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat @@ -61,4 +61,4 @@ define HAL_CAN_DRIVER_DEFAULT 1 # undefine to disable. Use -1 to allow on all ports, otherwise serial number index defined in SERIAL_ORDER starting at 0 define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0 - +define AP_ADC_ADS1115_ENABLED 1