aboutsummaryrefslogtreecommitdiff
path: root/src/modules/px4iofirmware
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r--src/modules/px4iofirmware/controls.c79
-rw-r--r--src/modules/px4iofirmware/dsm.c10
-rw-r--r--src/modules/px4iofirmware/mixer.cpp69
-rw-r--r--src/modules/px4iofirmware/module.mk3
-rw-r--r--src/modules/px4iofirmware/protocol.h7
-rw-r--r--src/modules/px4iofirmware/px4io.c6
-rw-r--r--src/modules/px4iofirmware/px4io.h3
-rw-r--r--src/modules/px4iofirmware/registers.c31
-rw-r--r--src/modules/px4iofirmware/sbus.c95
9 files changed, 223 insertions, 80 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 185cb20dd..ad60ee03e 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -43,6 +43,7 @@
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/ppm_decode.h>
+#include <rc/st24.h>
#include "px4io.h"
@@ -51,11 +52,62 @@
#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
+static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
static perf_counter_t c_gather_ppm;
+static int _dsm_fd;
+
+static uint16_t rc_value_override = 0;
+
+bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
+{
+ perf_begin(c_gather_dsm);
+ uint16_t temp_count = r_raw_rc_count;
+ uint8_t n_bytes = 0;
+ uint8_t *bytes;
+ *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
+ 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);
+
+ /* get data from FD and attempt to parse with DSM and ST24 libs */
+ uint8_t st24_rssi, rx_count;
+ uint16_t st24_channel_count = 0;
+
+ *st24_updated = false;
+
+ for (unsigned i = 0; i < n_bytes; i++) {
+ /* set updated flag if one complete packet was parsed */
+ *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
+ &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
+ }
+
+ if (*st24_updated) {
+
+ *rssi = st24_rssi;
+ r_raw_rc_count = st24_channel_count;
+
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+ }
+
+ return (*dsm_updated | *st24_updated);
+}
+
void
controls_init(void)
{
@@ -65,7 +117,7 @@ controls_init(void)
system_state.rc_channels_timestamp_valid = 0;
/* DSM input (USART1) */
- dsm_init("/dev/ttyS0");
+ _dsm_fd = dsm_init("/dev/ttyS0");
/* S.bus input (USART3) */
sbus_init("/dev/ttyS2");
@@ -116,20 +168,8 @@ controls_tick() {
#endif
perf_begin(c_gather_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);
-
- }
+ bool dsm_updated, st24_updated;
+ (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
@@ -191,7 +231,7 @@ controls_tick() {
/*
* If we received a new frame from any of the RC sources, process it.
*/
- if (dsm_updated || sbus_updated || ppm_updated) {
+ if (dsm_updated || sbus_updated || ppm_updated || st24_updated) {
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
@@ -278,6 +318,9 @@ controls_tick() {
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
+ } else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) {
+ /* pick out override channel, indicated by special mapping */
+ rc_value_override = SIGNED_TO_REG(scaled);
}
}
}
@@ -371,7 +414,7 @@ controls_tick() {
* requested override.
*
*/
- if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
override = true;
if (override) {
@@ -379,7 +422,7 @@ controls_tick() {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
/* mix new RC input control values to servos */
- if (dsm_updated || sbus_updated || ppm_updated)
+ if (dsm_updated || sbus_updated || ppm_updated || st24_updated)
mixer_tick();
} else {
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index d3f365822..6e57c9ec7 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -452,10 +452,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
*
* @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
+ * @param[out] n_butes number of bytes read
+ * @param[out] bytes pointer to the buffer of read bytes
* @return true=decoded raw channel values updated, false=no update
*/
bool
-dsm_input(uint16_t *values, uint16_t *num_values)
+dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes)
{
ssize_t ret;
hrt_abstime now;
@@ -478,8 +480,12 @@ dsm_input(uint16_t *values, uint16_t *num_values)
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)
+ if (ret < 1) {
return false;
+ } else {
+ *n_bytes = ret;
+ *bytes = &dsm_frame[dsm_partial_frame_count];
+ }
dsm_last_rx_time = now;
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 606c639f9..c0b8ac358 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -58,14 +58,7 @@ extern "C" {
/*
* Maximum interval in us before FMU signal is considered lost
*/
-#define FMU_INPUT_DROP_LIMIT_US 200000
-
-/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
-#define ROLL 0
-#define PITCH 1
-#define YAW 2
-#define THROTTLE 3
-#define OVERRIDE 4
+#define FMU_INPUT_DROP_LIMIT_US 500000
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
@@ -98,7 +91,8 @@ 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) {
+ if ((system_state.fmu_data_received_time == 0) ||
+ hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too long without FMU input, time to go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
@@ -109,6 +103,9 @@ mixer_tick(void)
} else {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
+
+ /* this flag is never cleared once OK */
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
}
/* default to failsafe mixing - it will be forced below if flag is set */
@@ -139,7 +136,9 @@ mixer_tick(void)
(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)) {
+ !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+ /* do not enter manual override if we asked for termination failsafe and FMU is lost */
+ !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
@@ -155,29 +154,11 @@ mixer_tick(void)
}
/*
- * Check if we should force failsafe - and do it if we have to
- */
- if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
- source = MIX_FAILSAFE;
- }
-
- /*
- * 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)
@@ -195,6 +176,38 @@ mixer_tick(void)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
/*
+ * Check if failsafe termination is set - if yes,
+ * set the force failsafe flag once entering the first
+ * failsafe condition.
+ */
+ if ( /* if we have requested flight termination style failsafe (noreturn) */
+ (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
+ /* and we ended up in a failsafe condition */
+ (source == MIX_FAILSAFE) &&
+ /* and we should be armed, so we intended to provide outputs */
+ should_arm &&
+ /* and FMU is initialized */
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) {
+ r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+ }
+
+ /*
+ * Check if we should force failsafe - and do it if we have to
+ */
+ if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
+ source = MIX_FAILSAFE;
+ }
+
+ /*
+ * 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);
+ }
+
+ /*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 01869569f..eb99e8a96 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -14,7 +14,8 @@ SRCS = adc.c \
../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \
../systemlib/mixer/mixer_simple.cpp \
- ../systemlib/pwm_limit/pwm_limit.c
+ ../systemlib/pwm_limit/pwm_limit.c \
+ ../../lib/rc/st24.c
ifeq ($(BOARD),px4io-v1)
SRCS += i2c.c
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 050783687..9b2e047cb 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013 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
@@ -111,6 +111,8 @@
#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_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */
+#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -180,6 +182,7 @@
#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_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */
+#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
#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 */
@@ -218,6 +221,7 @@ enum { /* DSM bind states */
hence index 12 can safely be used. */
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
+#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */
#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
/* autopilot control values, -10000..10000 */
@@ -243,6 +247,7 @@ enum { /* DSM bind states */
#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_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */
#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)
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index cd134ccb4..30f32b38e 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -124,6 +124,9 @@ heartbeat_blink(void)
{
static bool heartbeat = false;
LED_BLUE(heartbeat = !heartbeat);
+#ifdef GPIO_LED4
+ LED_RING(heartbeat);
+#endif
}
static uint64_t reboot_time;
@@ -191,6 +194,9 @@ user_start(int argc, char *argv[])
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
+#ifdef GPIO_LED4
+ LED_RING(false);
+#endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index b00a96717..8186e4c78 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -140,6 +140,7 @@ extern pwm_limit_t pwm_limit;
#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_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s))
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
@@ -215,7 +216,7 @@ extern uint16_t adc_measure(unsigned channel);
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 bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes);
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, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 43161aa70..49c2a9f56 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013 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
@@ -190,7 +190,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
- PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
+ PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
+ PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -285,7 +286,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
- r_page_servos[offset] = *values;
+ if (*values != PWM_IGNORE_THIS_CHANNEL) {
+ r_page_servos[offset] = *values;
+ }
offset++;
num_values--;
@@ -516,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}
+ /*
+ * If the failsafe termination flag is set, do not allow the autopilot to unset it
+ */
+ value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
+
+ /*
+ * If failsafe termination is enabled and force failsafe bit is set, do not allow
+ * the autopilot to clear it.
+ */
+ if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) {
+ value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
+ }
+
r_setup_arming = value;
break;
@@ -587,6 +603,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
dsm_bind(value & 0x0f, (value >> 4) & 0xF);
break;
+ case PX4IO_P_SETUP_FORCE_SAFETY_ON:
+ if (value == PX4IO_FORCE_SAFETY_MAGIC) {
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+ }
+ break;
+
case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
if (value == PX4IO_FORCE_SAFETY_MAGIC) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
@@ -670,7 +692,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
- } else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
+ } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
+ (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
count++;
}
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 0f0414148..d76ec55f0 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -57,11 +57,12 @@
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
+#define SBUS1_FRAME_DELAY 14000
/*
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)
+ -+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)
*/
@@ -80,6 +81,7 @@ static int sbus_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
+static hrt_abstime last_txframe_time = 0;
static uint8_t frame[SBUS_FRAME_SIZE];
@@ -87,13 +89,15 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
-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);
+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)
{
- if (sbus_fd < 0)
+ if (sbus_fd < 0) {
sbus_fd = open(device, O_RDWR | O_NONBLOCK);
+ }
if (sbus_fd >= 0) {
struct termios t;
@@ -113,16 +117,49 @@ sbus_init(const char *device)
} else {
debug("S.Bus: open failed");
}
+
return sbus_fd;
}
void
sbus1_output(uint16_t *values, uint16_t num_values)
{
- char a = 'A';
- write(sbus_fd, &a, 1);
-}
+ uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
+ uint8_t offset = 0;
+ uint16_t value;
+ hrt_abstime now;
+
+ now = hrt_absolute_time();
+
+ if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
+ last_txframe_time = now;
+ uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f };
+
+ /* 16 is sbus number of servos/channels minus 2 single bit channels.
+ * currently ignoring single bit channels. */
+ for (unsigned i = 0; (i < num_values) && (i < 16); ++i) {
+ value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
+
+ /*protect from out of bounds values and limit to 11 bits*/
+ if (value > 0x07ff ) {
+ value = 0x07ff;
+ }
+
+ while (offset >= 8) {
+ ++byteindex;
+ offset -= 8;
+ }
+
+ oframe[byteindex] |= (value << (offset)) & 0xff;
+ oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff;
+ oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff;
+ offset += 11;
+ }
+
+ write(sbus_fd, oframe, SBUS_FRAME_SIZE);
+ }
+}
void
sbus2_output(uint16_t *values, uint16_t num_values)
{
@@ -167,8 +204,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
/* if the read failed for any reason, just give up here */
- if (ret < 1)
+ if (ret < 1) {
return false;
+ }
last_rx_time = now;
@@ -180,8 +218,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
/*
* If we don't have a full frame, return
*/
- if (partial_frame_count < SBUS_FRAME_SIZE)
+ if (partial_frame_count < SBUS_FRAME_SIZE) {
return false;
+ }
/*
* Great, it looks like we might have a frame. Go ahead and
@@ -228,7 +267,8 @@ 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, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_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)) {
@@ -237,23 +277,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
}
switch (frame[24]) {
- case 0x00:
+ case 0x00:
/* this is S.BUS 1 */
break;
- case 0x03:
+
+ case 0x03:
/* S.BUS 2 SLOT0: RX battery and external voltage */
break;
- case 0x83:
+
+ case 0x83:
/* S.BUS 2 SLOT1 */
break;
- case 0x43:
- case 0xC3:
- case 0x23:
- case 0xA3:
- case 0x63:
- case 0xE3:
+
+ case 0x43:
+ case 0xC3:
+ case 0x23:
+ case 0xA3:
+ case 0x63:
+ case 0xE3:
break;
- default:
+
+ default:
/* we expect one of the bits above, but there are some we don't know yet */
break;
}
@@ -283,7 +327,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
/* 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;
+ values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
}
/* decode switch channels if data fields are wide enough */
@@ -304,16 +348,17 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
/* 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 */
+
+ } 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,
+ *
+ * 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;
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-20 12:34:39 +0000