Turned the mixer test into a real test, now also cross checking post mix results

1 files changed, 70 insertions, 10 deletions

diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index d330da39f..460e6f5e7 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -48,10 +48,12 @@
 #include <errno.h>
 #include <string.h>
 #include <time.h>
+#include <math.h>
 
 #include <systemlib/err.h>
 #include <systemlib/mixer/mixer.h>
 #include <systemlib/pwm_limit/pwm_limit.h>
+#include <drivers/drv_hrt.h>
 
 #include "tests.h"
 
@@ -67,6 +69,8 @@ uint16_t r_page_servo_disarmed[output_max];
 uint16_t r_page_servo_control_min[output_max];
 uint16_t r_page_servo_control_max[output_max];
 uint16_t r_page_servos[output_max];
+uint16_t servo_predicted[output_max];
+
 /*
  * PWM limit structure
  */
@@ -179,35 +183,87 @@ int test_mixer(int argc, char *argv[])
 
 	/* execute the mixer */
 	
-	float	outputs_unlimited[output_max];
 	float	outputs[output_max];
 	unsigned mixed;
-	const int jmax = 50;
+	const int jmax = 5;
 
 	pwm_limit_init(&pwm_limit);
-	pwm_limit.state = PWM_LIMIT_STATE_ON;
 	should_arm = true;
 
+	/* run through arming phase */
+	for (int i = 0; i < output_max; i++) {
+		actuator_controls[i] = 0.1f;
+		r_page_servo_disarmed[i] = 900;
+		r_page_servo_control_min[i] = 1000;
+		r_page_servo_control_max[i] = 2000;
+	}
+
+	warnx("ARMING TEST: STARTING RAMP");
+	unsigned sleep_quantum_us = 10000;
+
+	hrt_abstime starttime = hrt_absolute_time();
+	unsigned sleepcount = 0;
+
+	while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) {
+		
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], output_max);
+
+		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
+
+		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
+		for (int i = 0; i < mixed; i++)
+		{
+			/* check mixed outputs to be zero during init phase */
+			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
+				r_page_servos[i] != r_page_servo_disarmed[i]) {
+				warnx("disarmed servo value mismatch");
+				return 1;
+			}
+
+			if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
+				r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
+				warnx("ramp servo value mismatch");
+				return 1;
+			}
+
+			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+		}
+		usleep(sleep_quantum_us);
+		sleepcount++;
+
+		if (sleepcount % 10 == 0) {
+			printf(".");
+			fflush(stdout);
+		}
+	}
+	printf("\n");
+
+	warnx("ARMING TEST: NORMAL OPERATION");
+
 	for (int j = -jmax; j <= jmax; j++) {
 
 		for (int i = 0; i < output_max; i++) {
-			actuator_controls[i] = j/100.0f + 0.1f * i;
+			actuator_controls[i] = j/10.0f + 0.1f * i;
 			r_page_servo_disarmed[i] = 900;
 			r_page_servo_control_min[i] = 1000;
 			r_page_servo_control_max[i] = 2000;
 		}
 
 		/* mix */
-		mixed = mixer_group.mix(&outputs_unlimited[0], output_max);
-
-		memcpy(outputs, outputs_unlimited, sizeof(outputs));
+		mixed = mixer_group.mix(&outputs[0], output_max);
 
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
-		warnx("mixed %d outputs (max %d), values:", mixed, output_max);
+		warnx("mixed %d outputs (max %d)", mixed, output_max);
 		for (int i = 0; i < mixed; i++)
 		{
-			printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
+			servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
+			if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
+				printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
+				warnx("mixer violated predicted value");
+				return 1;
+			}
 		}
 	}
 
@@ -227,8 +283,12 @@ int test_mixer(int argc, char *argv[])
 	unsigned mc_loaded = loaded;
 	mixer_group.load_from_buf(&buf[0], mc_loaded);
 	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
-	if (mixer_group.count() != 8)
+	if (mixer_group.count() != 5) {
+		warnx("FAIL: Quad W mixer load failed");
 		return 1;
+	}
+
+	warnx("SUCCESS: No errors in mixer test");
 }
 
 static int