Skip to content

Commit

Permalink
add PID
Browse files Browse the repository at this point in the history
  • Loading branch information
@magicrub
magicrub committed Mar 19, 2024
1 parent 7c177dc commit cd19114
Show file tree
Hide file tree
Showing 6 changed files with 112 additions and 42 deletions.
70 changes: 50 additions & 20 deletions Tools/AP_Periph/adc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,50 +31,80 @@ 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) {
_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));
pkt1.voltages.len = MIN(ADS1115_CHANNELS_COUNT,ARRAY_SIZE(pkt1.voltages.data));
for (uint8_t i=0; i<pkt1.voltages.len; i++) {
pkt1.voltages.data[i] = _samples[i].data;
pkt1.voltages.data[i] = adc_lib.read_by_channel(i);
}

uint8_t buffer1[ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_MAX_SIZE] {};
Expand All @@ -88,9 +118,9 @@ void Periph_ADC::update()


rb_ADC pkt2 {};
pkt2.voltages.len = MIN(ARRAY_SIZE(_samples),ARRAY_SIZE(pkt2.voltages.data));
pkt2.voltages.len = MIN(ADS1115_CHANNELS_COUNT,ARRAY_SIZE(pkt2.voltages.data));
for (uint8_t i=0; i<pkt2.voltages.len; i++) {
pkt2.voltages.data[i] = _samples[i].data;
pkt2.voltages.data[i] = adc_lib.read_by_channel(i);
}

uint8_t buffer2[RB_ADC_MAX_SIZE] {};
Expand Down
24 changes: 21 additions & 3 deletions Tools/AP_Periph/adc.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#ifdef HAL_PERIPH_ENABLE_ADC

#include <AP_ADC/AP_ADC_ADS1115.h>
#include <AC_PID/AC_PID.h>

class Periph_ADC {
public:
Expand All @@ -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;

};

Expand Down
22 changes: 18 additions & 4 deletions libraries/AP_ADC/AP_ADC_ADS1115.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -224,15 +237,16 @@ 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;

/* 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
6 changes: 5 additions & 1 deletion libraries/AP_ADC/AP_ADC_ADS1115.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@

#include <inttypes.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>

#ifndef AP_ADC_ADS1115_ENABLED
#define AP_ADC_ADS1115_ENABLED 0
Expand All @@ -11,6 +10,10 @@
#define ADS1115_CHANNELS_COUNT 3

#if AP_ADC_ADS1115_ENABLED

#include <AP_HAL/Semaphores.h>
#include <AP_HAL/I2CDevice.h>

struct adc_report_s
{
uint8_t id;
Expand All @@ -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;}

Expand Down
26 changes: 13 additions & 13 deletions libraries/AP_DAC/AP_DAC_DACx0501.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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
};

Expand All @@ -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
Expand All @@ -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];
Expand Down
6 changes: 5 additions & 1 deletion libraries/AP_DAC/AP_DAC_DACx0501.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,18 +23,22 @@ 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<AP_HAL::I2CDevice> _dev;

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();
};

Expand Down

0 comments on commit cd19114

Please sign in to comment.