diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-22 14:28:04 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-22 14:28:04 +0100 |
commit | 977eca7b473f9e109e57ef8ca4194ebf28d9d49a (patch) | |
tree | 6b6ee28d200b79f111c0640295a297956e869788 /src/modules/px4iofirmware | |
parent | f43ab65e136a1c230d89fa5403b65e3a472b5f44 (diff) | |
download | px4-firmware-977eca7b473f9e109e57ef8ca4194ebf28d9d49a.tar.gz px4-firmware-977eca7b473f9e109e57ef8ca4194ebf28d9d49a.tar.bz2 px4-firmware-977eca7b473f9e109e57ef8ca4194ebf28d9d49a.zip |
Hook up S.BUS1 and S.BUS2 output in mixer, routines do not send to serial port yet.
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 29 |
1 files changed, 29 insertions, 0 deletions
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f39fcf7ec..9ef11aa8e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -187,15 +187,23 @@ mixer_tick(void) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + unsigned actuator_count = 0; + /* * Run the mixers. */ if (source == MIX_FAILSAFE) { + actuator_count = 0; + /* copy failsafe values to the servo outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; + /* get the count of the last valid actuator */ + if ((r_page_servos[i] != 0) && ((i + 1) > actuator_count)) + actuator_count = i + 1; + /* safe actuators for FMU feedback */ r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } @@ -211,6 +219,7 @@ mixer_tick(void) /* poor mans mutex */ in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + actuator_count = mixed; in_mixer = false; 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); @@ -254,10 +263,30 @@ mixer_tick(void) for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + /* update S.BUS1 outputs */ + if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(r_page_servos, actuator_count); + } + + /* update S.BUS2 outputs */ + if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { + sbus2_output(r_page_servos, actuator_count); + } + } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servo_disarmed[i]); + + /* update S.BUS1 outputs */ + if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(r_page_servos, actuator_count); + } + + /* update S.BUS2 outputs */ + if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { + sbus2_output(r_page_servos, actuator_count); + } } } |