Browse Source

Don't try to mix if we have no mixer installed.

sbg
px4dev 13 years ago
parent
commit
42ace38e31
  1. 20
      apps/px4/fmu/fmu.cpp

20
apps/px4/fmu/fmu.cpp

@ -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,17 +239,21 @@ 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);
/* do mixing */ /* can we mix? */
_mixers->mix(&outputs[0], num_outputs); if (_mixers != nullptr) {
/* iterate actuators */ /* do mixing */
for (unsigned i = 0; i < num_outputs; i++) { _mixers->mix(&outputs[0], num_outputs);
/* scale for PWM output 900 - 2100us */ /* iterate actuators */
up_pwm_servo_set(i, 1500 + (600 * outputs[i])); for (unsigned i = 0; i < num_outputs; i++) {
/* scale for PWM output 900 - 2100us */
up_pwm_servo_set(i, 1500 + (600 * outputs[i]));
}
} }
} }

Loading…
Cancel
Save