diff --git a/Tools/AP_Periph/adc.cpp b/Tools/AP_Periph/adc.cpp index 6f10529e949c16..3eddb4ced5d41b 100644 --- a/Tools/AP_Periph/adc.cpp +++ b/Tools/AP_Periph/adc.cpp @@ -31,40 +31,70 @@ const AP_Param::GroupInfo Periph_ADC::var_info[] { // @Increment: 1 // @Units: ms AP_GROUPINFO("ADC_RATE", 1, Periph_ADC, params.send_rate, 1000), - + + // @Group: GUIDED_ + // @Path: ../libraries/AC_PID/AC_PID.cpp + AP_SUBGROUPINFO(pid, "PID_", 2, Periph_ADC, AC_PID), + + // @Param: SRV_INDEX + // @DisplayName: SRV_INDEX + // @Description: SRV_INDEX + AP_GROUPINFO("SRV_INDEX", 3, Periph_ADC, params.servo_index, -1), + + AP_GROUPEND }; void Periph_ADC::init() { - lib.init(); + // this creates a thread in the background to read the ADC via I2C + adc_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))); + const float dt = (now_ms - _last_pid_update_ms) * 1e-3f; + _last_pid_update_ms = now_ms; + + + // get a new sample + const uint32_t last_sample_timestamp_ms = adc_lib.get_last_sample_timestamp_ms(); + const bool new_sample_available = (last_sample_timestamp_ms != 0) && (last_sample_timestamp_ms != _last_sample_timestamp_ms); + + + // params.servo_index + float target_srv_in = 0; + const SRV_Channel *c = SRV_Channels::srv_channel(params.servo_index); + if (c != nullptr && c->valid_function()) { + target_srv_in = linear_interpolate(0, 5.0, c->get_output_pwm(), c->get_output_min(), c->get_output_max()); } - 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); + float measurement_adc_in = adc_lib.read_by_channel(2); + const float output_dac_out = pid.update_all(target_srv_in, measurement_adc_in, dt); + + periph.dac.set_output_voltage(output_dac_out); - if (params.send_rate == -1) { + // TODO: push to 4 other servo outputs but with true PWM output at 20kHz + + + + + + + + // debug stuff + if (params.send_rate >= 1 || params.send_rate == -1) { + 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", + can_printf("ADC%s AN0SE:%.2f, AN1SE:%.2f, AN23DIF:%.2f", new_sample_available ? "" : " STALE", - _samples[0].data, - _samples[1].data, - _samples[2].data, - _samples[3].data, - _samples[4].data, - _samples[5].data); + adc_lib.read_by_channel(0), + adc_lib.read_by_channel(1), + adc_lib.read_by_channel(2)); } } else if (new_sample_available && now_ms - _last_send_ms >= params.send_rate) { @@ -72,9 +102,9 @@ void Periph_ADC::update() _last_send_ms = now_ms; ardupilot_equipment_power_BatteryCells pkt1 {}; - pkt1.voltages.len = MIN(ARRAY_SIZE(_samples),ARRAY_SIZE(pkt1.voltages.data)); + pkt1.voltages.len = MIN(ADS1115_CHANNELS_COUNT,ARRAY_SIZE(pkt1.voltages.data)); for (uint8_t i=0; i +#include class Periph_ADC { public: @@ -19,16 +20,33 @@ class Periph_ADC { private: + AC_PID pid { + // Initial Conditions + AC_PID::Defaults{ + .p = 1, + .i = 0, + .d = 0, + .ff = 0.0f, + .imax = 1000, + .filt_T_hz = 50, + .filt_E_hz = 0.0f, + .filt_D_hz = 50, + .srmax = 0, + .srtau = 1.0 + } + }; + + struct { AP_Int32 send_rate; + AP_Int16 servo_index; } params; - AP_ADC_ADS1115 lib; - adc_report_s _samples[ADS1115_CHANNELS_COUNT]; - uint32_t _last_update_ms; + AP_ADC_ADS1115 adc_lib; uint32_t _last_send_ms; uint32_t _last_sample_timestamp_ms; uint32_t _last_debug_ms; + uint32_t _last_pid_update_ms; }; diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.cpp b/libraries/AP_ADC/AP_ADC_ADS1115.cpp index 872e2154233edc..1edb3a37817019 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS1115.cpp @@ -155,14 +155,27 @@ bool AP_ADC_ADS1115::_start_conversion(uint8_t channel) return _dev->transfer((uint8_t *)&config, sizeof(config), nullptr, 0); } +float AP_ADC_ADS1115::read_by_channel(uint32_t channel_id) const +{ + WITH_SEMAPHORE(_dev->get_semaphore()); + for (uint32_t i = 0; i < _channels_number; i++) { + if (channel_id == _samples[i].id) { + return _samples[i].data; + } + } + return 0; +} + size_t AP_ADC_ADS1115::read(adc_report_s *report, size_t length) const { - for (size_t i = 0; i < length; i++) { + WITH_SEMAPHORE(_dev->get_semaphore()); + const uint16_t len = MIN(length, _channels_number); + for (size_t i = 0; i < len; i++) { report[i].data = _samples[i].data; report[i].id = _samples[i].id; } - return length; + return len; } float AP_ADC_ADS1115::_convert_register_data_to_mv(int16_t word) const @@ -224,8 +237,10 @@ void AP_ADC_ADS1115::_update() return; } - float sample = _convert_register_data_to_mv(be16toh(val)); + const float sample = _convert_register_data_to_mv(be16toh(val)); + _last_sample_timestamp_ms = AP_HAL::millis(); + WITH_SEMAPHORE(_dev->get_semaphore()); _samples[_channel_to_read].data = sample; _samples[_channel_to_read].id = _channel_to_read; @@ -233,6 +248,5 @@ void AP_ADC_ADS1115::_update() _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 2e80e3f3d66554..bcf6647d7b7acd 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.h +++ b/libraries/AP_ADC/AP_ADC_ADS1115.h @@ -2,7 +2,6 @@ #include #include -#include #ifndef AP_ADC_ADS1115_ENABLED #define AP_ADC_ADS1115_ENABLED 0 @@ -11,6 +10,10 @@ #define ADS1115_CHANNELS_COUNT 3 #if AP_ADC_ADS1115_ENABLED + +#include +#include + struct adc_report_s { uint8_t id; @@ -24,6 +27,7 @@ class AP_ADC_ADS1115 ~AP_ADC_ADS1115(); bool init(); + float read_by_channel(uint32_t id) const; size_t read(adc_report_s *report, size_t length) const; uint32_t get_last_sample_timestamp_ms() const { return _last_sample_timestamp_ms;} diff --git a/libraries/AP_DAC/AP_DAC_DACx0501.cpp b/libraries/AP_DAC/AP_DAC_DACx0501.cpp index dcb5be6a79262b..06d3b1b07ce52a 100644 --- a/libraries/AP_DAC/AP_DAC_DACx0501.cpp +++ b/libraries/AP_DAC/AP_DAC_DACx0501.cpp @@ -46,11 +46,6 @@ const AP_Param::GroupInfo AP_DAC_DACx0501::var_info[] = { // @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. @@ -64,6 +59,13 @@ const AP_Param::GroupInfo AP_DAC_DACx0501::var_info[] = { // @Incremenet: 0.01 AP_GROUPINFO("GAIN", 5, AP_DAC_DACx0501, params.gain, 1), + // @Param: INITIAL + // @DisplayName: Initial Voltage + // @Description: Initial Voltage at boot + // @Range: 0.0 5.0 + // @Incremenet: 0.01 + AP_GROUPINFO("INITIAL", 6, AP_DAC_DACx0501, params.initial_voltage, 0), + AP_GROUPEND }; @@ -73,6 +75,7 @@ void AP_DAC_DACx0501::init() if (!_dev) { return; } + target_voltage = params.initial_voltage.get(); _dev->set_retries(10); // Set gain to unity @@ -81,22 +84,19 @@ void AP_DAC_DACx0501::init() buf[1] = 0; buf[2] = 0; UNUSED_RESULT(_dev->transfer(buf, sizeof(buf), nullptr, 0)); - _dev->set_retries(3); + _dev->set_retries(2); - _dev->register_periodic_callback(20 * 1000, FUNCTOR_BIND_MEMBER(&AP_DAC_DACx0501::thread, void)); + const float samples_per_second_per_channel_Hz = 50; + const float interval_us = (1.0f/samples_per_second_per_channel_Hz) * AP_USEC_PER_SEC; + _dev->register_periodic_callback(interval_us, 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()); + uint16_t value = linear_interpolate(0, max_output, target_voltage, get_voltage_min(), get_voltage_max()); value *= MAX(0.01f, params.gain.get()); uint8_t buf[3]; diff --git a/libraries/AP_DAC/AP_DAC_DACx0501.h b/libraries/AP_DAC/AP_DAC_DACx0501.h index 50cffa83e852d2..5bedc4a4d7e4b5 100644 --- a/libraries/AP_DAC/AP_DAC_DACx0501.h +++ b/libraries/AP_DAC/AP_DAC_DACx0501.h @@ -23,6 +23,9 @@ class AP_DAC_DACx0501 static const struct AP_Param::GroupInfo var_info[]; void init(); + void set_output_voltage(float value) { target_voltage = value; } + float get_voltage_min() const { return 0.0f; } + float get_voltage_max() const { return 5.0f; } private: AP_HAL::OwnPtr _dev; @@ -30,11 +33,12 @@ class AP_DAC_DACx0501 struct { AP_Int8 i2c_address; AP_Int8 i2c_bus; - AP_Int16 servo_index; AP_Int16 bit_resolution; AP_Float gain; + AP_Float initial_voltage; } params; + float target_voltage; void thread(); };