actuator: Implement Smith predictor for actuator delay compensation.

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]);
+	}
+}
+
 /**
  * @}
  * @}

shared/uavobjectdefinition/actuatorsettings.xml

@@ -39,5 +39,14 @@
     <field defaultvalue="1.0" elements="1" limits="%BE:0.50:1.0" name="MotorInputOutputGain" type="float" units="">
       <description>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.</description>
     </field>
+    <field name="SmithPredictorDelay" elements="1" defaultvalue="3.2" type="float" units="ms">
+      <description>By how much milliseconds to delay the actuator commands fed into the predictor.</description>
+    </field>
+    <field name="SmithPredictorIIR" elements="1" defaultvalue="0.5" type="float" units="">
+      <description>IIR coefficient for smoothing.</description>
+    </field>
+    <field name="SmithPredictorMix" elements="1" defaultvalue="0.5" type="float" units="">
+      <description>Amount of contribution by the smith predictor to the control signal.</description>
+    </field>
   </object>
 </xml>