diff options
Diffstat (limited to 'src/modules/px4iofirmware')
| -rw-r--r-- | src/modules/px4iofirmware/adc.c | 53 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/controls.c | 288 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/dsm.c | 421 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/i2c.c | 11 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 238 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/module.mk | 12 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/protocol.h | 217 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/px4io.c | 135 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/px4io.h | 105 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/registers.c | 365 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/safety.c | 35 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/sbus.c | 95 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/serial.c | 338 |
13 files changed, 1795 insertions, 518 deletions
diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 81566eb2a..2f5908ac5 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -83,6 +83,14 @@ adc_init(void) { adc_perf = perf_alloc(PC_ELAPSED, "adc"); + /* put the ADC into power-down mode */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + + /* bring the ADC out of power-down mode */ + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + /* do calibration if supported */ #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; @@ -96,41 +104,25 @@ adc_init(void) if (rCR2 & ADC_CR2_CAL) return -1; - #endif - /* arbitrarily configure all channels for 55 cycle sample time */ - rSMPR1 = 0b00000011011011011011011011011011; + /* + * Configure sampling time. + * + * For electrical protection reasons, we want to be able to have + * 10K in series with ADC inputs that leave the board. At 12MHz this + * means we need 28.5 cycles of sampling time (per table 43 in the + * datasheet). + */ + rSMPR1 = 0b00000000011011011011011011011011; rSMPR2 = 0b00011011011011011011011011011011; - /* XXX for F2/4, might want to select 12-bit mode? */ - rCR1 = 0; - - /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = -#ifdef ADC_CR2_TSVREFE - /* enable the temperature sensor in CR2 */ - ADC_CR2_TSVREFE | -#endif - 0; - -#ifdef ADC_CCR_TSVREFE - /* enable temperature sensor in CCR */ - rCCR = ADC_CCR_TSVREFE; -#endif + rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */ /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ - - /* power-cycle the ADC and turn it on */ - rCR2 &= ~ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); + rSQR3 = 0; /* will be updated with the channel at conversion time */ return 0; } @@ -141,11 +133,12 @@ adc_init(void) uint16_t adc_measure(unsigned channel) { + perf_begin(adc_perf); /* clear any previous EOC */ - if (rSR & ADC_SR_EOC) - rSR &= ~ADC_SR_EOC; + rSR = 0; + (void)rDR; /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; @@ -158,7 +151,6 @@ adc_measure(unsigned channel) /* never spin forever - this will give a bogus result though */ if (hrt_elapsed_time(&now) > 100) { - debug("adc timeout"); perf_end(adc_perf); return 0xffff; } @@ -166,6 +158,7 @@ adc_measure(unsigned channel) /* read the result and clear EOC */ uint16_t result = rDR; + rSR = 0; perf_end(adc_perf); return result; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3cf9ca149..941500f0d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -50,7 +50,7 @@ #define RC_CHANNEL_HIGH_THRESH 5000 #define RC_CHANNEL_LOW_THRESH -5000 -static bool ppm_input(uint16_t *values, uint16_t *num_values); +static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -59,14 +59,19 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { - /* DSM input */ + /* no channels */ + r_raw_rc_count = 0; + system_state.rc_channels_timestamp_received = 0; + system_state.rc_channels_timestamp_valid = 0; + + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); - /* S.bus input */ + /* S.bus input (USART3) */ sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; @@ -94,16 +99,67 @@ controls_tick() { * other. Don't do that. */ + /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ + uint16_t rssi = 0; + +#ifdef ADC_RSSI + if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + /* use 1:1 scaling on 3.3V ADC input */ + unsigned mV = counts * 3300 / 4096; + + /* scale to 0..253 */ + rssi = mV / 13; + } + } +#endif + perf_begin(c_gather_dsm); - 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; + uint16_t temp_count = r_raw_rc_count; + bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); + if (dsm_updated) { + r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_count = temp_count & 0x7fff; + if (temp_count & 0x8000) + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + else + r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); - if (sbus_updated) + + bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + + bool sbus_failsafe, sbus_frame_drop; + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); + + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + + rssi = 255; + + if (sbus_frame_drop) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; + rssi = 100; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + } + + if (sbus_failsafe) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + rssi = 0; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + + } + perf_end(c_gather_sbus); /* @@ -112,12 +168,21 @@ controls_tick() { * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); - if (ppm_updated) + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); + if (ppm_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } perf_end(c_gather_ppm); - ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); + /* limit number of channels to allowable data size */ + if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) + r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + + /* store RSSI */ + r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; /* * In some cases we may have received a frame, but input has still @@ -130,95 +195,100 @@ controls_tick() { */ 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]; - - int16_t scaled; - - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - 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]; - - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - - } else { - /* in the configured dead zone, output zero */ - scaled = 0; + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_received = hrt_absolute_time(); + + /* do not command anything in failsafe, kick in the RC loss counter */ + if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + + /* 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]; + + int16_t scaled; + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + 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]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } + + /* invert channel if requested */ + 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]; + if (mapped < PX4IO_CONTROL_CHANNELS) { + + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + + } } - - /* invert channel if requested */ - 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); - - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; - - 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; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_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. - * - * This might happen if a protocol-based receiver returns an update - * that contains no channels that we have mapped. - */ - if (assigned_channels == 0) { - rc_input_lost = true; - } else { - /* set RC OK flag */ + /* set RC OK flag, as we got an update */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); + } } /* @@ -231,7 +301,7 @@ controls_tick() { * If we haven't seen any new control data in 200ms, assume we * have lost input. */ - if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { + if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ @@ -239,24 +309,32 @@ controls_tick() { PX4IO_P_STATUS_FLAGS_RC_PPM | PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SBUS); + } /* * Handle losing RC input */ - if (rc_input_lost) { + /* this kicks in if the receiver is gone or the system went to failsafe */ + if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); + /* Mark all channels as invalid, as we just lost the RX */ + r_rc_valid = 0; + /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; + } + + /* this kicks in if the receiver is completely gone */ + if (rc_input_lost) { - /* Mark the arrays as empty */ + /* Set channel count to zero */ r_raw_rc_count = 0; - r_rc_valid = 0; } /* @@ -279,7 +357,7 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH)) override = true; if (override) { @@ -293,11 +371,13 @@ controls_tick() { } else { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } + } else { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } } static bool -ppm_input(uint16_t *values, uint16_t *num_values) +ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) { bool result = false; @@ -312,8 +392,8 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > MAX_CONTROL_CHANNELS) - *num_values = MAX_CONTROL_CHANNELS; + if (*num_values > PX4IO_RC_INPUT_CHANNELS) + *num_values = PX4IO_RC_INPUT_CHANNELS; for (unsigned i = 0; i < *num_values; i++) values[i] = ppm_buffer[i]; @@ -321,6 +401,10 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* clear validity */ ppm_last_valid_decode = 0; + /* store PPM frame length */ + if (num_values) + *frame_len = ppm_frame_length; + /* good if we got any channels */ result = (*num_values > 0); } diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ea35e5513..d3f365822 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +40,7 @@ */ #include <nuttx/config.h> +#include <nuttx/arch.h> #include <fcntl.h> #include <unistd.h> @@ -47,121 +48,44 @@ #include <drivers/drv_hrt.h> -#define DEBUG - #include "px4io.h" -#define DSM_FRAME_SIZE 16 -#define DSM_FRAME_CHANNELS 7 - -static int dsm_fd = -1; - -static hrt_abstime last_rx_time; -static hrt_abstime last_frame_time; - -static uint8_t frame[DSM_FRAME_SIZE]; - -static unsigned partial_frame_count; -static unsigned channel_shift; - -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 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 | O_NONBLOCK); - - if (dsm_fd >= 0) { - struct termios t; +#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/ +#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/ - /* 115200bps, no parity, one stop bit */ - tcgetattr(dsm_fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(dsm_fd, TCSANOW, &t); - - /* initialise the decoder */ - partial_frame_count = 0; - last_rx_time = hrt_absolute_time(); - - /* reset the format detector */ - dsm_guess_format(true); - - debug("DSM: ready"); - - } else { - debug("DSM: open failed"); - } - - return dsm_fd; -} - -bool -dsm_input(uint16_t *values, uint16_t *num_values) -{ - ssize_t ret; - hrt_abstime now; - - /* - * The DSM* protocol doesn't provide any explicit framing, - * so we detect frame boundaries by the inter-frame delay. - * - * The minimum frame spacing is 11ms; with 16 bytes at 115200bps - * frame transmission time is ~1.4ms. - * - * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 5ms passes between calls, - * the first byte we read will be the first byte of a frame. - * - * In the case where byte(s) are dropped from a frame, this also - * provides a degree of protection. Of course, it would be better - * if we didn't drop bytes... - */ - now = hrt_absolute_time(); - - if ((now - last_rx_time) > 5000) { - if (partial_frame_count > 0) { - dsm_frame_drops++; - partial_frame_count = 0; - } - } - - /* - * Fetch bytes, but no more than we would need to complete - * the current frame. - */ - ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count); - - /* if the read failed for any reason, just give up here */ - if (ret < 1) - return false; - - last_rx_time = now; - - /* - * Add bytes to the current frame - */ - partial_frame_count += ret; - - /* - * If we don't have a full frame, return - */ - if (partial_frame_count < DSM_FRAME_SIZE) - return false; - - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - partial_frame_count = 0; - return dsm_decode(now, values, num_values); -} +static int dsm_fd = -1; /**< File handle to the DSM UART */ +static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */ +static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */ +static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */ +static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */ +static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */ +static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */ +/** + * Attempt to decode a single channel raw channel datum + * + * The DSM* protocol doesn't provide any explicit framing, + * so we detect dsm frame boundaries by the inter-dsm frame delay. + * + * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps + * dsm frame transmission time is ~1.4ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 5ms passes between calls, + * the first byte we read will be the first byte of a dsm frame. + * + * In the case where byte(s) are dropped from a dsm frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + * + * Upon receiving a full dsm frame we attempt to decode it + * + * @param[in] raw 16 bit raw channel value from dsm frame + * @param[in] shift position of channel number in raw data + * @param[out] channel pointer to returned channel number + * @param[out] value pointer to returned channel value + * @return true=raw value successfully decoded + */ static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) { @@ -179,6 +103,11 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va return true; } +/** + * Attempt to guess if receiving 10 or 11 bit channel values + * + * @param[in] reset true=reset the 10/11 bit state to unknown + */ static void dsm_guess_format(bool reset) { @@ -191,14 +120,14 @@ dsm_guess_format(bool reset) cs10 = 0; cs11 = 0; samples = 0; - channel_shift = 0; + dsm_channel_shift = 0; return; } - /* scan the channels in the current frame in both 10- and 11-bit mode */ + /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - uint8_t *dp = &frame[2 + (2 * i)]; + uint8_t *dp = &dsm_frame[2 + (2 * i)]; uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; @@ -209,16 +138,16 @@ dsm_guess_format(bool reset) if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) cs11 |= (1 << channel); - /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */ + /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ } - /* wait until we have seen plenty of frames - 2 should normally be enough */ + /* wait until we have seen plenty of frames - 5 should normally be enough */ if (samples++ < 5) return; /* * Iterate the set of sensible sniffed channel sets and see whether - * decoding in 10 or 11-bit mode has yielded anything we recognise. + * decoding in 10 or 11-bit mode has yielded anything we recognize. * * XXX Note that due to what seem to be bugs in the DSM2 high-resolution * stream, we may want to sniff for longer in some cases when we think we @@ -233,6 +162,7 @@ dsm_guess_format(bool reset) 0xff, /* 8 channels (DX8) */ 0x1ff, /* 9 channels (DX9, etc.) */ 0x3ff, /* 10 channels (DX10) */ + 0x1fff, /* 13 channels (DX10t) */ 0x3fff /* 18 channels (DX10) */ }; unsigned votes10 = 0; @@ -248,13 +178,13 @@ dsm_guess_format(bool reset) } if ((votes11 == 1) && (votes10 == 0)) { - channel_shift = 11; + dsm_channel_shift = 11; debug("DSM: 11-bit format"); return; } if ((votes10 == 1) && (votes11 == 0)) { - channel_shift = 10; + dsm_channel_shift = 10; debug("DSM: 10-bit format"); return; } @@ -264,27 +194,149 @@ dsm_guess_format(bool reset) dsm_guess_format(true); } +/** + * Initialize the DSM receive functionality + * + * Open the UART for receiving DSM frames and configure it appropriately + * + * @param[in] device Device name of DSM UART + */ +int +dsm_init(const char *device) +{ + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // enable power on DSM connector + POWER_SPEKTRUM(true); +#endif + + if (dsm_fd < 0) + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + + if (dsm_fd >= 0) { + + struct termios t; + + /* 115200bps, no parity, one stop bit */ + tcgetattr(dsm_fd, &t); + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + tcsetattr(dsm_fd, TCSANOW, &t); + + /* initialise the decoder */ + dsm_partial_frame_count = 0; + dsm_last_rx_time = hrt_absolute_time(); + + /* reset the format detector */ + dsm_guess_format(true); + + debug("DSM: ready"); + + } else { + + debug("DSM: open failed"); + + } + + return dsm_fd; +} + +/** + * Handle DSM satellite receiver bind mode handler + * + * @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart + * @param[in] pulses Number of pulses for dsm_bind_send_pulses command + */ +void +dsm_bind(uint16_t cmd, int pulses) +{ +#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) + #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM +#else + const uint32_t usart1RxAsOutp = + GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; + + if (dsm_fd < 0) + return; + + switch (cmd) { + + case dsm_bind_power_down: + + /*power down DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + POWER_RELAY1(0); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(0); +#endif + break; + + case dsm_bind_power_up: + + /*power up DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + POWER_RELAY1(1); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(1); +#endif + dsm_guess_format(true); + break; + + case dsm_bind_set_rx_out: + + /*Set UART RX pin to active output mode*/ + stm32_configgpio(usart1RxAsOutp); + break; + + case dsm_bind_send_pulses: + + /*Pulse RX pin a number of times*/ + for (int i = 0; i < pulses; i++) { + up_udelay(25); + stm32_gpiowrite(usart1RxAsOutp, false); + up_udelay(25); + stm32_gpiowrite(usart1RxAsOutp, true); + } + break; + + case dsm_bind_reinit_uart: + + /*Restore USART RX pin to RS232 receive mode*/ + stm32_configgpio(GPIO_USART1_RX); + break; + + } +#endif +} + +/** + * Decode the entire dsm frame (all contained channels) + * + * @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess. + * @param[out] values pointer to per channel array of decoded values + * @param[out] num_values pointer to number of raw channel values returned + * @return true=DSM frame successfully decoded, false=no update + */ static bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) { - /* - debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", - frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7], - frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]); + debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", + dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7], + dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]); */ /* * If we have lost signal for at least a second, reset the * format guessing heuristic. */ - if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) dsm_guess_format(true); - /* we have received something we think is a frame */ - last_frame_time = frame_time; + /* we have received something we think is a dsm_frame */ + dsm_last_frame_time = frame_time; - /* if we don't know the frame format, update the guessing state machine */ - if (channel_shift == 0) { + /* if we don't know the dsm_frame format, update the guessing state machine */ + if (dsm_channel_shift == 0) { dsm_guess_format(false); return false; } @@ -296,32 +348,46 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * Each channel is a 16-bit unsigned value containing either a 10- * or 11-bit channel value and a 4-bit channel number, shifted * either 10 or 11 bits. The MSB may also be set to indicate the - * second frame in variants of the protocol where more than + * second dsm_frame in variants of the protocol where more than * seven channels are being transmitted. */ for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - uint8_t *dp = &frame[2 + (2 * i)]; + uint8_t *dp = &dsm_frame[2 + (2 * i)]; uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; - if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) + if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) continue; /* ignore channels out of range */ - if (channel >= PX4IO_INPUT_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) continue; /* update the decoded channel count */ 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) - value /= 2; + /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ + if (dsm_channel_shift == 10) + value *= 2; - value += 998; + /* + * Spektrum scaling is special. There are these basic considerations + * + * * Midpoint is 1520 us + * * 100% travel channels are +- 400 us + * + * We obey the original Spektrum scaling (so a default setup will scale from + * 1100 - 1900 us), but we do not obey the weird 1520 us center point + * and instead (correctly) center the center around 1500 us. This is in order + * to get something useful without requiring the user to calibrate on a digital + * link for no reason. + */ + + /* scaled integer for decent accuracy while staying efficient */ + value = ((((int)value - 1024) * 1000) / 1700) + 1500; /* * Store the decoded channel into the R/C input buffer, taking into @@ -350,7 +416,88 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) } /* + * Spektrum likes to send junk in higher channel numbers to fill + * their packets. We don't know about a 13 channel model in their TX + * lines, so if we get a channel count of 13, we'll return 12 (the last + * data index that is stable). + */ + if (*num_values == 13) + *num_values = 12; + + if (dsm_channel_shift == 11) { + /* Set the 11-bit data indicator */ + *num_values |= 0x8000; + } + + /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. */ return true; } + +/** + * Called periodically to check for input data from the DSM UART + * + * The DSM* protocol doesn't provide any explicit framing, + * so we detect dsm frame boundaries by the inter-dsm frame delay. + * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps + * dsm frame transmission time is ~1.4ms. + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 5ms passes between calls, + * the first byte we read will be the first byte of a dsm frame. + * In the case where byte(s) are dropped from a dsm frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + * Upon receiving a full dsm frame we attempt to decode it. + * + * @param[out] values pointer to per channel array of decoded values + * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data + * @return true=decoded raw channel values updated, false=no update + */ +bool +dsm_input(uint16_t *values, uint16_t *num_values) +{ + ssize_t ret; + hrt_abstime now; + + /* + */ + now = hrt_absolute_time(); + + if ((now - dsm_last_rx_time) > 5000) { + if (dsm_partial_frame_count > 0) { + dsm_frame_drops++; + dsm_partial_frame_count = 0; + } + } + + /* + * Fetch bytes, but no more than we would need to complete + * the current dsm frame. + */ + ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count); + + /* if the read failed for any reason, just give up here */ + if (ret < 1) + return false; + + dsm_last_rx_time = now; + + /* + * Add bytes to the current dsm frame + */ + dsm_partial_frame_count += ret; + + /* + * If we don't have a full dsm frame, return + */ + if (dsm_partial_frame_count < DSM_FRAME_SIZE) + return false; + + /* + * Great, it looks like we might have a dsm frame. Go ahead and + * decode it. + */ + dsm_partial_frame_count = 0; + return dsm_decode(now, values, num_values); +} diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 4485daa5b..79b6546b3 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -69,6 +69,7 @@ static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); static void i2c_tx_complete(void); +static void i2c_dump(void); static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; @@ -92,7 +93,7 @@ enum { } direction; void -i2c_init(void) +interface_init(void) { debug("i2c init"); @@ -148,12 +149,12 @@ i2c_init(void) #endif } - /* reset the I2C bus used to recover from lockups */ -void i2c_reset(void) +void +i2c_reset(void) { rCR1 |= I2C_CR1_SWRST; rCR1 = 0; @@ -330,7 +331,7 @@ i2c_tx_complete(void) i2c_tx_setup(); } -void +static void i2c_dump(void) { debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1234e2eea..6a4429461 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -47,6 +47,7 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <systemlib/mixer/mixer.h> extern "C" { @@ -68,13 +69,17 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; +static bool should_arm = false; +static bool should_always_enable_pwm = false; +static volatile bool in_mixer = false; /* selected control values and count for mixing */ enum mixer_source { MIX_NONE, MIX_FMU, MIX_OVERRIDE, - MIX_FAILSAFE + MIX_FAILSAFE, + MIX_OVERRIDE_FMU_OK }; static mixer_source source; @@ -85,9 +90,13 @@ static int mixer_callback(uintptr_t handle, static MixerGroup mixer_group(mixer_callback, 0); +/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ +static void mixer_set_failsafe(); + void mixer_tick(void) { + /* check that we are receiving fresh data from the FMU */ if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { @@ -95,28 +104,31 @@ mixer_tick(void) if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } + /* default to failsafe mixing */ source = MIX_FAILSAFE; /* * Decide which set of controls we're using. */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - /* don't actually mix anything - we already have raw PWM values or - not a valid mixer. */ + /* do not mix if RAW_PWM mode is on and FMU is good */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ @@ -125,77 +137,127 @@ mixer_tick(void) if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; + } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + /* if allowed, mix from RC inputs directly up to available rc channels */ + source = MIX_OVERRIDE_FMU_OK; } } /* + * Set failsafe status flag depending on mixing source + */ + if (source == MIX_FAILSAFE) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; + } else { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); + } + + /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + * XXX correct behaviour for failsafe may require an additional case + * here. + */ + should_arm = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) + ) + ); + + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + + /* * Run the mixers. */ if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; - } else if (source != MIX_NONE) { + /* safe actuators for FMU feedback */ + r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); + } + + + } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_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++) { + /* poor mans mutex */ + in_mixer = true; + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + in_mixer = false; - /* save actuator values for FMU readback */ - r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - /* scale to servo output */ - r_page_servos[i] = (outputs[i] * 500.0f) + 1500; + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) + r_page_servos[i] = 0; + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); } - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) - r_page_servos[i] = 0; } - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - * XXX correct behaviour for failsafe may require an additional case - * here. - */ - bool should_arm = ( - /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* FMU is available or FMU is not available but override is an option */ - ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) - ); + /* set arming */ + bool needs_to_arm = (should_arm || should_always_enable_pwm); + + /* check any conditions that prevent arming */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { + needs_to_arm = false; + } + if (!should_arm && !should_always_enable_pwm) { + needs_to_arm = false; + } - if (should_arm && !mixer_servos_armed) { + if (needs_to_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; + isr_debug(5, "> PWM enabled"); - } else if (!should_arm && mixer_servos_armed) { + } else if (!needs_to_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + isr_debug(5, "> PWM disabled"); } - if (mixer_servos_armed) { + if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + + } else if (mixer_servos_armed && should_always_enable_pwm) { + /* set the disarmed servo outputs. */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servo_disarmed[i]); } } @@ -205,27 +267,38 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group != 0) + if (control_group > 3) return -1; switch (source) { case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS) { - control = REG_TO_FLOAT(r_page_controls[control_index]); + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } return -1; case MIX_OVERRIDE: - if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) { + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } return -1; + case MIX_OVERRIDE_FMU_OK: + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + break; + } + return -1; + case MIX_FAILSAFE: case MIX_NONE: - /* XXX we could allow for configuration of per-output failsafe values */ + control = 0.0f; return -1; } @@ -241,13 +314,17 @@ mixer_callback(uintptr_t handle, static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; -void +int mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { - return; + /* do not allow a mixer change while safety off */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { + return 1; + } + + /* abort if we're in the mixer */ + if (in_mixer) { + return 1; } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -255,7 +332,7 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) - return; + return 0; unsigned text_length = length - sizeof(px4io_mixdata); @@ -273,14 +350,16 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); - /* check for overflow - this is really fatal */ - /* XXX could add just what will fit & try to parse, then repeat... */ + /* disable mixing during the update */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + + /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - return; + return 0; } - /* append mixer text and nul-terminate */ + /* append mixer text and nul-terminate, guard against overflow */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; @@ -293,8 +372,13 @@ mixer_handle_text(const void *buffer, size_t length) /* if anything was parsed */ if (resid != mixer_text_length) { - /* ideally, this should test resid == 0 ? */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* only set mixer ok if no residual is left over */ + if (resid == 0) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + } else { + /* not yet reached the end of the mixer, set as not ok */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + } isr_debug(2, "used %u", mixer_text_length - resid); @@ -303,8 +387,46 @@ mixer_handle_text(const void *buffer, size_t length) memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); mixer_text_length = resid; + + /* update failsafe values */ + mixer_set_failsafe(); } break; } + + return 0; +} + +static void +mixer_set_failsafe() +{ + /* + * Check if a custom failsafe value has been written, + * or if the mixer is not ok and bail out. + */ + + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + return; + + /* set failsafe defaults to the values for all inputs = 0 */ + float outputs[PX4IO_SERVO_COUNT]; + unsigned mixed; + + /* mix */ + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + + /* scale to PWM and update the servo outputs as required */ + for (unsigned i = 0; i < mixed; i++) { + + /* scale to servo output */ + r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500; + + } + + /* disable the rest of the outputs */ + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) + r_page_servo_failsafe[i] = 0; + } diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 6379366f4..01869569f 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -3,17 +3,23 @@ SRCS = adc.c \ controls.c \ dsm.c \ - i2c.c \ px4io.c \ registers.c \ safety.c \ sbus.c \ ../systemlib/up_cxxinitialize.c \ - ../systemlib/hx_stream.c \ ../systemlib/perf_counter.c \ mixer.cpp \ ../systemlib/mixer/mixer.cpp \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ -
\ No newline at end of file + ../systemlib/pwm_limit/pwm_limit.c + +ifeq ($(BOARD),px4io-v1) +SRCS += i2c.c +endif +ifeq ($(BOARD),px4io-v2) +SRCS += serial.c \ + ../systemlib/hx_stream.c +endif diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e575bd841..d48c6c529 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +36,7 @@ /** * @file protocol.h * - * PX4IO I2C interface protocol. + * PX4IO interface protocol. * * Communication is performed via writes to and reads from 16-bit virtual * registers organised into pages of 255 registers each. @@ -45,7 +45,7 @@ * respectively. Subsequent reads and writes increment the offset within * the page. * - * Most pages are readable or writable but not both. + * Some pages are read- or write-only. * * Note that some pages may permit offset values greater than 255, which * can only be achieved by long writes. The offset does not wrap. @@ -62,12 +62,11 @@ * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be * packed. + * + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. */ -#define PX4IO_CONTROL_CHANNELS 8 -#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)) @@ -75,17 +74,23 @@ #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)) +#define PX4IO_PROTOCOL_VERSION 4 + +/* maximum allowable sizes on this protocol version */ +#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ #define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ #define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ -#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ +#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT 8 /**< hardcoded # of control groups*/ /* dynamic status page */ #define PX4IO_PAGE_STATUS 1 @@ -93,7 +98,7 @@ #define PX4IO_P_STATUS_CPULOAD 1 #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ -#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ #define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ @@ -104,18 +109,24 @@ #define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ +#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #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_VBATT_LOW (1 << 0) /* [1] 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_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ #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) /* timed out waiting for RC input */ #define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ -#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ -#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */ +#define PX4IO_P_STATUS_VBATT 4 /* [1] battery voltage in mV */ +#define PX4IO_P_STATUS_IBATT 5 /* [1] battery current (raw ADC) */ +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -126,7 +137,17 @@ /* array of raw RC input values, microseconds */ #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 */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ /* array of scaled RC input values, -10000..10000 */ #define PX4IO_PAGE_RC_INPUT 5 @@ -141,45 +162,99 @@ #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 100 +#define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ #define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ + +#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ -#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ -#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ +#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */ +#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ +#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ +#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ +#endif + +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; + /* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ + +#define PX4IO_P_CONTROLS_GROUP_VALID 64 +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 102 +#define PX4IO_PAGE_MIXERLOAD 52 /* 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_PAGE_RC_CONFIG 53 /**< 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 */ +#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 */ +#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ + +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. @@ -200,3 +275,81 @@ struct px4io_mixdata { }; #pragma pack(pop) +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = +{ + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) + c = crc8_tab[c ^ *(p++)]; + + return c; +} diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index bc8dfc116..d4c25911e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,11 +45,13 @@ #include <string.h> #include <poll.h> #include <signal.h> +#include <crc32.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <stm32_uart.h> @@ -64,10 +66,7 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -#ifdef CONFIG_STM32_I2C1 -/* store i2c reset count XXX this should be a register, together with other error counters */ -volatile uint32_t i2c_loop_resets = 0; -#endif +pwm_limit_t pwm_limit; /* * a set of debug buffers to allow us to send debug information from ISRs @@ -119,6 +118,48 @@ show_debug_messages(void) } } +static void +heartbeat_blink(void) +{ + static bool heartbeat = false; + LED_BLUE(heartbeat = !heartbeat); +} + +static uint64_t reboot_time; + +/** + schedule a reboot in time_delta_usec microseconds + */ +void schedule_reboot(uint32_t time_delta_usec) +{ + reboot_time = hrt_absolute_time() + time_delta_usec; +} + +/** + check for a scheduled reboot + */ +static void check_reboot(void) +{ + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } +} + +static void +calculate_fw_crc(void) +{ +#define APP_SIZE_MAX 0xf000 +#define APP_LOAD_ADDRESS 0x08001000 + // compute CRC of the current firmware + uint32_t sum = 0; + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { + uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); + sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); + } + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; + r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16; +} + int user_start(int argc, char *argv[]) { @@ -131,6 +172,9 @@ user_start(int argc, char *argv[]) /* configure the high-resolution time/callout interface */ hrt_init(); + /* calculate our fw CRC so FMU can decide if we need to update */ + calculate_fw_crc(); + /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. @@ -147,8 +191,15 @@ user_start(int argc, char *argv[]) LED_BLUE(false); LED_SAFETY(false); - /* turn on servo power */ + /* turn on servo power (if supported) */ +#ifdef POWER_SERVO POWER_SERVO(true); +#endif + + /* turn off S.Bus out (if supported) */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(false); +#endif /* start the safety switch handler */ safety_init(); @@ -159,10 +210,11 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); -#ifdef CONFIG_STM32_I2C1 - /* start the i2c handler */ - i2c_init(); -#endif + /* set up the ADC */ + adc_init(); + + /* start the FMU interface */ + interface_init(); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -176,30 +228,51 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); -#if 0 - /* not enough memory, lock down */ - if (minfo.mxordblk < 500) { + /* initialize PWM limit lib */ + pwm_limit_init(&pwm_limit); + + /* + * P O L I C E L I G H T S + * + * Not enough memory, lock down. + * + * We might need to allocate mixers later, and this will + * ensure that a developer doing a change will notice + * that he just burned the remaining RAM with static + * allocations. We don't want him to be able to + * get past that point. This needs to be clearly + * documented in the dev guide. + * + */ + if (minfo.mxordblk < 600) { + lowsyslog("ERR: not enough MEM"); bool phase = false; - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - } else { - LED_AMBER(false); - LED_BLUE(true); - } + while (true) { - phase = !phase; - usleep(300000); + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + up_udelay(250000); + + phase = !phase; + } } -#endif + + /* Start the failsafe led init */ + failsafe_led_init(); /* * Run everything in a tight loop. */ uint64_t last_debug_time = 0; + uint64_t last_heartbeat_time = 0; for (;;) { /* track the rate at which the loop is running */ @@ -215,20 +288,28 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); - /* check for debug activity */ + if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } + + check_reboot(); + + /* check for debug activity (default: none) */ show_debug_messages(); - /* post debug state at ~1Hz */ + /* post debug state at ~1Hz - this is via an auxiliary serial port + * DEFAULTS TO OFF! + */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets, (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf..bb224f388 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,15 +42,20 @@ #include <stdbool.h> #include <stdint.h> -#include <drivers/boards/px4io/px4io_internal.h> +#include <board_config.h> #include "protocol.h" +#include <systemlib/pwm_limit/pwm_limit.h> + /* * Constants and limits. */ -#define MAX_CONTROL_CHANNELS 12 -#define IO_SERVO_COUNT 8 +#define PX4IO_SERVO_COUNT 8 +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_CONTROL_GROUPS 2 +#define PX4IO_RC_INPUT_CHANNELS 18 +#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ /* * Debug logging @@ -77,6 +82,9 @@ 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 */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ +extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ +extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ +extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ /* * Register aliases. @@ -88,15 +96,18 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ #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_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] #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_rc_values (&r_page_rc_input[PX4IO_P_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_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] #define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] +#endif #define r_control_values (&r_page_controls[0]) @@ -105,7 +116,8 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ */ struct sys_state_s { - volatile uint64_t rc_channels_timestamp; + volatile uint64_t rc_channels_timestamp_received; + volatile uint64_t rc_channels_timestamp_valid; /** * Last FMU receive time, in microseconds since system boot @@ -117,56 +129,75 @@ struct sys_state_s { extern struct sys_state_s system_state; /* + * PWM limit structure + */ +extern pwm_limit_t pwm_limit; + +/* * GPIO handling. */ -#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) -#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) -#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) +#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) +#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) +#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + +# define PX4IO_RELAY_CHANNELS 4 +# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) +# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +# define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) +# define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VBATT 4 +# define ADC_IN5 5 -#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -#ifdef GPIO_ACC1_PWR_EN - #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) -#endif -#ifdef GPIO_ACC2_PWR_EN - #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) -#endif -#ifdef GPIO_RELAY1_EN - #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #endif -#ifdef GPIO_RELAY2_EN - #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + +# define PX4IO_RELAY_CHANNELS 0 +# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) + +# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VSERVO 4 +# define ADC_RSSI 5 + #endif -#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) -#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) -#define ADC_VBATT 4 -#define ADC_IN5 5 -#define ADC_CHANNEL_COUNT 2 +#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel) /* * Mixer */ extern void mixer_tick(void); -extern void mixer_handle_text(const void *buffer, size_t length); +extern int mixer_handle_text(const void *buffer, size_t length); /** * Safety switch/LED. */ extern void safety_init(void); +extern void failsafe_led_init(void); -#ifdef CONFIG_STM32_I2C1 /** * FMU communications */ -extern void i2c_init(void); -#endif +extern void interface_init(void); +extern void interface_tick(void); /** * Register space */ -extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); +extern int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values); /** @@ -184,16 +215,16 @@ extern void controls_init(void); extern void controls_tick(void); extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); +extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; -/* send a debug message to the console */ -extern void isr_debug(uint8_t level, const char *fmt, ...); +/** send a debug message to the console */ +extern void isr_debug(uint8_t level, const char *fmt, ...); + +/** schedule a reboot */ +extern void schedule_reboot(uint32_t time_delta_usec); -#ifdef CONFIG_STM32_I2C1 -void i2c_dump(void); -void i2c_reset(void); -#endif diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9698a0f2f..97d25bbfa 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,8 +41,12 @@ #include <stdbool.h> #include <stdlib.h> +#include <string.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> +#include <systemlib/systemlib.h> +#include <stm32_pwr.h> #include "px4io.h" #include "protocol.h" @@ -56,14 +60,18 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION, +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2, +#else + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 1, +#endif [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT, - [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT, + [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS, + [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, }; @@ -78,7 +86,10 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_FLAGS] = 0, [PX4IO_P_STATUS_ALARMS] = 0, [PX4IO_P_STATUS_VBATT] = 0, - [PX4IO_P_STATUS_IBATT] = 0 + [PX4IO_P_STATUS_IBATT] = 0, + [PX4IO_P_STATUS_VSERVO] = 0, + [PX4IO_P_STATUS_VRSSI] = 0, + [PX4IO_P_STATUS_PRSSI] = 0, }; /** @@ -86,14 +97,14 @@ uint16_t r_page_status[] = { * * Post-mixed actuator values. */ -uint16_t r_page_actuators[IO_SERVO_COUNT]; +uint16_t r_page_actuators[PX4IO_SERVO_COUNT]; /** * PAGE 3 * * Servo PWM values */ -uint16_t r_page_servos[IO_SERVO_COUNT]; +uint16_t r_page_servos[PX4IO_SERVO_COUNT]; /** * PAGE 4 @@ -103,7 +114,13 @@ uint16_t r_page_servos[IO_SERVO_COUNT]; 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 + [PX4IO_P_RAW_RC_FLAGS] = 0, + [PX4IO_P_RAW_RC_NRSSI] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, + [PX4IO_P_RAW_FRAME_COUNT] = 0, + [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; /** @@ -113,7 +130,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 }; /** @@ -131,22 +148,46 @@ uint16_t r_page_scratch[32]; */ volatile uint16_t r_page_setup[] = { +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + /* default to RSSI ADC functionality */ + [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, +#else [PX4IO_P_SETUP_FEATURES] = 0, +#endif [PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 [PX4IO_P_SETUP_RELAYS] = 0, +#endif +#ifdef ADC_VSERVO + [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, +#else [PX4IO_P_SETUP_VBATT_SCALE] = 10000, +#endif [PX4IO_P_SETUP_SET_DEBUG] = 0, + [PX4IO_P_SETUP_REBOOT_BL] = 0, + [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, }; -#define PX4IO_P_SETUP_FEATURES_VALID (0) +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 +#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ + PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ + PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ + PX4IO_P_SETUP_FEATURES_PWM_RSSI) +#else +#define PX4IO_P_SETUP_FEATURES_VALID 0 +#endif #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK) -#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) + PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ + PX4IO_P_SETUP_ARMING_LOCKDOWN) +#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) /** @@ -154,7 +195,7 @@ volatile uint16_t r_page_setup[] = * * Control values from the FMU. */ -volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; +volatile uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS]; /* * PAGE 102 does not have a buffer. @@ -165,7 +206,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; * * R/C channel input configuration. */ -uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; /* valid options */ #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) @@ -178,10 +219,36 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE * PAGE 105 * * Failsafe servo PWM values + * + * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; -void +/** + * PAGE 106 + * + * minimum PWM values when armed + * + */ +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN }; + +/** + * PAGE 107 + * + * maximum PWM values when armed + * + */ +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX }; + +/** + * PAGE 108 + * + * disarmed PWM values for difficult ESCs + * + */ +uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + +int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -191,7 +258,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_CONTROLS: /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ r_page_controls[offset] = *values; @@ -230,20 +297,112 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* ignore 0 */ + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; + } else { + r_page_servo_failsafe[offset] = *values; + } - /* XXX range-check value? */ - r_page_servo_failsafe[offset] = *values; + /* flag the failsafe values as custom */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_CONTROL_MIN_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* ignore 0 */ + } else if (*values > PWM_HIGHEST_MIN) { + r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_control_min[offset] = PWM_LOWEST_MIN; + } else { + r_page_servo_control_min[offset] = *values; + } offset++; num_values--; values++; } break; + + case PX4IO_PAGE_CONTROL_MAX_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* ignore 0 */ + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; + } else if (*values < PWM_LOWEST_MAX) { + r_page_servo_control_max[offset] = PWM_LOWEST_MAX; + } else { + r_page_servo_control_max[offset] = *values; + } + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_DISARMED_PWM: + { + /* flag for all outputs */ + bool all_disarmed_off = true; + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* 0 means disabling always PWM */ + r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; + all_disarmed_off = false; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; + all_disarmed_off = false; + } else { + r_page_servo_disarmed[offset] = *values; + all_disarmed_off = false; + } + + offset++; + num_values--; + values++; + } + + if (all_disarmed_off) { + /* disable PWM output if disarmed */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); + } else { + /* enable PWM output always */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + } + } + break; /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - mixer_handle_text(values, num_values * sizeof(*values)); + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + return mixer_handle_text(values, num_values * sizeof(*values)); + } break; default: @@ -254,11 +413,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* iterate individual registers, set each in turn */ while (num_values--) { if (registers_set_one(page, offset, *values)) - break; + return -1; offset++; values++; } + break; } + return 0; } static int @@ -294,9 +455,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FEATURES: value &= PX4IO_P_SETUP_FEATURES_VALID; - r_setup_features = value; - /* no implemented feature selection at this point */ + /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ + + /* switch S.Bus output pin as needed */ + #ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); + + /* disable the conflicting options */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } + #endif + + /* disable the conflicting options with ADC RSSI */ + if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ + if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* apply changes */ + r_setup_features = value; break; @@ -311,8 +498,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + + if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } r_setup_arming = value; @@ -340,13 +528,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; 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); - POWER_ACC2(value & (1 << 3) ? 1 : 0); + POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); + POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); + POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); + POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); + break; +#endif + + case PX4IO_P_SETUP_VBATT_SCALE: + r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; break; case PX4IO_P_SETUP_SET_DEBUG: @@ -354,6 +548,27 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; + case PX4IO_P_SETUP_REBOOT_BL: + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + // don't allow reboot while armed + break; + } + + // check the magic value + if (value != PX4IO_REBOOT_BL_MAGIC) + break; + + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); + break; + + case PX4IO_P_SETUP_DSM: + dsm_bind(value & 0x0f, (value >> 4) & 0xF); + break; + default: return -1; } @@ -361,9 +576,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /** + * do not allow a RC config change while outputs armed + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } @@ -371,7 +588,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - if (channel >= MAX_CONTROL_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ @@ -391,6 +608,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + /* clear any existing RC disabled flag */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); + /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; @@ -398,6 +618,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* this option is normally set last */ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { uint8_t count = 0; + bool disabled = false; /* assert min..center..max ordering */ if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { @@ -416,7 +637,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { + disabled = true; + } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } @@ -424,7 +648,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (count) { isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; - } else { + } else if (!disabled) { conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } } @@ -436,6 +660,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* case PX4IO_RC_PAGE_CONFIG */ } + case PX4IO_PAGE_TEST: + switch (offset) { + case PX4IO_P_TEST_LED: + LED_AMBER(value & 1); + break; + } + break; + default: return -1; } @@ -472,6 +704,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_ALARMS maintained externally */ +#ifdef ADC_VBATT /* PX4IO_P_STATUS_VBATT */ { /* @@ -505,7 +738,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val r_page_status[PX4IO_P_STATUS_VBATT] = corrected; } } - +#endif +#ifdef ADC_IBATT /* PX4IO_P_STATUS_IBATT */ { /* @@ -515,26 +749,60 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val FMU sort it out, with user selectable configuration for their sensor */ - unsigned counts = adc_measure(ADC_IN5); + unsigned counts = adc_measure(ADC_IBATT); if (counts != 0xffff) { r_page_status[PX4IO_P_STATUS_IBATT] = counts; } } +#endif +#ifdef ADC_VSERVO + /* PX4IO_P_STATUS_VSERVO */ + { + unsigned counts = adc_measure(ADC_VSERVO); + if (counts != 0xffff) { + // use 3:1 scaling on 3.3V ADC input + unsigned mV = counts * 9900 / 4096; + r_page_status[PX4IO_P_STATUS_VSERVO] = mV; + } + } +#endif +#ifdef ADC_RSSI + /* PX4IO_P_STATUS_VRSSI */ + { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + // use 1:1 scaling on 3.3V ADC input + unsigned mV = counts * 3300 / 4096; + r_page_status[PX4IO_P_STATUS_VRSSI] = mV; + } + } +#endif + /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); break; case PX4IO_PAGE_RAW_ADC_INPUT: memset(r_page_scratch, 0, sizeof(r_page_scratch)); +#ifdef ADC_VBATT r_page_scratch[0] = adc_measure(ADC_VBATT); - r_page_scratch[1] = adc_measure(ADC_IN5); - +#endif +#ifdef ADC_IBATT + r_page_scratch[1] = adc_measure(ADC_IBATT); +#endif + +#ifdef ADC_VSERVO + r_page_scratch[0] = adc_measure(ADC_VSERVO); +#endif +#ifdef ADC_RSSI + r_page_scratch[1] = adc_measure(ADC_RSSI); +#endif SELECT_PAGE(r_page_scratch); break; case PX4IO_PAGE_PWM_INFO: memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); SELECT_PAGE(r_page_scratch); @@ -577,6 +845,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + SELECT_PAGE(r_page_servo_control_min); + break; + case PX4IO_PAGE_CONTROL_MAX_PWM: + SELECT_PAGE(r_page_servo_control_max); + break; + case PX4IO_PAGE_DISARMED_PWM: + SELECT_PAGE(r_page_servo_disarmed); + break; default: return -1; @@ -606,7 +883,7 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) { for (unsigned pass = 0; pass < 2; pass++) { - for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { + for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) { /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 4dbecc274..ff2e4af6e 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -45,7 +45,6 @@ #include "px4io.h" static struct hrt_call arming_call; -static struct hrt_call heartbeat_call; static struct hrt_call failsafe_call; /* @@ -77,7 +76,6 @@ static unsigned blink_counter = 0; static bool safety_button_pressed; static void safety_check_button(void *arg); -static void heartbeat_blink(void *arg); static void failsafe_blink(void *arg); void @@ -85,10 +83,11 @@ safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); +} - /* arrange for the heartbeat handler to be called at 4Hz */ - hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL); - +void +failsafe_led_init(void) +{ /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -110,7 +109,7 @@ 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 && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -118,18 +117,18 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } @@ -140,7 +139,7 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; - if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_IO_FMU_ARMED; @@ -164,23 +163,13 @@ safety_check_button(void *arg) } static void -heartbeat_blink(void *arg) -{ - static bool heartbeat = false; - - /* XXX add flags here that need to be frobbed by various loops */ - - LED_BLUE(heartbeat = !heartbeat); -} - -static void failsafe_blink(void *arg) { /* indicate that a serious initialisation error occured */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { LED_AMBER(true); - return; - } + return; + } static bool failsafe = false; @@ -192,4 +181,4 @@ failsafe_blink(void *arg) } LED_AMBER(failsafe); -}
\ No newline at end of file +} diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 073ddeaba..f6ec542eb 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -54,6 +54,27 @@ #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 +#define SBUS_FLAGS_BYTE 23 +#define SBUS_FAILSAFE_BIT 3 +#define SBUS_FRAMELOST_BIT 2 + +/* + Measured values with Futaba FX-30/R6108SB: + -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV) + -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) + -+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks) +*/ + +/* define range mapping here, -+100% -> 1000..2000 */ +#define SBUS_RANGE_MIN 200.0f +#define SBUS_RANGE_MAX 1800.0f + +#define SBUS_TARGET_MIN 1000.0f +#define SBUS_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) +#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) static int sbus_fd = -1; @@ -66,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) @@ -97,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values) +sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -154,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values) * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values); + return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); } /* @@ -194,28 +215,41 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { + if ((frame[0] != 0x0f)) { sbus_frame_drops++; return false; } - /* if the failsafe or connection lost bit is set, we consider the frame invalid */ - if ((frame[23] & (1 << 2)) && /* signal lost */ - (frame[23] & (1 << 3))) { /* failsafe */ - - /* actively announce signal loss */ - *values = 0; - return false; + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + break; + case 0x03: + /* S.BUS 2 SLOT0: RX battery and external voltage */ + break; + case 0x83: + /* S.BUS 2 SLOT1 */ + break; + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: + break; + default: + /* we expect one of the bits above, but there are some we don't know yet */ + break; } /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_values; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -234,22 +268,43 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) } } - /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - values[channel] = (value / 2) + 998; + + /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; } /* decode switch channels if data fields are wide enough */ - if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) { + if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) { chancount = 18; /* channel 17 (index 16) */ - values[16] = (frame[23] & (1 << 0)) * 1000 + 998; + values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ - values[17] = (frame[23] & (1 << 1)) * 1000 + 998; + values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ *num_values = chancount; + /* decode and handle failsafe and frame-lost flags */ + if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ + /* report that we failed to read anything valid off the receiver */ + *sbus_failsafe = true; + *sbus_frame_drop = true; + } + else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + /* set a special warning flag + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, + * e.g. by prematurely issueing return-to-launch!!! */ + + *sbus_failsafe = false; + *sbus_frame_drop = true; + } else { + *sbus_failsafe = false; + *sbus_frame_drop = false; + } + return true; } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c new file mode 100644 index 000000000..e9adc8cd6 --- /dev/null +++ b/src/modules/px4iofirmware/serial.c @@ -0,0 +1,338 @@ +/**************************************************************************** + * + * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file serial.c + * + * Serial communication for the PX4IO module. + */ + +#include <stdint.h> +#include <unistd.h> +#include <termios.h> +#include <fcntl.h> +#include <string.h> + +#include <nuttx/arch.h> +#include <arch/board/board.h> + +/* XXX might be able to prune these */ +#include <chip.h> +#include <up_internal.h> +#include <up_arch.h> +#include <stm32.h> +#include <systemlib/perf_counter.h> + +//#define DEBUG +#include "px4io.h" + +static perf_counter_t pc_txns; +static perf_counter_t pc_errors; +static perf_counter_t pc_ore; +static perf_counter_t pc_fe; +static perf_counter_t pc_ne; +static perf_counter_t pc_idle; +static perf_counter_t pc_badidle; +static perf_counter_t pc_regerr; +static perf_counter_t pc_crcerr; + +static void rx_handle_packet(void); +static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static DMA_HANDLE tx_dma; +static DMA_HANDLE rx_dma; + +static int serial_interrupt(int irq, void *context); +static void dma_reset(void); + +static struct IOPacket dma_packet; + +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + +void +interface_init(void) +{ + pc_txns = perf_alloc(PC_ELAPSED, "txns"); + pc_errors = perf_alloc(PC_COUNT, "errors"); + pc_ore = perf_alloc(PC_COUNT, "overrun"); + pc_fe = perf_alloc(PC_COUNT, "framing"); + pc_ne = perf_alloc(PC_COUNT, "noise"); + pc_idle = perf_alloc(PC_COUNT, "idle"); + pc_badidle = perf_alloc(PC_COUNT, "badidle"); + pc_regerr = perf_alloc(PC_COUNT, "regerr"); + pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); + + /* allocate DMA */ + tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); + rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); + + /* configure pins for serial use */ + stm32_configgpio(PX4FMU_SERIAL_TX_GPIO); + stm32_configgpio(PX4FMU_SERIAL_RX_GPIO); + + /* reset and configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* clear status/errors */ + (void)rSR; + (void)rDR; + + /* configure line speed */ + uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* connect our interrupt */ + irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt); + up_enable_irq(PX4FMU_SERIAL_VECTOR); + + /* enable UART and error/idle interrupts */ + rCR3 = USART_CR3_EIE; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; + +#if 0 /* keep this for signal integrity testing */ + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xfa; + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xa0; + } +#endif + + /* configure RX DMA and return to listening state */ + dma_reset(); + + debug("serial init"); +} + +static void +rx_handle_packet(void) +{ + /* check packet CRC */ + uint8_t crc = dma_packet.crc; + dma_packet.crc = 0; + if (crc != crc_packet(&dma_packet)) { + perf_count(pc_crcerr); + + /* send a CRC error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xff; + + return; + } + + if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { + + /* it's a blind write - pass it on */ + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { + perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + dma_packet.count_code = PKT_CODE_SUCCESS; + } + return; + } + + if (PKT_CODE(dma_packet) == PKT_CODE_READ) { + + /* it's a read - get register pointer for reply */ + unsigned count; + uint16_t *registers; + + if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { + perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + /* constrain reply to requested size */ + if (count > PKT_MAX_REGS) + count = PKT_MAX_REGS; + if (count > PKT_COUNT(dma_packet)) + count = PKT_COUNT(dma_packet); + + /* copy reply registers into DMA buffer */ + memcpy((void *)&dma_packet.regs[0], registers, count * 2); + dma_packet.count_code = count | PKT_CODE_SUCCESS; + } + return; + } + + /* send a bad-packet error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xfe; +} + +static void +rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + /* + * We are here because DMA completed, or UART reception stopped and + * we think we have a packet in the buffer. + */ + perf_begin(pc_txns); + + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + /* handle the received packet */ + rx_handle_packet(); + + /* re-set DMA for reception first, so we are ready to receive before we start sending */ + dma_reset(); + + /* send the reply to the just-processed request */ + dma_packet.crc = 0; + dma_packet.crc = crc_packet(&dma_packet); + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + PKT_SIZE(dma_packet), + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + stm32_dmastart(tx_dma, NULL, NULL, false); + rCR3 |= USART_CR3_DMAT; + + perf_end(pc_txns); +} + +static int +serial_interrupt(int irq, void *context) +{ + static bool abort_on_idle = false; + + uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* required to clear any of the interrupt status that brought us here */ + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + perf_count(pc_errors); + if (sr & USART_SR_ORE) + perf_count(pc_ore); + if (sr & USART_SR_NE) + perf_count(pc_ne); + if (sr & USART_SR_FE) + perf_count(pc_fe); + + /* send a line break - this will abort transmission/reception on the other end */ + rCR1 |= USART_CR1_SBK; + + /* when the line goes idle, abort rather than look at the packet */ + abort_on_idle = true; + } + + if (sr & USART_SR_IDLE) { + + /* + * If we saw an error, don't bother looking at the packet - it should have + * been aborted by the sender and will definitely be bad. Get the DMA reconfigured + * ready for their retry. + */ + if (abort_on_idle) { + + abort_on_idle = false; + dma_reset(); + return 0; + } + + /* + * The sender has stopped sending - this is probably the end of a packet. + * Check the received length against the length in the header to see if + * we have something that looks like a packet. + */ + unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { + + /* it was too short - possibly truncated */ + perf_count(pc_badidle); + dma_reset(); + return 0; + } + + /* + * Looks like we received a packet. Stop the DMA and go process the + * packet. + */ + perf_count(pc_idle); + stm32_dmastop(rx_dma); + rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); + } + + return 0; +} + +static void +dma_reset(void) +{ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + (void)rSR; + (void)rDR; + (void)rDR; + + /* kill any pending DMA */ + stm32_dmastop(tx_dma); + stm32_dmastop(rx_dma); + + /* reset the RX side */ + stm32_dmasetup( + rx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIVERYHI); + + /* start receive DMA ready for the next packet */ + stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAR; +} + |