diff options
Diffstat (limited to 'apps/px4io')
-rw-r--r-- | apps/px4io/comms.c | 15 | ||||
-rw-r--r-- | apps/px4io/mixer.cpp | 37 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 7 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 2 |
4 files changed, 40 insertions, 21 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index df07c34d8..c208db3ad 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -71,6 +71,8 @@ static struct px4io_report report; static void comms_handle_frame(void *arg, const void *buffer, size_t length); +perf_counter_t comms_rx_errors; + static void comms_init(void) { @@ -78,6 +80,9 @@ comms_init(void) fmu_fd = open("/dev/ttyS1", O_RDWR); stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); + comms_rx_errors = perf_alloc(PC_COUNT, "rx_err"); + hx_stream_set_counters(stream, 0, 0, comms_rx_errors); + /* default state in the report to FMU */ report.i2f_magic = I2F_MAGIC; @@ -173,14 +178,12 @@ comms_handle_command(const void *buffer, size_t length) irqstate_t flags = irqsave(); /* fetch new PWM output values */ - for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) { - system_state.fmu_channel_data[i] = cmd->servo_command[i]; - } + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) + system_state.fmu_channel_data[i] = cmd->output_control[i]; - /* if IO is armed and FMU gets disarmed, IO must also disarm */ - if (system_state.arm_ok && !cmd->arm_ok) { + /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ + if (system_state.arm_ok && !cmd->arm_ok) system_state.armed = false; - } system_state.arm_ok = cmd->arm_ok; system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index b8ea308f3..2acd60ce3 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -56,6 +56,7 @@ #include <systemlib/mixer/mixer.h> extern "C" { +//#define DEBUG #include "px4io.h" } @@ -94,15 +95,17 @@ mixer_tick(void) /* * Decide which set of inputs we're using. */ + /* this is for planes, where manual override makes sense */ if(system_state.manual_override_ok) { /* 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_OUTPUT_CHANNELS; + control_count = PX4IO_CONTROL_CHANNELS; control_values = &system_state.fmu_channel_data[0]; - /* when override is on or the fmu is not available */ + } else if (system_state.rc_channels > 0) { + /* when override is on or the fmu is not available, but RC is present */ control_count = system_state.rc_channels; control_values = &system_state.rc_channel_data[0]; } else { @@ -116,7 +119,7 @@ mixer_tick(void) } else { /* if the fmu is available whe are good */ if(system_state.mixer_fmu_available) { - control_count = PX4IO_OUTPUT_CHANNELS; + control_count = PX4IO_CONTROL_CHANNELS; control_values = &system_state.fmu_channel_data[0]; /* we better shut everything off */ } else { @@ -183,20 +186,22 @@ mixer_callback(uintptr_t handle, 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] - 1500.0f) / 500.0f; return 0; } +static char mixer_text[256]; +static unsigned mixer_text_length = 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; + debug("mixer text %u", length); + if (length < sizeof(px4io_mixdata)) return; @@ -204,11 +209,13 @@ mixer_handle_text(const void *buffer, size_t length) switch (msg->action) { case F2I_MIXER_ACTION_RESET: + debug("reset"); mixer_group.reset(); mixer_text_length = 0; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: + debug("append %d", length); /* check for overflow - this is really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) @@ -218,14 +225,22 @@ mixer_handle_text(const void *buffer, size_t length) memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; + debug("buflen %u", mixer_text_length); /* 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); + unsigned resid = mixer_text_length; + mixer_group.load_from_buf(&mixer_text[0], resid); - /* 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); + /* if anything was parsed */ + if (resid != mixer_text_length) { + debug("used %u", mixer_text_length - resid); + + /* copy any leftover text to the base of the buffer for re-use */ + if (resid > 0) + memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + + mixer_text_length = resid; + } break; } diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 5b485e2ff..c53b94eb0 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -41,7 +41,7 @@ #pragma once -#define PX4IO_OUTPUT_CHANNELS 8 +#define PX4IO_CONTROL_CHANNELS 8 #define PX4IO_INPUT_CHANNELS 12 #define PX4IO_RELAY_CHANNELS 4 @@ -55,7 +55,8 @@ struct px4io_command { #define F2I_MAGIC 0x636d uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; /**< servo output channels */ - uint16_t servo_rate; /**< PWM output rate in Hz */ + uint16_t servo_rate; + uint16_t output_control[PX4IO_CONTROL_CHANNELS]; /**< PWM output rate in Hz */ bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */ bool arm_ok; /**< FMU allows full arming */ bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */ @@ -102,6 +103,6 @@ struct px4io_mixdata { }; /* maximum size is limited by the HX frame size */ -#define F2I_MIXER_MAX_TEXT (sizeof(struct px4io_mixdata) - HX_STREAM_MAX_FRAME) +#define F2I_MIXER_MAX_TEXT (HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata)) #pragma pack(pop)
\ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 1c9d11e99..fef0a9ba4 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -82,7 +82,7 @@ struct sys_state_s { /** * Control signals from FMU. */ - uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS]; + uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; /** * Mixed servo outputs |