diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index acde4a1a50..20b77f1e27 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -56,6 +56,8 @@ #include #include +#include + #include "tests.h" static int mixer_callback(uintptr_t handle, @@ -65,6 +67,9 @@ static int mixer_callback(uintptr_t handle, const unsigned output_max = 8; static float actuator_controls[output_max]; +static bool should_prearm = false; + +#define NAN_VALUE 0.0f/0.0f int test_mixer(int argc, char *argv[]) { @@ -72,7 +77,7 @@ int test_mixer(int argc, char *argv[]) * PWM limit structure */ pwm_limit_t pwm_limit; - static bool should_arm = false; + 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]; @@ -184,7 +189,6 @@ int test_mixer(int argc, char *argv[]) const int jmax = 5; pwm_limit_init(&pwm_limit); - should_arm = true; /* run through arming phase */ for (unsigned i = 0; i < output_max; i++) { @@ -194,6 +198,35 @@ int test_mixer(int argc, char *argv[]) r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } + warnx("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++) { + + warnx("pre-arm:\t %d: out: %8.4f, servo: %d", 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; + warnx("ARMING TEST: STARTING RAMP"); unsigned sleep_quantum_us = 10000; @@ -205,11 +238,14 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + 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++) { + + warnx("ramp:\t %d: out: %8.4f, servo: %d", 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]) { @@ -222,8 +258,6 @@ int test_mixer(int argc, char *argv[]) warnx("ramp servo value mismatch"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -251,7 +285,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + 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)", mixed, output_max); @@ -278,18 +312,19 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + 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++) { + + warnx("disarmed:\t %d: out: %8.4f, servo: %d", 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]) { 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); @@ -314,7 +349,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + 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); @@ -324,6 +359,8 @@ int test_mixer(int argc, char *argv[]) /* check ramp */ + warnx("ramp:\t %d: out: %8.4f, servo: %d", 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])) { @@ -338,8 +375,6 @@ int test_mixer(int argc, char *argv[]) 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); @@ -397,5 +432,10 @@ mixer_callback(uintptr_t handle, control = actuator_controls[control_index]; + if (should_prearm && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + control = NAN_VALUE; + } + return 0; }