diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c
index 60986298ba..828f2cf3c6 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
@@ -75,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
@@ -97,6 +99,30 @@ 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;
@@ -410,6 +436,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,
@@ -733,6 +765,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);
+ }
+ }
}
/**
@@ -782,6 +822,13 @@ static void actuator_task(void* parameters)
compute_mixer();
+ /* If we can't calculate a proper inverse mixer,
+ * set failsafe.
+ */
+ if (compute_inverse_mixer()) {
+ inverse_mixer_computed = true;
+ }
+
MixerSettingsThrottleCurve2Get(curve2);
MixerSettingsCurve2SourceGet(&curve2_src);
settings_updated = false;
@@ -859,6 +906,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,
@@ -891,6 +940,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);
}
@@ -1096,6 +1147,90 @@ 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++) {
+ 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 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;
+ }
+
+ }
+}
+
/**
* @}
* @}
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 */
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.
+