dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Lorenz Meier 2014-01-02 09:18:36 +01:00
parent a60fcc2535
commit 9a9a6f3d86
1 changed files with 70 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
src/systemcmds/tests/test_mixer.cpp
View File

@ -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