|
|
@ -226,7 +226,7 @@ FMUServo::task_main() |
|
|
|
while (!_task_should_exit) { |
|
|
|
while (!_task_should_exit) { |
|
|
|
|
|
|
|
|
|
|
|
/* sleep waiting for data, but no more than 100ms */ |
|
|
|
/* sleep waiting for data, but no more than 100ms */ |
|
|
|
int ret = ::poll(&fds[0], 2, 100); |
|
|
|
int ret = ::poll(&fds[0], 2, 1000); |
|
|
|
|
|
|
|
|
|
|
|
/* this would be bad... */ |
|
|
|
/* this would be bad... */ |
|
|
|
if (ret < 0) { |
|
|
|
if (ret < 0) { |
|
|
@ -239,9 +239,12 @@ FMUServo::task_main() |
|
|
|
if (fds[0].revents & POLLIN) { |
|
|
|
if (fds[0].revents & POLLIN) { |
|
|
|
float outputs[num_outputs]; |
|
|
|
float outputs[num_outputs]; |
|
|
|
|
|
|
|
|
|
|
|
/* get controls */ |
|
|
|
/* get controls - must always do this to avoid spinning */ |
|
|
|
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); |
|
|
|
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* can we mix? */ |
|
|
|
|
|
|
|
if (_mixers != nullptr) { |
|
|
|
|
|
|
|
|
|
|
|
/* do mixing */ |
|
|
|
/* do mixing */ |
|
|
|
_mixers->mix(&outputs[0], num_outputs); |
|
|
|
_mixers->mix(&outputs[0], num_outputs); |
|
|
|
|
|
|
|
|
|
|
@ -252,6 +255,7 @@ FMUServo::task_main() |
|
|
|
up_pwm_servo_set(i, 1500 + (600 * outputs[i])); |
|
|
|
up_pwm_servo_set(i, 1500 + (600 * outputs[i])); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/* how about an arming update? */ |
|
|
|
/* how about an arming update? */ |
|
|
|
if (fds[1].revents & POLLIN) { |
|
|
|
if (fds[1].revents & POLLIN) { |
|
|
|