From 6b9d5dac4dd866903ed28f062a3c998a85058e63 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 29 Dec 2012 12:58:41 -0800 Subject: [PATCH] Rough in the new mixer path for PX4IO. --- apps/px4io/Makefile | 4 +- apps/px4io/comms.c | 4 +- apps/px4io/{mixer.c => mixer.cpp} | 111 ++++++++++++++++++++++-------- apps/px4io/protocol.h | 48 ++++++++++--- apps/px4io/px4io.h | 6 ++ 5 files changed, 132 insertions(+), 41 deletions(-) rename apps/px4io/{mixer.c => mixer.cpp} (59%) diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile index 801695f842..d97f963ab4 100644 --- a/apps/px4io/Makefile +++ b/apps/px4io/Makefile @@ -41,7 +41,9 @@ # CSRCS = $(wildcard *.c) \ ../systemlib/hx_stream.c \ - ../systemlib/perf_counter.c + ../systemlib/perf_counter.c \ + ../systemlib/up_cxxinitialize.c +CXXSRCS = $(wildcard *.cpp) INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 83a006d43b..bc2142a4b8 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -192,7 +192,6 @@ comms_handle_command(const void *buffer, size_t length) irqrestore(flags); } - static void comms_handle_frame(void *arg, const void *buffer, size_t length) { @@ -208,6 +207,9 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) case F2I_CONFIG_MAGIC: comms_handle_config(buffer, length); break; + case F2I_MIXER_MAGIC: + mixer_handle_text(buffer, length); + break; default: frame_bad++; break; diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.cpp similarity index 59% rename from apps/px4io/mixer.c rename to apps/px4io/mixer.cpp index f02e98ae45..6af1dd3dc6 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mixer.c + * @file mixer.cpp * * Control channel input/output mixer and failsafe. */ @@ -49,8 +49,11 @@ #include #include +#include +extern "C" { #include "px4io.h" +} /* * Count of periodic calls in which we have no FMU input. @@ -58,28 +61,23 @@ static unsigned fmu_input_drops; #define FMU_INPUT_DROP_LIMIT 20 -/* - * Update a mixer based on the current control signals. - */ -static void mixer_update(int mixer, uint16_t *inputs, int input_count); - /* current servo arm/disarm state */ bool mixer_servos_armed = false; -/* - * Each mixer consumes a set of inputs and produces a single output. - */ -struct mixer { - uint16_t current_value; - /* XXX more config here */ -} mixers[IO_SERVO_COUNT]; +/* selected control values and count for mixing */ +static uint16_t *control_values; +static int control_count; + +static int mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + +static MixerGroup mixer_group(mixer_callback, 0); void mixer_tick(void) { - uint16_t *control_values; - int control_count; - int i; bool should_arm; /* @@ -115,17 +113,30 @@ mixer_tick(void) } /* - * Tickle each mixer, if we have control data. + * Run the mixers if we have any control data at all. */ if (control_count > 0) { - for (i = 0; i < IO_SERVO_COUNT; i++) { - mixer_update(i, control_values, control_count); + 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] * 1000.0f) + 1000; + } else { + /* set to zero to inhibit PWM pulse output */ + system_state.servos[i] = 0; + } /* * If we are armed, update the servo output. */ if (system_state.armed && system_state.arm_ok) - up_pwm_servo_set(i, mixers[i].current_value); + up_pwm_servo_set(i, system_state.servos[i]); } } @@ -145,13 +156,57 @@ mixer_tick(void) } } -static void -mixer_update(int mixer, uint16_t *inputs, int input_count) +static int +mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control) { - /* simple passthrough for now */ - if (mixer < input_count) { - mixers[mixer].current_value = inputs[mixer]; - } else { - mixers[mixer].current_value = 0; - } + /* if the control index refers to an input that's not valid, we can't return it */ + if (control_index >= control_count) + return -1; + + /* scale from current PWM units (1000-2000) to mixer input values */ + /* XXX this presents some ugly problems w.r.t failsafe and R/C input scaling that have to be addressed */ + control = ((float)control_values[control_index] - 1000.0f) / 1000.0f; + + return 0; } + +void +mixer_handle_text(const void *buffer, size_t length) +{ + static char mixer_text[256]; + static unsigned mixer_text_length = 0; + + px4io_mixdata *msg = (px4io_mixdata *)buffer; + if (length < sizeof(px4io_mixdata)) + return; + + unsigned text_length = length - sizeof(px4io_mixdata); + + switch (msg->action) { + case F2I_MIXER_ACTION_RESET: + mixer_group.reset(); + mixer_text_length = 0; + /* FALLTHROUGH */ + case F2I_MIXER_ACTION_APPEND: + /* check for overflow - this is really fatal */ + if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) + return; + + /* append mixer text and nul-terminate */ + memcpy(&mixer_text[mixer_text_length], msg->text, text_length); + mixer_text_length += text_length; + mixer_text[mixer_text_length] = '\0'; + + /* process the text buffer, adding new mixers as their descriptions can be parsed */ + char *end = &mixer_text[mixer_text_length]; + mixer_group.load_from_buf(&mixer_text[0], mixer_text_length); + + /* copy any leftover text to the base of the buffer for re-use */ + if (mixer_text_length > 0) + memcpy(&mixer_text[0], end - mixer_text_length, mixer_text_length); + break; + } +} \ No newline at end of file diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c704b12016..080d33001d 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -47,25 +47,21 @@ #pragma pack(push, 1) -/* command from FMU to IO */ +/** + * Periodic command from FMU to IO. + */ struct px4io_command { uint16_t f2i_magic; -#define F2I_MAGIC 0x636d +#define F2I_MAGIC 0x636d uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; bool relay_state[PX4IO_RELAY_CHANNELS]; bool arm_ok; }; -/* config message from FMU to IO */ -struct px4io_config { - uint16_t f2i_config_magic; -#define F2I_CONFIG_MAGIC 0x6366 - - /* XXX currently nothing here */ -}; - -/* report from IO to FMU */ +/** + * Periodic report from IO to FMU + */ struct px4io_report { uint16_t i2f_magic; #define I2F_MAGIC 0x7570 @@ -75,4 +71,34 @@ struct px4io_report { uint8_t channel_count; }; +/** + * As-needed config message from FMU to IO + */ +struct px4io_config { + uint16_t f2i_config_magic; +#define F2I_CONFIG_MAGIC 0x6366 + + /* XXX currently nothing here */ +}; + +/** + * As-needed mixer data upload. + * + * This message adds text to the mixer text buffer; the text + * buffer is drained as the definitions are consumed. + */ +struct px4io_mixdata { + uint16_t f2i_mixer_magic; +#define F2I_MIXER_MAGIC 0x6d74 + + uint8_t action; +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 + + char text[0]; /* actual text size may vary */ +}; + +/* maximum size is limited by the HX frame size */ +#define F2I_MIXER_MAX_TEXT (sizeof(struct px4io_mixdata) - HX_STREAM_MAX_FRAME) + #pragma pack(pop) \ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 45b7cf847a..f127df4a3d 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -84,6 +84,11 @@ struct sys_state_s */ uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS]; + /* + * Mixed servo outputs + */ + uint16_t servos[IO_SERVO_COUNT]; + /* * Relay controls */ @@ -149,6 +154,7 @@ extern volatile int timers[TIMER_NUM_TIMERS]; * Mixer */ extern void mixer_tick(void); +extern void mixer_handle_text(const void *buffer, size_t length); /* * Safety switch/LED.