From 4e38615595abd9d27d0cb000caafb98cc3670abe Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 13 Jan 2013 18:57:27 -0800 Subject: [PATCH] Major workover of the PX4IO firmware for I2C operation. --- apps/px4io/Makefile | 12 +- apps/px4io/controls.c | 288 +++++++++++++++++++++++++++++------------ apps/px4io/dsm.c | 44 +++---- apps/px4io/i2c.c | 1 - apps/px4io/mixer.cpp | 130 ++++++++++--------- apps/px4io/protocol.h | 39 +++++- apps/px4io/px4io.c | 32 +++-- apps/px4io/px4io.h | 77 ++++++----- apps/px4io/registers.c | 187 +++++++++++++++++++------- apps/px4io/safety.c | 29 ++--- apps/px4io/sbus.c | 42 +++--- 11 files changed, 561 insertions(+), 320 deletions(-) diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile index d97f963ab4..0eb3ffcf7a 100644 --- a/apps/px4io/Makefile +++ b/apps/px4io/Makefile @@ -39,11 +39,19 @@ # Note that we pull a couple of specific files from the systemlib, since # we can't support it all. # -CSRCS = $(wildcard *.c) \ +CSRCS = adc.c \ + controls.c \ + dsm.c \ + i2c.c \ + px4io.c \ + registers.c \ + safety.c \ + sbus.c \ ../systemlib/hx_stream.c \ ../systemlib/perf_counter.c \ ../systemlib/up_cxxinitialize.c -CXXSRCS = $(wildcard *.cpp) + +CXXSRCS = mixer.cpp INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 169d5bb817..072d296dab 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -37,34 +37,22 @@ * R/C inputs and servo outputs. */ - #include -#include #include -#include -#include -#include -#include -#include -#include -#include #include -#include - #include -#include #include #include -#define DEBUG +//#define DEBUG #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 1700 -#define RC_CHANNEL_LOW_THRESH 1300 +#define RC_CHANNEL_HIGH_THRESH 5000 +#define RC_CHANNEL_LOW_THRESH -5000 -static void ppm_input(void); +static bool ppm_input(uint16_t *values, uint16_t *num_values); void controls_main(void) @@ -79,9 +67,26 @@ controls_main(void) fds[1].fd = sbus_init("/dev/ttyS2"); fds[1].events = POLLIN; + ASSERT(fds[0].fd >= 0); + ASSERT(fds[1].fd >= 0); + + /* default to a 1:1 input map */ + for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 50; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i; + } + for (;;) { /* run this loop at ~100Hz */ - poll(fds, 2, 10); + int result = poll(fds, 2, 10); + + ASSERT(result >= 0); /* * Gather R/C control inputs from supported sources. @@ -90,95 +95,212 @@ controls_main(void) * one control input source, they're going to fight each * other. Don't do that. */ - bool locked = false; + + bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); + if (dsm_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); + if (sbus_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; /* - * Store RC channel count to detect switch to RC loss sooner - * than just by timeout - */ - unsigned rc_channels = system_state.rc_channels; - - if (fds[0].revents & POLLIN) - locked |= dsm_input(); - - if (fds[1].revents & POLLIN) - locked |= sbus_input(); - - /* - * If we don't have lock from one of the serial receivers, - * look for PPM. It shares an input with S.bus, so there's - * a possibility it will mis-parse an S.bus frame. - * * XXX each S.bus frame will cause a PPM decoder interrupt * storm (lots of edges). It might be sensible to actually - * disable the PPM decoder completely if we have an alternate - * receiver lock. + * disable the PPM decoder completely if we have S.bus signal. */ - if (!locked) - ppm_input(); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); + if (ppm_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; - /* check for manual override status */ - if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { - /* force manual input override */ - system_state.mixer_manual_override = true; + ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - } else { - /* override not engaged, use FMU */ - system_state.mixer_manual_override = false; + /* + * In some cases we may have received a frame, but input has still + * been lost. + */ + bool rc_input_lost = false; + + /* + * If we received a new frame from any of the RC sources, process it. + */ + if (dsm_updated | sbus_updated | ppm_updated) { + + /* update RC-received timestamp */ + system_state.rc_channels_timestamp = hrt_absolute_time(); + + /* record a bitmask of channels assigned */ + unsigned assigned_channels = 0; + + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { + + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] && PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + + uint16_t raw = r_raw_rc_values[i]; + + /* implement the deadzone */ + if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) { + raw += conf[PX4IO_P_RC_CONFIG_DEADZONE]; + if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) + raw = conf[PX4IO_P_RC_CONFIG_CENTER]; + } + if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) { + raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE]; + if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) + raw = conf[PX4IO_P_RC_CONFIG_CENTER]; + } + + /* constrain to min/max values */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; + + int16_t scaled = raw; + + /* adjust to zero-relative (-500..500) */ + scaled -= 1500; + + /* scale to fixed-point representation (-10000..10000) */ + scaled *= 20; + + ASSERT(scaled >= -15000); + ASSERT(scaled <= 15000); + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; + + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + ASSERT(mapped < MAX_CONTROL_CHANNELS); + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } + } + + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } + + /* + * If we got an update with zero channels, treat it as + * a loss of input. + */ + if (assigned_channels == 0) + rc_input_lost = true; + + /* + * Export the valid channel bitmap + */ + r_rc_valid = assigned_channels; } /* * If we haven't seen any new control data in 200ms, assume we - * have lost input and tell FMU. + * have lost input. */ if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { - - if (system_state.rc_channels > 0) { - /* - * If the RC signal status (lost / present) has - * just changed, request an update immediately. - */ - system_state.fmu_report_due = true; - } - - /* set the number of channels to zero - no inputs */ - system_state.rc_channels = 0; + rc_input_lost = true; } /* - * PWM output updates are performed in addition on each comm update. - * the updates here are required to ensure operation if FMU is not started - * or stopped responding. + * Handle losing RC input */ - mixer_tick(); - } -} + if (rc_input_lost) { -static void -ppm_input(void) -{ - /* - * Look for new PPM input. - */ - if (ppm_last_valid_decode != 0) { + /* Clear the RC input status flags, clear manual override control */ + r_status_flags &= ~( + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK | + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS); - /* avoid racing with PPM updates */ - irqstate_t state = irqsave(); + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; - /* PPM data exists, copy it */ - system_state.rc_channels = ppm_decoded_channels; - - for (unsigned i = 0; i < ppm_decoded_channels; i++) { - system_state.rc_channel_data[i] = ppm_buffer[i]; + /* Mark the arrays as empty */ + r_raw_rc_count = 0; + r_rc_valid = 0; } - /* copy the timestamp and clear it */ - system_state.rc_channels_timestamp = ppm_last_valid_decode; - ppm_last_valid_decode = 0; + /* + * Check for manual override. + * + * The OVERRIDE_OK feature must be set, and we must have R/C input. + * Override is enabled if either the hardcoded channel / value combination + * is selected, or the AP has requested it. + */ + if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { - irqrestore(state); + bool override = false; + + /* + * Check mapped channel 5; if the value is 'high' then the pilot has + * requested override. + */ + if ((r_rc_valid & (1 << 4)) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) + override = true; + + /* + * Check for an explicit manual override request from the AP. + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)) + override = true; + + if (override) { + + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + + /* mix new RC input control values to servos */ + if (dsm_updated | sbus_updated | ppm_updated) + mixer_tick(); + + } else { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + } + } - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; } } + +static bool +ppm_input(uint16_t *values, uint16_t *num_values) +{ + bool result = false; + + /* avoid racing with PPM updates */ + irqstate_t state = irqsave(); + + /* + * Look for recent PPM input. + */ + if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) { + + /* PPM data exists, copy it */ + result = true; + *num_values = ppm_decoded_channels; + if (*num_values > MAX_CONTROL_CHANNELS) + *num_values = MAX_CONTROL_CHANNELS; + + for (unsigned i = 0; i < *num_values; i++) + values[i] = ppm_buffer[i]; + + /* clear validity */ + ppm_last_valid_decode = 0; + } + + irqrestore(state); + + return result; +} diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 560ef47d94..f0e8e0f322 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -43,17 +43,13 @@ #include #include -#include #include -#include - #include #define DEBUG #include "px4io.h" -#include "protocol.h" #define DSM_FRAME_SIZE 16 #define DSM_FRAME_CHANNELS 7 @@ -72,13 +68,13 @@ unsigned dsm_frame_drops; static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); static void dsm_guess_format(bool reset); -static void dsm_decode(hrt_abstime now); +static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values); int dsm_init(const char *device) { if (dsm_fd < 0) - dsm_fd = open(device, O_RDONLY); + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); if (dsm_fd >= 0) { struct termios t; @@ -106,7 +102,7 @@ dsm_init(const char *device) } bool -dsm_input(void) +dsm_input(uint16_t *values, uint16_t *num_values) { ssize_t ret; hrt_abstime now; @@ -143,7 +139,7 @@ dsm_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - goto out; + return false; last_rx_time = now; @@ -156,20 +152,14 @@ dsm_input(void) * If we don't have a full frame, return */ if (partial_frame_count < DSM_FRAME_SIZE) - goto out; + return false; /* * Great, it looks like we might have a frame. Go ahead and * decode it. */ - dsm_decode(now); partial_frame_count = 0; - -out: - /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' - */ - return (now - last_frame_time) < 200000; + return dsm_decode(now, values, num_values); } static bool @@ -273,8 +263,8 @@ dsm_guess_format(bool reset) dsm_guess_format(true); } -static void -dsm_decode(hrt_abstime frame_time) +static bool +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) { /* @@ -295,7 +285,7 @@ dsm_decode(hrt_abstime frame_time) /* if we don't know the frame format, update the guessing state machine */ if (channel_shift == 0) { dsm_guess_format(false); - return; + return false; } /* @@ -323,8 +313,8 @@ dsm_decode(hrt_abstime frame_time) continue; /* update the decoded channel count */ - if (channel >= system_state.rc_channels) - system_state.rc_channels = channel + 1; + if (channel >= *num_values) + *num_values = channel + 1; /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ if (channel_shift == 11) @@ -355,13 +345,11 @@ dsm_decode(hrt_abstime frame_time) break; } - system_state.rc_channel_data[channel] = value; + values[channel] = value; } - /* and note that we have received data from the R/C controller */ - /* XXX failsafe will cause problems here - need a strategy for detecting it */ - system_state.rc_channels_timestamp = frame_time; - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + /* + * XXX Note that we may be in failsafe here; we need to work out how to detect that. + */ + return true; } diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c index 4e95dd5831..d08da2ff14 100644 --- a/apps/px4io/i2c.c +++ b/apps/px4io/i2c.c @@ -259,7 +259,6 @@ i2c_tx_complete(void) /* XXX handle transmit-done */ /* prepare for the next transaction */ - memset(tx_buf, 0, sizeof(tx_buf)); i2c_tx_setup(); } diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ed7d684b6f..7ccf915b00 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -38,22 +38,13 @@ */ #include -#include #include #include #include -#include -#include -#include -#include -#include - -#include #include #include -#include #include @@ -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. + */ + 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; /* - * Run the mixers if we have any control data at all. + * Update the servo outputs. */ - 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; - } - - /* - * 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 = 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; diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index aabc476dd7..4cb8a54ef3 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -55,7 +55,9 @@ * * As convention, values that would be floating point in other parts of * the PX4 system are expressed as signed integer values scaled by 10000, - * e.g. control values range from -10000..10000. + * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and + * SIGNED_TO_REG macros to convert between register representation and + * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. * * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be @@ -66,6 +68,12 @@ #define PX4IO_INPUT_CHANNELS 12 #define PX4IO_RELAY_CHANNELS 4 +/* Per C, this is safe for all 2's complement systems */ +#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) +#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) + +#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) +#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 @@ -91,13 +99,15 @@ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ #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_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 */ #define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ #define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */ #define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ -#define PX4IO_P_STATUS_ALARMS_AP_LOST (1 << 4) +#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) #define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ #define PX4IO_P_STATUS_TEMPERATURE 5 /* temperature in (units tbd) */ @@ -109,19 +119,26 @@ #define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* array of raw RC input values, microseconds */ -#define PX4IO_PAGE_RAW_RC_INPUT 4 /* 0..CONFIG_RC_INPUT_COUNT-1 */ +#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ +#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ /* array of scaled RC input values, -10000..10000 */ -#define PX4IO_PAGE_RC_INPUT 5 /* 0..CONFIG_RC_INPUT_COUNT-1 */ +#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ +#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ /* array of raw ADC values */ #define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ /* setup page */ #define PX4IO_PAGE_SETUP 100 +#define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK (1 << 0) /* OK to switch to manual override */ + #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */ -#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE (1 << 1) /* request local override */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE (1 << 2) /* request switch to manual override */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ @@ -136,7 +153,17 @@ /* raw text load to the mixer parser - ignores offset */ #define PX4IO_PAGE_MIXERLOAD 102 - +/* R/C channel config */ +#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */ +#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) +#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) +#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ /* * Old serial PX4FMU <-> PX4IO messaging protocol. diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index e51c1c73c1..13f46939a8 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -37,22 +37,22 @@ */ #include -#include + +#include // required for task_create #include -#include -#include -#include #include #include #include - -#include +#include #include #include +#include + #include +#define DEBUG #include "px4io.h" __EXPORT int user_start(int argc, char *argv[]); @@ -70,8 +70,6 @@ int user_start(int argc, char *argv[]) /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); - /* default to 50 Hz PWM outputs */ - system_state.servo_rate = 50; /* configure the high-resolution time/callout interface */ hrt_init(); @@ -83,7 +81,7 @@ int user_start(int argc, char *argv[]) hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); /* print some startup info */ - lib_lowprintf("\nPX4IO: starting\n"); + debug("\nPX4IO: starting\n"); /* default all the LEDs to off while we start */ LED_AMBER(false); @@ -108,11 +106,19 @@ int user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); - lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); + debug("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); - /* start the i2c test code */ + /* start the i2c handler */ i2c_init(); - /* we're done here, go run the communications loop */ - comms_main(); + /* add a performance counter for mixing */ + perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); + + /* run the mixer at 100Hz (for now...) */ + for (;;) { + poll(NULL, 0, 10); + perf_begin(mixer_perf); + mixer_tick(); + perf_end(mixer_perf); + } } \ No newline at end of file diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index fb06596023..2e2c50a3a8 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -75,6 +75,29 @@ extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ +extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ + +/* + * Register aliases. + * + * Handy aliases for registers that are widely used. + */ +#define r_status_flags r_page_status[PX4IO_P_STATUS_FLAGS] +#define r_status_alarms r_page_status[PX4IO_P_STATUS_ALARMS] + +#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] +#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] +#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE]) + +#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] +#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] +#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] +#define r_setup_pwm_lowrate r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] +#define r_setup_pwm_highrate r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] +#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] + +#define r_control_values (&r_page_controls[0]) /* * System state structure. @@ -84,9 +107,9 @@ extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ */ struct sys_state_s { - bool armed; /* IO armed */ - bool arm_ok; /* FMU says OK to arm */ - uint16_t servo_rate; /* output rate of servos in Hz */ +// bool armed; /* IO armed */ +// bool arm_ok; /* FMU says OK to arm */ +// uint16_t servo_rate; /* output rate of servos in Hz */ /** * Remote control input(s) channel mappings @@ -105,39 +128,39 @@ struct sys_state_s { /** * Data from the remote control input(s) */ - unsigned rc_channels; - uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; +// unsigned rc_channels; +// uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; /** * Control signals from FMU. */ - uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; +// uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS]; /** * Mixed servo outputs */ - uint16_t servos[IO_SERVO_COUNT]; +// uint16_t servos[IO_SERVO_COUNT]; /** * Relay controls */ - bool relays[PX4IO_RELAY_CHANNELS]; +// bool relays[PX4IO_RELAY_CHANNELS]; /** * If true, we are using the FMU controls, else RC input if available. */ - bool mixer_manual_override; +// bool mixer_manual_override; /** * If true, FMU input is available. */ - bool mixer_fmu_available; +// bool mixer_fmu_available; /** * If true, state that should be reported to FMU has been updated. */ - bool fmu_report_due; +// bool fmu_report_due; /** * Last FMU receive time, in microseconds since system boot @@ -147,12 +170,12 @@ struct sys_state_s { /** * If true, the RC signal has been lost for more than a timeout interval */ - bool rc_lost; +// bool rc_lost; /** * If true, the connection to FMU has been lost for more than a timeout interval */ - bool fmu_lost; +// bool fmu_lost; /** * If true, FMU is ready for autonomous position control. Only used for LED indication @@ -162,41 +185,27 @@ struct sys_state_s { /** * If true, IO performs an on-board manual override with the RC channel values */ - bool manual_override_ok; +// bool manual_override_ok; /* * Measured battery voltage in mV */ - uint16_t battery_mv; +// uint16_t battery_mv; /* * ADC IN5 measurement */ - uint16_t adc_in5; +// uint16_t adc_in5; /* * Power supply overcurrent status bits. */ - uint8_t overcurrent; +// uint8_t overcurrent; }; extern struct sys_state_s system_state; -#if 0 -/* - * Software countdown timers. - * - * Each timer counts down to zero at one tick per ms. - */ -#define TIMER_BLINK_AMBER 0 -#define TIMER_BLINK_BLUE 1 -#define TIMER_STATUS_PRINT 2 -#define TIMER_SANITY 7 -#define TIMER_NUM_TIMERS 8 -extern volatile int timers[TIMER_NUM_TIMERS]; -#endif - /* * GPIO handling. */ @@ -249,9 +258,11 @@ extern uint16_t adc_measure(unsigned channel); /* * R/C receiver handling. + * + * Input functions return true when they receive an update from the RC controller. */ extern void controls_main(void); extern int dsm_init(const char *device); -extern bool dsm_input(void); +extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern int sbus_init(const char *device); -extern bool sbus_input(void); +extern bool sbus_input(uint16_t *values, uint16_t *num_values); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 1397dd2a44..59684f1ee8 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -37,6 +37,11 @@ * Implementation of the PX4IO register space. */ +#include + +#include +#include + #include "px4io.h" #include "protocol.h" @@ -45,8 +50,9 @@ static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); /** * Setup registers */ -uint16_t r_page_setup[] = +volatile uint16_t r_page_setup[] = { + [PX4IO_P_SETUP_FEATURES] = 0, [PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_LOWRATE] = 50, @@ -54,14 +60,16 @@ uint16_t r_page_setup[] = [PX4IO_P_SETUP_RELAYS] = 0, }; -#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE) +#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) +#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \ + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE) #define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) -#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PXIO_RELAY_CHANNELS) - 1) +#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) /** * Control values from the FMU. */ -uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; +volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; /** * Static configuration parameters. @@ -108,13 +116,26 @@ uint16_t r_page_servos[IO_SERVO_COUNT]; /** * Scaled/routed RC input */ -uint16_t r_page_rc_input[MAX_CONTROL_CHANNELS]; +uint16_t r_page_rc_input[] = { + [PX4IO_P_RC_VALID] = 0, + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 +}; /** * Raw RC input */ -uint16_t r_page_raw_rc_input[MAX_CONTROL_CHANNELS]; +uint16_t r_page_raw_rc_input[] = +{ + [PX4IO_P_RAW_RC_COUNT] = 0, + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 +}; +/** + * R/C channel input configuration. + */ +uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; + +#define PX4IO_P_RC_CONFIG_OPTIONS_VALID PX4IO_P_RC_CONFIG_OPTIONS_REVERSE void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -136,7 +157,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } /* XXX we should cause a mixer tick ASAP */ - system_state.mixer_fmu_available = true; + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; break; /* handle text going to the mixer parser */ @@ -168,7 +189,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) switch (offset) { case PX4IO_P_STATUS_ALARMS: /* clear bits being written */ - r_page_status[PX4IO_P_STATUS_ALARMS] &= ~value; + r_status_alarms &= ~value; break; default: @@ -179,20 +200,30 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_SETUP: switch (offset) { + case PX4IO_P_SETUP_FEATURES: + + value &= PX4IO_P_SETUP_FEATURES_VALID; + r_setup_features = value; + + /* update manual override state - disable if no longer OK */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && !(value & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK)) + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + + break; + case PX4IO_P_SETUP_ARMING: value &= PX4IO_P_SETUP_ARMING_VALID; - r_page_setup[PX4IO_P_SETUP_ARMING] = value; + r_setup_arming = value; /* update arming state - disarm if no longer OK */ - if (system_state.armed && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) - system_state.armed = false; - + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; break; case PX4IO_P_SETUP_PWM_RATES: value &= PX4IO_P_SETUP_RATES_VALID; - r_page_setup[PX4IO_P_SETUP_PWM_RATES] = value; + r_setup_pwm_rates = value; /* XXX re-configure timers */ break; @@ -201,7 +232,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value = 50; if (value > 400) value = 400; - r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] = value; + r_setup_pwm_lowrate = value; /* XXX re-configure timers */ break; @@ -210,13 +241,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value = 50; if (value > 400) value = 400; - r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] = value; + r_setup_pwm_highrate = value; /* XXX re-configure timers */ break; case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; - r_page_setup[PX4IO_P_SETUP_RELAYS] = value; + r_setup_relays = value; POWER_RELAY1(value & (1 << 0) ? 1 : 0); POWER_RELAY2(value & (1 << 1) ? 1 : 0); POWER_ACC1(value & (1 << 2) ? 1 : 0); @@ -228,6 +259,63 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) } break; + case PX4IO_PAGE_RC_CONFIG: { + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; + unsigned index = offset % PX4IO_P_RC_CONFIG_STRIDE; + uint16_t *conf = &r_page_rc_input_config[offset * PX4IO_P_RC_CONFIG_STRIDE]; + + if (channel >= MAX_CONTROL_CHANNELS) + return -1; + + /* disable the channel until we have a chance to sanity-check it */ + conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + switch (index) { + + case PX4IO_P_RC_CONFIG_MIN: + case PX4IO_P_RC_CONFIG_CENTER: + case PX4IO_P_RC_CONFIG_MAX: + case PX4IO_P_RC_CONFIG_DEADZONE: + case PX4IO_P_RC_CONFIG_ASSIGNMENT: + conf[index] = value; + break; + + case PX4IO_P_RC_CONFIG_OPTIONS: + value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + + /* set all options except the enabled option */ + conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + /* should the channel be enabled? */ + /* this option is normally set last */ + if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + /* assert min..center..max ordering */ + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) + break; + if (conf[PX4IO_P_RC_CONFIG_MAX] < 2500) + break; + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) + break; + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) + break; + /* assert deadzone is sane */ + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) + break; + if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) + break; + if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) + break; + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) + break; + + /* sanity checks pass, enable channel */ + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } + break; + + } + } + default: return -1; } @@ -237,27 +325,27 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values) { +#define SELECT_PAGE(_page_name) { *values = _page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); } switch (page) { - case PX4IO_PAGE_CONFIG: - *values = r_page_config; - *num_values = sizeof(r_page_config) / sizeof(r_page_config[0]); - break; + /* + * Handle pages that are updated dynamically at read time. + */ case PX4IO_PAGE_STATUS: + /* PX4IO_P_STATUS_FREEMEM */ { struct mallinfo minfo = mallinfo(); r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks; } + /* XXX PX4IO_P_STATUS_CPULOAD */ - r_page_status[PX4IO_P_STATUS_FLAGS] = - (system_state.armed ? PX4IO_P_STATUS_FLAGS_ARMED : 0) | - (system_state.manual_override_ok ? PX4IO_P_STATUS_FLAGS_OVERRIDE : 0) | - ((system_state.rc_channels > 0) ? PX4IO_P_STATUS_FLAGS_RC_OK : 0)) - /* XXX specific receiver status */ - /* XXX PX4IO_P_STATUS_ALARMS] */ + /* PX4IO_P_STATUS_FLAGS maintained externally */ + /* PX4IO_P_STATUS_ALARMS maintained externally */ + + /* PX4IO_P_STATUS_VBATT */ { /* * Coefficients here derived by measurement of the 5-16V @@ -285,43 +373,42 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val unsigned counts = adc_measure(ADC_VBATT); r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10; } + /* XXX PX4IO_P_STATUS_TEMPERATURE */ - *values = r_page_status; - *num_values = sizeof(r_page_status) / sizeof(r_page_status[0]); + SELECT_PAGE(r_page_status); break; - case PX4IO_PAGE_ACTUATORS: - *values = r_page_actuators; - *num_values = sizeof(r_page_actuators) / sizeof(r_page_actuators[0]); - break; - - case PX4IO_PAGE_SERVOS: - *values = system_state.servos; - *num_values = IO_SERVO_COUNT; - break; - - case PX4IO_PAGE_RAW_RC_INPUT: - *values = r_page_raw_rc_input; - *num_values = sizeof(r_page_raw_rc_input) / sizeof(r_page_raw_rc_input[0]); - break; - - case PX4IO_PAGE_RC_INPUT: - *values = system_state.rc_channel_data; - *num_values = system_state.rc_channels; - return -1; - case PX4IO_PAGE_RAW_ADC_INPUT: r_page_adc[0] = adc_measure(ADC_VBATT); r_page_adc[1] = adc_measure(ADC_IN5); - *values = r_page_adc; - *num_values = ADC_CHANNEL_COUNT; + + SELECT_PAGE(r_page_adc); break; + /* + * Pages that are just a straight read of the register state. + */ +#define COPY_PAGE(_page_name, _page) case _page_name: SELECT_PAGE(_page); break; + + /* status pages */ + COPY_PAGE(PX4IO_PAGE_CONFIG, r_page_config); + COPY_PAGE(PX4IO_PAGE_ACTUATORS, r_page_actuators); + COPY_PAGE(PX4IO_PAGE_SERVOS, r_page_servos); + COPY_PAGE(PX4IO_PAGE_RAW_RC_INPUT, r_page_raw_rc_input); + COPY_PAGE(PX4IO_PAGE_RC_INPUT, r_page_rc_input); + + /* readback of input pages */ + COPY_PAGE(PX4IO_PAGE_SETUP, r_page_setup); + COPY_PAGE(PX4IO_PAGE_CONTROLS, r_page_controls); + COPY_PAGE(PX4IO_PAGE_RC_CONFIG, r_page_rc_input_config); + default: return -1; } +#undef SELECT_PAGE + /* if the offset is beyond the end of the page, we have no data */ if (*num_values <= offset) return -1; diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 0cae29ac98..a14051f76d 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -36,15 +36,8 @@ */ #include -#include -#include -#include -#include -#include -#include -#include -#include +#include #include @@ -116,30 +109,28 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !system_state.armed) { + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { - /* change to armed state and notify the FMU */ - system_state.armed = true; + /* switch to armed state */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; counter++; - system_state.fmu_report_due = true; } /* Disarm quickly */ - } else if (safety_button_pressed && system_state.armed) { + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - system_state.armed = false; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; counter++; - system_state.fmu_report_due = true; } } else { @@ -149,15 +140,15 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_SAFE; - if (system_state.armed) { - if (system_state.arm_ok) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { pattern = LED_PATTERN_IO_FMU_ARMED; } else { pattern = LED_PATTERN_IO_ARMED; } - } else if (system_state.arm_ok) { + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) { pattern = LED_PATTERN_FMU_ARMED; } else if (system_state.vector_flight_mode_ok) { @@ -188,7 +179,7 @@ failsafe_blink(void *arg) static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ - if (!system_state.mixer_fmu_available) { + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; } else { diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 568ef8091c..d199a9361c 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -66,13 +66,13 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static void sbus_decode(hrt_abstime frame_time); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); int sbus_init(const char *device) { if (sbus_fd < 0) - sbus_fd = open(device, O_RDONLY); + sbus_fd = open(device, O_RDONLY | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -97,7 +97,7 @@ sbus_init(const char *device) } bool -sbus_input(void) +sbus_input(uint16_t *values, uint16_t *num_values) { ssize_t ret; hrt_abstime now; @@ -134,7 +134,7 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - goto out; + return false; last_rx_time = now; @@ -147,20 +147,14 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - goto out; + return false; /* * Great, it looks like we might have a frame. Go ahead and * decode it. */ - sbus_decode(now); partial_frame_count = 0; - -out: - /* - * If we have seen a frame in the last 200ms, we consider ourselves 'locked' - */ - return (now - last_frame_time) < 200000; + return sbus_decode(now, values, num_values); } /* @@ -199,13 +193,13 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } }; -static void -sbus_decode(hrt_abstime frame_time) +static bool +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; - return; + return false; } /* if the failsafe or connection lost bit is set, we consider the frame invalid */ @@ -213,8 +207,8 @@ sbus_decode(hrt_abstime frame_time) (frame[23] & (1 << 3))) { /* failsafe */ /* actively announce signal loss */ - system_state.rc_channels = 0; - return 1; + *values = 0; + return false; } /* we have received something we think is a frame */ @@ -241,23 +235,19 @@ sbus_decode(hrt_abstime frame_time) } /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - system_state.rc_channel_data[channel] = (value / 2) + 998; + values[channel] = (value / 2) + 998; } /* decode switch channels if data fields are wide enough */ if (chancount > 17) { /* channel 17 (index 16) */ - system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998; + values[16] = (frame[23] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ - system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998; + values[17] = (frame[23] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ - system_state.rc_channels = chancount; + *num_values = chancount; - /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; - - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; + return true; }