diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c
index e0fd7ab517..d459f573e5 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
@@ -97,6 +98,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;
@@ -699,6 +723,14 @@ static void actuator_settings_update()
desired_3d_mask |= (1 << i);
}
}
+
+ 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);
+ }
+ }
}
/**
@@ -754,17 +786,13 @@ static void actuator_task(void* parameters)
SystemSettingsAirframeTypeGet(&airframe_type);
compute_mixer();
-<<<<<<< HEAD
-=======
/* If we can't calculate a proper inverse mixer,
* set failsafe.
*/
if (compute_inverse_mixer()) {
- set_failsafe();
- continue;
+ inverse_mixer_computed = true;
}
->>>>>>> mlyle/mpl-actmodel
MixerSettingsThrottleCurve2Get(curve2);
MixerSettingsCurve2SourceGet(&curve2_src);
@@ -838,6 +866,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,
@@ -870,6 +900,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);
}
@@ -1076,6 +1108,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 068ce4e846..35a71cf997 100644
--- a/shared/uavobjectdefinition/actuatorsettings.xml
+++ b/shared/uavobjectdefinition/actuatorsettings.xml
@@ -33,5 +33,14 @@
Actuator mapping of input in [-1,1] to output on [-1,1], using power equation of type x^value. This is intended to correct for the non-linear relationship between input command and output power inherent in brushless ESC/motor combinations. A setting below 1.0 will improve high-throttle control stability.
+
+ 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.
+