Plane: Fetch mixer status for assessing mixer success

This commit is contained in:
Michael du Breuil 2016-09-04 18:38:42 -07:00 committed by Andrew Tridgell
parent f2997901bd
commit d0da7411b2
1 changed files with 7 additions and 0 deletions

View File

@ -239,6 +239,7 @@ bool Plane::setup_failsafe_mixing(void)
int px4io_fd = -1;
enum AP_HAL::Util::safety_state old_state = hal.util->safety_switch_state();
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 8};
unsigned mixer_status = 0;
buf = (char *)malloc(buf_size);
if (buf == NULL) {
@ -390,6 +391,12 @@ bool Plane::setup_failsafe_mixing(void)
goto failed;
}
if (ioctl(px4io_fd, PWM_IO_GET_STATUS, (unsigned long)&mixer_status) != 0 ||
(mixer_status & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
hal.console->printf("Mixer failed: 0x%04x\n", mixer_status);
goto failed;
}
ret = true;
failed: