From 5874ba4f78cd59445adf55c427772b6427020a5b Mon Sep 17 00:00:00 2001 From: Mario Goebbels Date: Sat, 3 Feb 2018 01:07:45 +0100 Subject: [PATCH] actuator: Implement Smith predictor for actuator delay compensation. --- flight/Modules/Actuator/actuator.c | 106 +++++++++++++++++- .../uavobjectdefinition/actuatorsettings.xml | 9 ++ 2 files changed, 113 insertions(+), 2 deletions(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index f954dc3455..4970b761f4 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -51,6 +51,7 @@ #include "manualcontrolcommand.h" #include "pios_thread.h" #include "pios_queue.h" +#include "pios_sensors.h" #include "misc_math.h" // Private constants @@ -98,6 +99,29 @@ static MixerSettingsMixer1TypeOptions types_mixer[MAX_MIX_ACTUATORS]; static float motor_mixer[MAX_MIX_ACTUATORS * MIXERSETTINGS_MIXER1VECTOR_NUMELEM]; static float motor_mixer_inv[MAX_MIX_ACTUATORS * MIXERSETTINGS_MIXER1VECTOR_NUMELEM]; +static bool inverse_mixer_computed = false; + +#define SMITHP_MAX_DELAY 64 + +struct smith_predictor { + + uint32_t samples; + uint32_t idx; + + /* This is gonna be a ring buffer. */ + float *data; + + float iir_alpha; + float iir[MAX_MIX_ACTUATORS]; + + float mix; +}; + +static struct smith_predictor* smithp_init(float delay, float iir, float mix); +static void smithp_push_vect(struct smith_predictor *m, float *motor_vect); +static void smithp_compensate(struct smith_predictor *m, float *motor_vect); + +struct smith_predictor *smithp; /* These are various settings objects used throughout the actuator code */ static ActuatorSettingsData actuatorSettings; @@ -740,6 +764,14 @@ static void actuator_settings_update() } hangtime_leakybucket_timeconstant = actuatorSettings.LowPowerStabilizationTimeConstant; + + if (!smithp && actuatorSettings.SmithPredictorDelay > 0.1f) { + float dT = 1.0f / (float)PIOS_SENSORS_GetSampleRate(PIOS_SENSOR_GYRO) * 1000.0f; + int delay = (int)(actuatorSettings.SmithPredictorDelay / dT + 0.5f); + if (delay > 0) { + smithp = smithp_init(delay, actuatorSettings.SmithPredictorIIR, actuatorSettings.SmithPredictorMix); + } + } } /** @@ -793,8 +825,7 @@ static void actuator_task(void* parameters) * set failsafe. */ if (compute_inverse_mixer()) { - set_failsafe(); - continue; + inverse_mixer_computed = true; } MixerSettingsThrottleCurve2Get(curve2); @@ -874,6 +905,8 @@ static void actuator_task(void* parameters) normalize_input_data(this_systime, &desired_vect, &armed, &spin_while_armed, &stabilize_now); + smithp_compensate(smithp, desired_vect); + /* Multiply the actuators x desired matrix by the * desired x 1 column vector. */ matrix_mul_check(motor_mixer, desired_vect, motor_vect, @@ -906,6 +939,8 @@ static void actuator_task(void* parameters) dT, armed, spin_while_armed, stabilize_now, &maxpoweradd_bucket); + smithp_push_vect(smithp, motor_vect); + /* If we got this far, everything is OK. */ AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); } @@ -1111,6 +1146,73 @@ static void set_failsafe() ActuatorCommandChannelSet(Channel); } +static struct smith_predictor* smithp_init(float delay, float iir, float mix) +{ + struct smith_predictor *m = PIOS_malloc_no_dma(sizeof(*m)); + if (!m) + return NULL; + memset(m, 0, sizeof(*m)); + + if (delay > SMITHP_MAX_DELAY) delay = SMITHP_MAX_DELAY; + + m->samples = delay; + m->iir_alpha = iir; + m->mix = mix; + + m->data = PIOS_malloc_no_dma(delay * MAX_MIX_ACTUATORS * sizeof(*m->data)); + if (!m->data) + return NULL; + memset(m->data, 0, sizeof(*m->data) * delay * MAX_MIX_ACTUATORS); + + return m; +} + +/* + Call order for it to work should be: + - Compensate + - Push vect +*/ + +static void smithp_push_vect(struct smith_predictor *m, float *motor_vect) +{ + if (!m) return; + + uint32_t p = (m->idx % m->samples) * MAX_MIX_ACTUATORS; + + /* Write motor vector to position. */ + for (int i = 0; i < MAX_MIX_ACTUATORS; i++) { + float v = motor_vect[i]; + m->data[p++] = v != v ? 0 : v; + } + + /* Moves pointer to oldest data point (via modulo) for _compensate. */ + m->idx++; +} + +static void smithp_compensate(struct smith_predictor *m, float *desired_vect) +{ + if (!m || !inverse_mixer_computed) return; + + uint32_t p = (m->idx % m->samples) * MAX_MIX_ACTUATORS; + + float inv[MIXERSETTINGS_MIXER1VECTOR_NUMELEM]; + + for (int i = 0; i < MAX_MIX_ACTUATORS; i++) { + /* Do IIR on delayed data*/ + m->iir[i] = m->iir[i] * m->iir_alpha + (1 - m->iir_alpha) * m->data[p++]; + } + + /* Do inverse stuff. */ + matrix_mul_check(motor_mixer_inv, m->iir, inv, + MIXERSETTINGS_MIXER1VECTOR_NUMELEM, + MAX_MIX_ACTUATORS, + 1); + + for (int i = 0; i < MIXERSETTINGS_MIXER1VECTOR_NUMELEM; i++) { + desired_vect[i] += m->mix * (desired_vect[i] - inv[i]); + } +} + /** * @} * @} diff --git a/shared/uavobjectdefinition/actuatorsettings.xml b/shared/uavobjectdefinition/actuatorsettings.xml index fa984221e4..9ec21defb9 100644 --- a/shared/uavobjectdefinition/actuatorsettings.xml +++ b/shared/uavobjectdefinition/actuatorsettings.xml @@ -39,5 +39,14 @@ Actuator mapping of input to reduce the maximum values sent to motors. Provides "virtual KV" functionality; e.g. you can use 0.67 to drive 4S motors from 6S. This setting is applied after the motor curve fit. + + By how much milliseconds to delay the actuator commands fed into the predictor. + + + IIR coefficient for smoothing. + + + Amount of contribution by the smith predictor to the control signal. +