static int load(const char *devname, const char *fname) { // sleep a while to ensure device has been set up usleep(20000); int dev; char buf[2048]; /* open the device */ if ((dev = px4_open(devname, 0)) < 0) { warnx("can't open %s\n", devname); return 1; } /* reset mixers on the device */ if (px4_ioctl(dev, MIXERIOCRESET, 0)) { warnx("can't reset mixers on %s", devname); return 1; } if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) { warnx("can't load mixer: %s", fname); return 1; } /* XXX pass the buffer to the device */ int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); if (ret < 0) { warnx("error loading mixers from %s", fname); return 1; } return 0; }
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; }
static void load(const char *devname, const char *fname) { int dev; char buf[2048]; /* open the device */ if ((dev = open(devname, 0)) < 0) err(1, "can't open %s\n", devname); /* reset mixers on the device */ if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) err(1, "can't load mixer: %s", fname); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); if (ret < 0) err(1, "error loading mixers from %s", fname); exit(0); }
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"); }
/* setup mixer on PX4 so that if FMU dies the pilot gets manual control */ bool Plane::setup_failsafe_mixing(void) { // we create MIXER.MIX regardless of whether we will be using it, // as it gives a template for the user to modify to create their // own CUSTOM.MIX file const char *mixer_filename = "/fs/microsd/APM/MIXER.MIX"; const char *custom_mixer_filename = "/fs/microsd/APM/CUSTOM.MIX"; bool ret = false; char *buf = NULL; const uint16_t buf_size = 2048; if (!create_mixer_file(mixer_filename)) { return false; } struct stat st; const char *filename; if (stat(custom_mixer_filename, &st) == 0) { filename = custom_mixer_filename; } else { filename = mixer_filename; } enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state(); struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8}; int px4io_fd = open("/dev/px4io", 0); if (px4io_fd == -1) { // px4io isn't started, no point in setting up a mixer return false; } buf = (char *)malloc(buf_size); if (buf == NULL) { goto failed; } if (load_mixer_file(filename, &buf[0], buf_size) != 0) { hal.console->printf("Unable to load %s\n", filename); goto failed; } if (old_state == AP_HAL::Util::SAFETY_ARMED) { // make sure the throttle has a non-zero failsafe value before we // disable safety. This prevents sending zero PWM during switch over hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->radio_min); } // we need to force safety on to allow us to load a mixer hal.rcout->force_safety_on(); /* reset any existing mixer in px4io. This shouldn't be needed, * but is good practice */ if (ioctl(px4io_fd, MIXERIOCRESET, 0) != 0) { hal.console->printf("Unable to reset mixer\n"); goto failed; } /* pass the buffer to the device */ if (ioctl(px4io_fd, MIXERIOCLOADBUF, (unsigned long)buf) != 0) { hal.console->printf("Unable to send mixer to IO\n"); goto failed; } // setup RC config for each channel based on user specified // mix/max/trim. We only do the first 8 channels due to // a RC config limitation in px4io.c limiting to PX4IO_RC_MAPPED_CONTROL_CHANNELS for (uint8_t i=0; i<8; i++) { RC_Channel *ch = RC_Channel::rc_channel(i); if (ch == NULL) { continue; } struct pwm_output_rc_config config; /* we use a min/max of 900/2100 to allow for pass-thru of larger values than the RC min/max range. This mimics the APM behaviour of pass-thru in manual, which allows for dual-rate transmitter setups in manual mode to go beyond the ranges used in stabilised modes */ config.channel = i; config.rc_min = 900; config.rc_max = 2100; if (rcmap.throttle()-1 == i) { // throttle uses a trim of 1500, so we don't get division // by small numbers near RC3_MIN config.rc_trim = 1500; } else { config.rc_trim = constrain_int16(ch->radio_trim, config.rc_min, config.rc_max); } config.rc_dz = 0; // zero for the purposes of manual takeover config.rc_assignment = i; // we set reverse as false, as users of ArduPilot will have // input reversed on transmitter, so from the point of view of // the mixer the input is never reversed. The one exception is // the 2nd channel, which is reversed inside the PX4IO code, // so needs to be unreversed here to give sane behaviour. if (i == 1) { config.rc_reverse = true; } else { config.rc_reverse = false; } if (ioctl(px4io_fd, PWM_SERVO_SET_RC_CONFIG, (unsigned long)&config) != 0) { hal.console->printf("SET_RC_CONFIG failed\n"); goto failed; } } for (uint8_t i = 0; i < pwm_values.channel_count; i++) { pwm_values.values[i] = 900; } ioctl(px4io_fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); for (uint8_t i = 0; i < pwm_values.channel_count; i++) { pwm_values.values[i] = 2100; } ioctl(px4io_fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_OK, 0); // setup for immediate manual control if FMU dies ioctl(px4io_fd, PWM_SERVO_SET_OVERRIDE_IMMEDIATE, 1); ret = true; failed: if (buf != NULL) { free(buf); } if (px4io_fd != -1) { close(px4io_fd); } // restore safety state if it was previously armed if (old_state == AP_HAL::Util::SAFETY_ARMED) { hal.rcout->force_safety_off(); } return ret; }
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; }