int test_mixer(int argc, char *argv[])
{
warnx("testing mixer");
char *filename = "/etc/mixers/IO_pass.mix";
if (argc > 1)
filename = argv[1];
warnx("loading: %s", filename);
char buf[2048];
load_mixer_file(filename, &buf[0], sizeof(buf));
unsigned loaded = strlen(buf);
warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
/* load the mixer in chunks, like
* in the case of a remote load,
* e.g. on PX4IO.
*/
unsigned nused = 0;
const unsigned chunk_size = 64;
MixerGroup mixer_group(mixer_callback, 0);
/* load at once test */
unsigned xx = loaded;
mixer_group.load_from_buf(&buf[0], xx);
warnx("complete buffer load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 8)
return 1;
unsigned empty_load = 2;
char empty_buf[2];
empty_buf[0] = ' ';
empty_buf[1] = '\0';
mixer_group.reset();
mixer_group.load_from_buf(&empty_buf[0], empty_load);
warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
if (empty_load != 0)
return 1;
/* FIRST mark the mixer as invalid */
bool mixer_ok = false;
/* THEN actually delete it */
mixer_group.reset();
char mixer_text[256]; /* large enough for one mixer */
unsigned mixer_text_length = 0;
unsigned transmitted = 0;
warnx("transmitted: %d, loaded: %d", transmitted, loaded);
while (transmitted < loaded) {
unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted;
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
bool mixer_ok = false;
return 1;
}
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
/* only set mixer ok if no residual is left over */
if (resid == 0) {
mixer_ok = true;
} else {
/* not yet reached the end of the mixer, set as not ok */
mixer_ok = false;
}
warnx("used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0)
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
mixer_text_length = resid;
}
transmitted += text_length;
}
warnx("chunked load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 8)
return 1;
/* execute the mixer */
float outputs[output_max];
unsigned mixed;
const int jmax = 5;
pwm_limit_init(&pwm_limit);
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] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
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/10.0f + 0.1f * i;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
/* 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)", mixed, output_max);
for (int i = 0; i < mixed; 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;
}
}
}
warnx("ARMING TEST: DISARMING");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = false;
while (hrt_elapsed_time(&starttime) < 600000) {
/* 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 (r_page_servos[i] != r_page_servo_disarmed[i]) {
warnx("disarmed 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: REARMING: STARTING RAMP");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = true;
while (hrt_elapsed_time(&starttime) < 600000 + 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++)
{
/* predict value */
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
/* check ramp */
if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
r_page_servos[i] > servo_predicted[i])) {
warnx("ramp servo value mismatch");
return 1;
}
/* check post ramp phase */
if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
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;
}
//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");
/* load multirotor at once test */
mixer_group.reset();
if (argc > 2)
filename = argv[2];
else
filename = "/etc/mixers/FMU_quad_w.mix";
load_mixer_file(filename, &buf[0], sizeof(buf));
loaded = strlen(buf);
warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
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() != 5) {
warnx("FAIL: Quad W mixer load failed");
return 1;
}
warnx("SUCCESS: No errors in mixer test");
}
int test_mixer(int argc, char *argv[])
{
/*
* PWM limit structure
*/
pwm_limit_t pwm_limit;
bool should_arm = false;
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];
int16_t reverse_pwm_mask = 0;
//PX4_INFO("testing mixer");
#if !defined(CONFIG_ARCH_BOARD_SITL)
const char *filename = "/etc/mixers/IO_pass.mix";
#else
const char *filename = "ROMFS/px4fmu_test/mixers/IO_pass.mix";
#endif
//PX4_INFO("loading: %s", filename);
char buf[2048];
load_mixer_file(filename, &buf[0], sizeof(buf));
unsigned loaded = strlen(buf);
//fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
/* load the mixer in chunks, like
* in the case of a remote load,
* e.g. on PX4IO.
*/
const unsigned chunk_size = 64;
MixerGroup mixer_group(mixer_callback, 0);
/* load at once test */
unsigned xx = loaded;
mixer_group.load_from_buf(&buf[0], xx);
//ASSERT_EQ(mixer_group.count(), 8);
unsigned empty_load = 2;
char empty_buf[2];
empty_buf[0] = ' ';
empty_buf[1] = '\0';
mixer_group.reset();
mixer_group.load_from_buf(&empty_buf[0], empty_load);
//PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
//ASSERT_NE(empty_load, 0);
if (empty_load != 0) {
return 1;
}
/* FIRST mark the mixer as invalid */
/* THEN actually delete it */
mixer_group.reset();
char mixer_text[256]; /* large enough for one mixer */
unsigned mixer_text_length = 0;
unsigned transmitted = 0;
//PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
while (transmitted < loaded) {
unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted;
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
return 1;
}
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
//fprintf(stderr, "buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
//fprintf(stderr, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0) {
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
}
mixer_text_length = resid;
}
transmitted += text_length;
}
//PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 8) {
return 1;
}
/* execute the mixer */
float outputs[output_max];
unsigned mixed;
const int jmax = 5;
pwm_limit_init(&pwm_limit);
/* run through arming phase */
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
r_page_servo_disarmed[i] = PWM_MOTOR_OFF;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
//PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
/* mix */
should_prearm = true;
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, 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 (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (i != actuator_controls_s::INDEX_THROTTLE) {
if (r_page_servos[i] < r_page_servo_control_min[i]) {
warnx("active servo < min");
return 1;
}
} else {
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
return 1;
}
}
}
should_arm = true;
should_prearm = false;
/* simulate another orb_copy() from actuator controls */
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
}
//PX4_INFO("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 + 2 * sleep_quantum_us) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, 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 (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[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]) {
PX4_ERR("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]);
return 1;
}
if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
PX4_ERR("ramp servo value mismatch");
return 1;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
fflush(stdout);
}
}
//PX4_INFO("ARMING TEST: NORMAL OPERATION");
for (int j = -jmax; j <= jmax; j++) {
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = j / 10.0f + 0.1f * i;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
if (abs(servo_predicted[i] - r_page_servos[i]) > MIXER_DIFFERENCE_THRESHOLD) {
fprintf(stderr, "\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i],
(int)r_page_servos[i]);
PX4_ERR("mixer violated predicted value");
return 1;
}
}
}
//PX4_INFO("ARMING TEST: DISARMING");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = false;
while (hrt_elapsed_time(&starttime) < 600000) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, 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 (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
PX4_ERR("disarmed servo value mismatch");
return 1;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
//printf(".");
//fflush(stdout);
}
}
//printf("\n");
//PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = true;
while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, 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 (unsigned i = 0; i < mixed; i++) {
/* predict value */
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
/* check ramp */
//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
r_page_servos[i] > servo_predicted[i])) {
PX4_ERR("ramp servo value mismatch");
return 1;
}
/* check post ramp phase */
if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
abs(servo_predicted[i] - r_page_servos[i]) > 2) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
PX4_ERR("mixer violated predicted value");
return 1;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
// printf(".");
// fflush(stdout);
}
}
//printf("\n");
/* load multirotor at once test */
mixer_group.reset();
#if !defined(CONFIG_ARCH_BOARD_SITL)
filename = "/etc/mixers/quad_test.mix";
#else
filename = "ROMFS/px4fmu_test/mixers/quad_test.mix";
#endif
load_mixer_file(filename, &buf[0], sizeof(buf));
loaded = strlen(buf);
//fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
unsigned mc_loaded = loaded;
mixer_group.load_from_buf(&buf[0], mc_loaded);
//PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 5) {
PX4_ERR("FAIL: Quad test mixer load failed");
return 1;
}
//PX4_INFO("SUCCESS: No errors in mixer test");
return 0;
}
