bool MixerTest::load_mixer(const char *filename, unsigned expected_count, bool verbose)
{
char buf[2048];
load_mixer_file(filename, &buf[0], sizeof(buf));
unsigned loaded = strlen(buf);
if (verbose) {
PX4_INFO("loaded: \n\"%s\"\n (file: %s, %d chars)", &buf[0], filename, loaded);
}
// Test a number of chunk sizes
for (unsigned chunk_size = 6; chunk_size < PX4IO_MAX_TRANSFER_LEN + 1; chunk_size++) {
bool ret = load_mixer(filename, buf, loaded, expected_count, chunk_size, verbose);
if (!ret) {
PX4_ERR("Mixer load failed with chunk size %u", chunk_size);
return ret;
}
}
return true;
}
bool MixerTest::mixerTest()
{
/*
* 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;
bool load_ok = load_mixer(MIXER_PATH(IO_pass.mix), 8);
if (!load_ok) {
return load_ok;
}
/* 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, nullptr);
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 false;
}
} else {
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
return false;
}
}
}
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, nullptr);
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 false;
}
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 false;
}
}
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, nullptr);
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 false;
}
}
}
//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, nullptr);
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 false;
}
}
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, nullptr);
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 false;
}
/* 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 false;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
// printf(".");
// fflush(stdout);
}
}
return true;
}
bool MixerTest::loadComplexTest()
{
return load_mixer(MIXER_PATH(complex_test.mix), 8);
}
bool MixerTest::loadAllTest()
{
PX4_INFO("Testing all mixers in %s", MIXER_ONBOARD_PATH);
DIR *dp = opendir(MIXER_ONBOARD_PATH);
if (dp == nullptr) {
PX4_ERR("File open failed");
return false;
}
struct dirent *result = nullptr;
for (;;) {
errno = 0;
result = readdir(dp);
// read the directory entry
if (result == nullptr) {
if (errno) {
PX4_ERR("readdir failed");
closedir(dp);
return false;
}
// We are just at the last directory entry
break;
}
// Determine the directory entry type
switch (result->d_type) {
#ifdef __PX4_NUTTX
case DTYPE_FILE:
#else
case DT_REG:
#endif
if (strncmp(result->d_name, ".", 1) != 0) {
char buf[PATH_MAX];
(void)strncpy(&buf[0], MIXER_ONBOARD_PATH, sizeof(buf) - 1);
/* enforce null termination */
buf[sizeof(buf) - 1] = '\0';
(void)strncpy(&buf[strlen(MIXER_ONBOARD_PATH)], "/", 1);
(void)strncpy(&buf[strlen(MIXER_ONBOARD_PATH) + 1], result->d_name, sizeof(buf) - strlen(MIXER_ONBOARD_PATH) - 1);
bool ret = load_mixer(buf, 0);
if (!ret) {
PX4_ERR("Error testing mixer %s", buf);
return false;
}
}
break;
default:
break;
}
}
closedir(dp);
return true;
}
bool MixerTest::loadVTOL2Test()
{
return load_mixer(MIXER_PATH(vtol2_test.mix), 6);
}
bool MixerTest::loadVTOL1Test()
{
return load_mixer(MIXER_PATH(vtol1_test.mix), 4);
}
bool MixerTest::loadQuadTest()
{
return load_mixer(MIXER_PATH(quad_test.mix), 5);
}
bool MixerTest::loadIOPass()
{
return load_mixer(MIXER_PATH(IO_pass.mix), 8);
}
bool MixerTest::loadAllTest()
{
PX4_INFO("Testing all mixers in %s", MIXER_ONBOARD_PATH);
DIR *dp = opendir(MIXER_ONBOARD_PATH);
if (dp == nullptr) {
PX4_ERR("File open failed");
return false;
}
struct dirent *result = nullptr;
for (;;) {
errno = 0;
result = readdir(dp);
// read the directory entry
if (result == nullptr) {
if (errno) {
PX4_ERR("readdir failed");
closedir(dp);
return false;
}
// We are just at the last directory entry
break;
}
// Determine the directory entry type
switch (result->d_type) {
#ifdef __PX4_NUTTX
case DTYPE_FILE:
#else
case DT_REG:
#endif
if (strncmp(result->d_name, ".", 1) != 0) {
char buf[PATH_MAX];
if (snprintf(buf, PATH_MAX, "%s/%s", MIXER_ONBOARD_PATH, result->d_name) >= PATH_MAX) {
PX4_ERR("mixer path too long %s", result->d_name);
closedir(dp);
return false;
}
bool ret = load_mixer(buf, 0);
if (!ret) {
PX4_ERR("Error testing mixer %s", buf);
closedir(dp);
return false;
}
}
break;
default:
break;
}
}
closedir(dp);
return true;
}
