aboutsummaryrefslogtreecommitdiff
path: root/src/systemcmds/tests/test_mixer.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/systemcmds/tests/test_mixer.cpp')
-rw-r--r--src/systemcmds/tests/test_mixer.cpp82
1 files changed, 51 insertions, 31 deletions
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index 2896a8e40..64e734b00 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -83,8 +83,9 @@ int test_mixer(int argc, char *argv[])
const char *filename = "/etc/mixers/IO_pass.mix";
- if (argc > 1)
+ if (argc > 1) {
filename = argv[1];
+ }
warnx("loading: %s", filename);
@@ -108,8 +109,10 @@ int test_mixer(int argc, char *argv[])
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)
+
+ if (mixer_group.count() != 8) {
return 1;
+ }
unsigned empty_load = 2;
char empty_buf[2];
@@ -118,8 +121,10 @@ int test_mixer(int argc, char *argv[])
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)
+
+ if (empty_load != 0) {
return 1;
+ }
/* FIRST mark the mixer as invalid */
/* THEN actually delete it */
@@ -155,8 +160,9 @@ int test_mixer(int argc, char *argv[])
warnx("used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
- if (resid > 0)
+ if (resid > 0) {
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+ }
mixer_text_length = resid;
}
@@ -166,11 +172,12 @@ int test_mixer(int argc, char *argv[])
warnx("chunked load: loaded %u mixers", mixer_group.count());
- if (mixer_group.count() != 8)
+ if (mixer_group.count() != 8) {
return 1;
+ }
/* execute the mixer */
-
+
float outputs[output_max];
unsigned mixed;
const int jmax = 5;
@@ -193,30 +200,31 @@ int test_mixer(int argc, char *argv[])
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);
+ 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 (unsigned i = 0; i < mixed; i++)
- {
+ for (unsigned 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]) {
+ 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]) {
+ 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, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]);
}
+
usleep(sleep_quantum_us);
sleepcount++;
@@ -225,6 +233,7 @@ int test_mixer(int argc, char *argv[])
fflush(stdout);
}
}
+
printf("\n");
warnx("ARMING TEST: NORMAL OPERATION");
@@ -232,7 +241,7 @@ int test_mixer(int argc, char *argv[])
for (int j = -jmax; j <= jmax; j++) {
for (unsigned i = 0; i < output_max; i++) {
- actuator_controls[i] = j/10.0f + 0.1f * 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;
@@ -241,12 +250,14 @@ int test_mixer(int argc, char *argv[])
/* 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);
+ 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 (unsigned i = 0; i < mixed; i++)
- {
+
+ 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 (fabsf(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]);
warnx("mixer violated predicted value");
@@ -262,15 +273,15 @@ int test_mixer(int argc, char *argv[])
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);
+ 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 (unsigned i = 0; i < mixed; i++)
- {
+ for (unsigned 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");
@@ -279,6 +290,7 @@ int test_mixer(int argc, char *argv[])
//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++;
@@ -287,6 +299,7 @@ int test_mixer(int argc, char *argv[])
fflush(stdout);
}
}
+
printf("\n");
warnx("ARMING TEST: REARMING: STARTING RAMP");
@@ -296,30 +309,30 @@ int test_mixer(int argc, char *argv[])
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);
+ 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 (unsigned i = 0; i < mixed; i++)
- {
+ 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 */
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])) {
+ (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) {
+ fabsf(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]);
warnx("mixer violated predicted value");
return 1;
@@ -327,6 +340,7 @@ int test_mixer(int argc, char *argv[])
//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++;
@@ -335,15 +349,18 @@ int test_mixer(int argc, char *argv[])
fflush(stdout);
}
}
+
printf("\n");
/* load multirotor at once test */
mixer_group.reset();
- if (argc > 2)
+ if (argc > 2) {
filename = argv[2];
- else
+
+ } else {
filename = "/etc/mixers/quad_w.main.mix";
+ }
load_mixer_file(filename, &buf[0], sizeof(buf));
loaded = strlen(buf);
@@ -353,6 +370,7 @@ 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() != 5) {
warnx("FAIL: Quad W mixer load failed");
return 1;
@@ -368,11 +386,13 @@ mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control)
{
- if (control_group != 0)
+ if (control_group != 0) {
return -1;
+ }
- if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0])))
+ if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0]))) {
return -1;
+ }
control = actuator_controls[control_index];
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-13 20:28:20 +0000