From 112f5ea9697a2ada9e3852f9c2e7c10ab0e78a8a Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 15 Jan 2013 00:41:13 -0800 Subject: Add support for raw PWM passthrough from FMU via IO. --- apps/px4io/mixer.cpp | 68 ++++++++++++++++++++++++++++++++------------------ apps/px4io/protocol.h | 6 ++++- apps/px4io/px4io.c | 1 + apps/px4io/registers.c | 22 +++++++++++++++- 4 files changed, 71 insertions(+), 26 deletions(-) (limited to 'apps') diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index f394233f4..178b0bb43 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -81,6 +81,7 @@ static int mixer_callback(uintptr_t handle, uint8_t control_index, float &control); +static void mix(); static MixerGroup mixer_group(mixer_callback, 0); void @@ -91,7 +92,7 @@ mixer_tick(void) /* too long without FMU input, time to go to failsafe */ r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; debug("AP RX timeout"); } @@ -100,13 +101,26 @@ mixer_tick(void) * Decide which set of controls we're using. */ if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { /* this is for planes, where manual override makes sense */ source = MIX_OVERRIDE; + + /* mix from the override controls */ + mix(); + } else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - source = MIX_FMU; + + if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) { + /* FMU has already provided PWM values */ + } else { + /* mix from FMU controls */ + source = MIX_FMU; + mix(); + } } else { source = MIX_FAILSAFE; + /* XXX actually, have no idea what to do here... load hardcoded failsafe controls? */ } #if 0 @@ -173,27 +187,6 @@ mixer_tick(void) } } #endif - /* - * Run the mixers. - */ - 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; - - } - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) - r_page_servos[i] = 0; /* * Update the servo outputs. @@ -219,6 +212,33 @@ mixer_tick(void) } } +static void +mix() +{ + /* + * Run the mixers. + */ + 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; + + } + for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + r_page_servos[i] = 0; + +} + static int mixer_callback(uintptr_t handle, uint8_t control_group, diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 501691cd2..a2be4bfc4 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -100,6 +100,7 @@ #define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ #define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ +#define PX4IO_P_STATUS_FLAGS_RAW_PPM (1 << 7) /* raw PPM from FMU is bypassing the mixer */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ @@ -149,7 +150,7 @@ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..STATUS_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ #define PX4IO_PAGE_MIXERLOAD 102 @@ -166,6 +167,9 @@ #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ +/* PWM output - overrides mixer */ +#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + /** * As-needed mixer data upload. * diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 13f46939a..c3dacb2a6 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -115,6 +115,7 @@ int user_start(int argc, char *argv[]) perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); /* run the mixer at 100Hz (for now...) */ + /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */ for (;;) { poll(NULL, 0, 10); perf_begin(mixer_perf); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 0dd8fe28d..2f411eebf 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -152,7 +152,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { - /* XXX scaling - should be -10000..10000 */ + /* XXX range-check value? */ r_page_controls[offset] = *values; offset++; @@ -163,6 +163,26 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* XXX we should cause a mixer tick ASAP */ system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PPM; + break; + + /* handle raw PWM input */ + case PX4IO_PAGE_DIRECT_PWM: + + /* copy channel data */ + while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + + /* XXX range-check value? */ + r_page_servos[offset] = *values; + + offset++; + num_values--; + values++; + } + + /* XXX need to force these values to the servos */ + system_state.fmu_data_received_time = hrt_absolute_time(); + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM; break; /* handle text going to the mixer parser */ -- cgit v1.2.3