diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index d330da39f3..460e6f5e7d 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -48,10 +48,12 @@ #include #include #include +#include #include #include #include +#include #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