diff options
Diffstat (limited to 'apps/px4io/mixer.cpp')
-rw-r--r-- | apps/px4io/mixer.cpp | 128 |
1 files changed, 70 insertions, 58 deletions
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ed7d684b6..7ccf915b0 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -38,22 +38,13 @@ */ #include <nuttx/config.h> -#include <nuttx/arch.h> #include <sys/types.h> #include <stdbool.h> #include <string.h> -#include <assert.h> -#include <errno.h> -#include <unistd.h> -#include <fcntl.h> -#include <sched.h> - -#include <debug.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> -#include <drivers/device/device.h> #include <systemlib/mixer/mixer.h> @@ -78,10 +69,12 @@ extern "C" { bool mixer_servos_armed = false; /* selected control values and count for mixing */ -static uint16_t *control_values; -static int control_count; - -static uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; +enum mixer_source { + MIX_FMU, + MIX_OVERRIDE, + MIX_FAILSAFE +}; +static mixer_source source; static int mixer_callback(uintptr_t handle, uint8_t control_group, @@ -93,23 +86,32 @@ static MixerGroup mixer_group(mixer_callback, 0); void mixer_tick(void) { - bool should_arm; - /* check that we are receiving fresh data from the FMU */ if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + /* too many frames without FMU input, time to go to failsafe */ - system_state.mixer_manual_override = true; - system_state.mixer_fmu_available = false; - lib_lowprintf("RX timeout\n"); + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; + debug("AP RX timeout"); } /* - * Decide which set of inputs we're using. + * Decide which set of controls we're using. */ - - /* this is for planes, where manual override makes sense */ - if (system_state.manual_override_ok) { + if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE)) { + /* this is for planes, where manual override makes sense */ + source = MIX_OVERRIDE; + } else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { + source = MIX_FMU; + } else { + source = MIX_FAILSAFE; + } + +#if 0 /* if everything is ok */ + if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { /* we have recent control data from the FMU */ control_count = PX4IO_CONTROL_CHANNELS; @@ -170,43 +172,40 @@ mixer_tick(void) control_count = 0; } } - +#endif /* - * Run the mixers if we have any control data at all. + * Run the mixers. */ - if (control_count > 0) { - float outputs[IO_SERVO_COUNT]; - unsigned mixed; - - /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); - - /* scale to PWM and update the servo outputs as required */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { - if (i < mixed) { - /* scale to servo output */ - system_state.servos[i] = (outputs[i] * 500.0f) + 1500; - - } else { - /* set to zero to inhibit PWM pulse output */ - system_state.servos[i] = 0; - } + float outputs[IO_SERVO_COUNT]; + unsigned mixed; + + /* mix */ + mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + + /* scale to PWM and update the servo outputs as required */ + for (unsigned i = 0; i < mixed; i++) { + + /* save actuator values for FMU readback */ + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + + /* scale to servo output */ + r_page_servos[i] = (outputs[i] * 500.0f) + 1500; - /* - * If we are armed, update the servo output. - */ - if (system_state.armed && system_state.arm_ok) { - up_pwm_servo_set(i, system_state.servos[i]); - } - } } + for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + r_page_servos[i] = 0; + + /* + * Update the servo outputs. + */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servos[i]); /* * Decide whether the servos should be armed right now. - * A sufficient reason is armed state and either FMU or RC control inputs */ - should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); + bool should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ @@ -226,21 +225,33 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - /* if the control index refers to an input that's not valid, we can't return it */ - if (control_index >= control_count) + if (control_group != 0) return -1; - /* scale from current PWM units (1000-2000) to mixer input values */ - if (system_state.manual_override_ok && system_state.mixer_manual_override && control_index == 3) { - control = ((float)control_values[control_index] - 1000.0f) / 1000.0f; - } else { - control = ((float)control_values[control_index] - 1500.0f) / 500.0f; + switch (source) { + case MIX_FMU: + if (control_index < PX4IO_CONTROL_CHANNELS) { + control = REG_TO_FLOAT(r_page_controls[control_index]); + break; + } + return -1; + + case MIX_OVERRIDE: + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + } + return -1; + + case MIX_FAILSAFE: + /* XXX we could allow for configuration of per-output failsafe values */ + return -1; } return 0; } -static char mixer_text[256]; +static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; void @@ -267,6 +278,7 @@ mixer_handle_text(const void *buffer, size_t length) debug("append %d", length); /* check for overflow - this is really fatal */ + /* XXX could add just what will fit & try to parse, then repeat... */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) return; |