From 00000042f16fe0182bb6aff8e5c956710ec35b25 Mon Sep 17 00:00:00 2001 From: Michael Lyle Date: Sun, 20 Aug 2017 00:50:58 -0700 Subject: [PATCH 1/5] actuator: generate an inverse mixer matrix --- flight/Modules/Actuator/actuator.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 60986298ba..f954dc3455 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -97,6 +97,7 @@ 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]; /* These are various settings objects used throughout the actuator code */ static ActuatorSettingsData actuatorSettings; @@ -410,6 +411,12 @@ static void compute_mixer() #endif } +static bool compute_inverse_mixer() +{ + return matrix_pseudoinv(motor_mixer, motor_mixer_inv, + MAX_MIX_ACTUATORS, MIXERSETTINGS_MIXER1VECTOR_NUMELEM); +} + static void fill_desired_vector( ActuatorDesiredData *desired, float val1, float val2, @@ -782,6 +789,14 @@ static void actuator_task(void* parameters) compute_mixer(); + /* If we can't calculate a proper inverse mixer, + * set failsafe. + */ + if (compute_inverse_mixer()) { + set_failsafe(); + continue; + } + MixerSettingsThrottleCurve2Get(curve2); MixerSettingsCurve2SourceGet(&curve2_src); settings_updated = false; From 5874ba4f78cd59445adf55c427772b6427020a5b Mon Sep 17 00:00:00 2001 From: Mario Goebbels Date: Sat, 3 Feb 2018 01:07:45 +0100 Subject: [PATCH 2/5] 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. + From 241d738d7116d47354fb218d9a8cd16c547765f7 Mon Sep 17 00:00:00 2001 From: Mario Goebbels Date: Tue, 17 Apr 2018 20:16:30 +0200 Subject: [PATCH 3/5] actuator: Make sure throttle is fine after the Smith predictor. --- flight/Modules/Actuator/actuator.c | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 4970b761f4..3f4215800a 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -76,6 +76,7 @@ DONT_BUILD_IF((MIXERSETTINGS_MIXER1VECTOR_NUMELEM - MIXERSETTINGS_MIXER1VECTOR_A #define MIXER_SCALE 128 #define ACTUATOR_EPSILON 0.00001f +#define THROTTLE_EPSILON 0.000001f // Private types @@ -1209,7 +1210,23 @@ static void smithp_compensate(struct smith_predictor *m, float *desired_vect) 1); for (int i = 0; i < MIXERSETTINGS_MIXER1VECTOR_NUMELEM; i++) { - desired_vect[i] += m->mix * (desired_vect[i] - inv[i]); + float v = m->mix * (desired_vect[i] - inv[i]); + + if (i == MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1) { + /* This predictive stuff is noisy and can apparently cause some feedback in the low throttle + region, that makes throttle eventually oscillate around the zero point under certain conditions. + This leads to funny business with the motors, e.g. grinding. */ + float t = desired_vect[i]; + if ((t >= THROTTLE_EPSILON && (t+v) < THROTTLE_EPSILON) || + (t <= -THROTTLE_EPSILON && (t+v) > -THROTTLE_EPSILON)) { + v = 0; + } + /* Also bound throttle. */ + desired_vect[i] = bound_sym(desired_vect[i] + v, 1.0f); + } else { + desired_vect[i] += v; + } + } } From 778fce602550c2945cecb2632331718c68fcf62f Mon Sep 17 00:00:00 2001 From: Mario Goebbels Date: Tue, 17 Apr 2018 20:24:42 +0200 Subject: [PATCH 4/5] sparky: Sacrifice LightTelemetry to avoid flash overflow. --- flight/targets/sparky/fw/pios_config.h | 1 - 1 file changed, 1 deletion(-) diff --git a/flight/targets/sparky/fw/pios_config.h b/flight/targets/sparky/fw/pios_config.h index c351b5504f..76fc433f29 100644 --- a/flight/targets/sparky/fw/pios_config.h +++ b/flight/targets/sparky/fw/pios_config.h @@ -51,7 +51,6 @@ /* Com systems to include */ #define PIOS_INCLUDE_MAVLINK -#define PIOS_INCLUDE_LIGHTTELEMETRY /* Supported receiver interfaces */ From 40f37f3270b6664727c6a7cd0119ca259f167572 Mon Sep 17 00:00:00 2001 From: Mario Goebbels Date: Thu, 19 Apr 2018 23:56:51 +0200 Subject: [PATCH 5/5] actuator: Avoid zero-crossing throttle. Part Trois. --- flight/Modules/Actuator/actuator.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 3f4215800a..828f2cf3c6 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -1216,13 +1216,14 @@ static void smithp_compensate(struct smith_predictor *m, float *desired_vect) /* This predictive stuff is noisy and can apparently cause some feedback in the low throttle region, that makes throttle eventually oscillate around the zero point under certain conditions. This leads to funny business with the motors, e.g. grinding. */ - float t = desired_vect[i]; - if ((t >= THROTTLE_EPSILON && (t+v) < THROTTLE_EPSILON) || - (t <= -THROTTLE_EPSILON && (t+v) > -THROTTLE_EPSILON)) { - v = 0; - } - /* Also bound throttle. */ - desired_vect[i] = bound_sym(desired_vect[i] + v, 1.0f); + float r = desired_vect[i]+v; + float s = sign(desired_vect[i]); + + /* Don't cross zero via prediction. Also bound throttle. */ + if (sign(r) != s) + desired_vect[i] = s*THROTTLE_EPSILON; + else + desired_vect[i] = bound_sym(r, 1.0f); } else { desired_vect[i] += v; }