aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/drv_pwm_output.h4
-rw-r--r--src/drivers/px4io/px4io.cpp53
-rw-r--r--src/modules/px4iofirmware/controls.c53
-rw-r--r--src/modules/px4iofirmware/mixer.cpp71
-rw-r--r--src/modules/px4iofirmware/module.mk1
-rw-r--r--src/modules/px4iofirmware/protocol.h7
-rw-r--r--src/modules/px4iofirmware/px4io.h10
-rw-r--r--src/modules/px4iofirmware/registers.c16
-rw-r--r--src/modules/px4iofirmware/safelink.c163
-rw-r--r--src/modules/px4iofirmware/sbus.c158
10 files changed, 471 insertions, 65 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 972573f9f..8b623119f 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -202,6 +202,10 @@ ORB_DECLARE(output_pwm);
/** force safety switch off (to disable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
+#define SAFELINK_CONTROL_INPUT _IOC(_PWM_SERVO_BASE, 24)
+
+#define SAFELINK_CONTROL_OUTPUT _IOC(_PWM_SERVO_BASE, 25)
+
/*
*
*
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 24da4c68b..50e6e6059 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1901,7 +1901,8 @@ PX4IO::print_status(bool extended_status)
((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"),
- ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : ""));
+ ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : ""),
+ ((flags & PX4IO_P_STATUS_FLAGS_SAFELINK_FS) ? " SAFELINK_FAILSAFE" : ""));
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n",
alarms,
@@ -1994,7 +1995,9 @@ PX4IO::print_status(bool extended_status)
((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""),
((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""),
((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""),
- ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
+ ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : ""),
+ ((features & PX4IO_P_SETUP_FEATURES_SAFELINK_IN) ? " SAFELINK_IN" : ""),
+ ((features & PX4IO_P_SETUP_FEATURES_SAFELINK_OUT) ? " SAFELINK_OUT" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s%s\n",
@@ -2475,6 +2478,26 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
break;
+ case SAFELINK_CONTROL_INPUT:
+
+ if (arg == 1) {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SAFELINK_IN);
+ } else {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_SAFELINK_IN, 0);
+ }
+
+ break;
+
+ case SAFELINK_CONTROL_OUTPUT:
+
+ if (arg == 1) {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SAFELINK_OUT);
+ } else {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_SAFELINK_OUT, 0);
+ }
+
+ break;
+
default:
/* see if the parent class can make any use of it */
ret = CDev::ioctl(filep, cmd, arg);
@@ -3213,6 +3236,32 @@ px4io_main(int argc, char *argv[])
exit(0);
}
+ if (!strcmp(argv[1], "safelink_in")) {
+ /* we can cheat and call the driver directly, as it
+ * doesn't reference filp in ioctl()
+ */
+ int ret = g_dev->ioctl(nullptr, SAFELINK_CONTROL_INPUT, 1);
+
+ if (ret != 0) {
+ errx(ret, "SAFELINK input failed");
+ }
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "safelink_out")) {
+ /* we can cheat and call the driver directly, as it
+ * doesn't reference filp in ioctl()
+ */
+ int ret = g_dev->ioctl(nullptr, SAFELINK_CONTROL_OUTPUT, 1);
+
+ if (ret != 0) {
+ errx(ret, "SAFELINK output failed");
+ }
+
+ exit(0);
+ }
+
if (!strcmp(argv[1], "rssi_analog")) {
/* we can cheat and call the driver directly, as it
* doesn't reference filp in ioctl()
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 185cb20dd..4f5a86fcb 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -67,8 +67,11 @@ controls_init(void)
/* DSM input (USART1) */
dsm_init("/dev/ttyS0");
- /* S.bus input (USART3) */
- sbus_init("/dev/ttyS2");
+ /* S.BUS input (USART3) */
+ int sbus_fd = sbus_init("/dev/ttyS2");
+
+ /* Safelink init (USART3) */
+ safelink_init(sbus_fd);
/* default to a 1:1 input map, all enabled */
for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
@@ -132,34 +135,42 @@ controls_tick() {
}
perf_end(c_gather_dsm);
- perf_begin(c_gather_sbus);
+ bool sbus_updated;
- 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 (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SAFELINK_IN) {
+ bool safelink_failsafe;
+ bool safelink_updated = safelink_input(r_safelink_values, &r_safelink_count, &safelink_failsafe, PX4IO_SERVO_COUNT);
+ } else {
- if (sbus_updated) {
- r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
+ perf_begin(c_gather_sbus);
- rssi = 255;
+ bool sbus_failsafe, sbus_frame_drop;
+ sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
- 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_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);
+ }
- 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);
}
- perf_end(c_gather_sbus);
-
/*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 2f721bf1e..3a82a5821 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -79,7 +79,8 @@ enum mixer_source {
MIX_FMU,
MIX_OVERRIDE,
MIX_FAILSAFE,
- MIX_OVERRIDE_FMU_OK
+ MIX_OVERRIDE_FMU_OK,
+ MIX_SAFELINK
};
static mixer_source source;
@@ -159,6 +160,18 @@ mixer_tick(void)
*/
if (source == MIX_FAILSAFE) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
+
+ /*
+ * Check if we can fall back in safelink failsave
+ */
+
+ if ((r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SAFELINK_OUT) && (r_safelink_count > 0)) {
+ source = MIX_SAFELINK;
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFELINK_FS;
+ } else {
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_SAFELINK_FS);
+ }
+
} else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
}
@@ -187,15 +200,35 @@ mixer_tick(void)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+ unsigned actuator_count = 0;
+
/*
* Run the mixers.
*/
- if (source == MIX_FAILSAFE) {
+ if (source == MIX_SAFELINK) {
+
+ actuator_count = r_safelink_count;
+
+ /* copy failsafe values to the servo outputs */
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
+ r_page_servos[i] = r_safelink_values[i];
+
+ /* safe actuators for FMU feedback */
+ r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
+ }
+
+ } else if (source == MIX_FAILSAFE) {
+
+ actuator_count = 0;
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
+ /* get the count of the last valid actuator */
+ if ((r_page_servos[i] != 0) && ((i + 1) > actuator_count))
+ actuator_count = i + 1;
+
/* safe actuators for FMU feedback */
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
}
@@ -211,6 +244,7 @@ mixer_tick(void)
/* poor mans mutex */
in_mixer = true;
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+ actuator_count = mixed;
in_mixer = false;
/* the pwm limit call takes care of out of band errors */
@@ -255,12 +289,19 @@ mixer_tick(void)
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
- /* set S.BUS1 or S.BUS2 outputs */
+ /* update S.BUS1 outputs */
+ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
+ sbus1_output(r_page_servos, actuator_count);
+ }
+ /* update S.BUS2 outputs */
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
- sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
- } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
- sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+ sbus2_output(r_page_servos, actuator_count);
+ }
+
+ /* update safelink outputs */
+ if (source != MIX_FAILSAFE && r_setup_features & PX4IO_P_SETUP_FEATURES_SAFELINK_OUT) {
+ safelink_output(r_page_servos, actuator_count);
}
} else if (mixer_servos_armed && should_always_enable_pwm) {
@@ -268,12 +309,20 @@ mixer_tick(void)
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
- /* set S.BUS1 or S.BUS2 outputs */
- if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT)
- sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+ /* update S.BUS1 outputs */
+ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
+ sbus1_output(r_page_servos, actuator_count);
+ }
+
+ /* update S.BUS2 outputs */
+ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
+ sbus2_output(r_page_servos, actuator_count);
+ }
- if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
- sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
+ /* update safelink outputs */
+ if (source != MIX_FAILSAFE && r_setup_features & PX4IO_P_SETUP_FEATURES_SAFELINK_OUT) {
+ safelink_output(r_page_servos, actuator_count);
+ }
}
}
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 01869569f..fff88fe99 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -7,6 +7,7 @@ SRCS = adc.c \
registers.c \
safety.c \
sbus.c \
+ safelink.c \
../systemlib/up_cxxinitialize.c \
../systemlib/perf_counter.c \
mixer.cpp \
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index d5a6b1ec4..b55ac698b 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -111,6 +111,7 @@
#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_SAFELINK_FS (1 << 13) /* SAFELINK failsave is active */
#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 */
@@ -162,6 +163,10 @@
#define PX4IO_PAGE_PWM_INFO 7
#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
+#define PX4IO_PAGE_SAFELINK 8 /**< redundancy link page */
+#define PX4IO_P_SAFELINK_COUNT 0 /**< number of inputs */
+#define PX4IO_P_SAFELINK_BASE 1 /**< index of safelink actuator zero */
+
/* setup page */
#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
@@ -169,6 +174,8 @@
#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_FEATURES_SAFELINK_IN (1 << 4) /**< enable SAFELINK input parsing on S.BUS port */
+#define PX4IO_P_SETUP_FEATURES_SAFELINK_OUT (1 << 5) /**< enable SAFELINK output on S.BUS port */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index b00a96717..b809ccf7e 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -77,6 +77,7 @@ extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */
extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */
extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */
+extern uint16_t r_page_safelink[]; /* PX4IO_PAGE_SAFELINK */
extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
@@ -111,6 +112,8 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
#define r_control_values (&r_page_controls[0])
+#define r_safelink_values (&r_page_safelink[PX4IO_P_SAFELINK_BASE])
+#define r_safelink_count r_page_safelink[PX4IO_P_SAFELINK_COUNT]
/*
* System state structure.
@@ -222,6 +225,13 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsa
extern void sbus1_output(uint16_t *values, uint16_t num_values);
extern void sbus2_output(uint16_t *values, uint16_t num_values);
+extern void sbus1_output(uint16_t *values, uint16_t num_values);
+extern void sbus2_output(uint16_t *values, uint16_t num_values);
+
+extern int safelink_init(int fd);
+extern void safelink_output(uint16_t *values, uint16_t num_values);
+extern bool safelink_input(uint16_t *values, uint16_t *num_values, bool *failsafe, uint16_t max_channels);
+
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index b37259997..03849cf65 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -141,6 +141,16 @@ uint16_t r_page_rc_input[] = {
uint16_t r_page_scratch[32];
/**
+ * PAGE 8
+ *
+ * Inputs from redundant autopilot
+ */
+uint16_t r_page_safelink[] = {
+ [PX4IO_P_SAFELINK_COUNT] = 0,
+ [PX4IO_P_SAFELINK_BASE ... (PX4IO_P_SAFELINK_BASE + PX4IO_SERVO_COUNT)] = 0
+};
+
+/**
* PAGE 100
*
* Setup registers
@@ -178,7 +188,9 @@ volatile uint16_t r_page_setup[] =
#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)
+ PX4IO_P_SETUP_FEATURES_PWM_RSSI | \
+ PX4IO_P_SETUP_FEATURES_SAFELINK_IN | \
+ PX4IO_P_SETUP_FEATURES_SAFELINK_OUT)
#else
#define PX4IO_P_SETUP_FEATURES_VALID 0
#endif
@@ -494,6 +506,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
}
+ // XXX handle safelink enabling / disabling here.
+
/* apply changes */
r_setup_features = value;
diff --git a/src/modules/px4iofirmware/safelink.c b/src/modules/px4iofirmware/safelink.c
new file mode 100644
index 000000000..f5c91d0fa
--- /dev/null
+++ b/src/modules/px4iofirmware/safelink.c
@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ * 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
+ * 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 safelink.c
+ *
+ * Serial protocol encoder / decoder for the <put a good name here>
+ * simple redundancy protocol (SRP)
+ */
+
+#include <nuttx/config.h>
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <termios.h>
+#include <stdbool.h>
+
+#include <drivers/drv_hrt.h>
+#include "px4io.h"
+
+#define SAFELINK_FRAME_SIZE (2 + 2 * PX4IO_SERVO_COUNT + 2)
+#define SAFELINK_STX1 0xFA
+#define SAFELINK_STX2 0x01 /**< version */
+
+static int safelink_fd = -1;
+
+static hrt_abstime last_rx_time;
+static hrt_abstime last_frame_time;
+
+static uint8_t frame[SAFELINK_FRAME_SIZE];
+
+static unsigned partial_frame_count;
+
+unsigned safelink_frame_drops;
+
+static bool safelink_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *failsafe, uint16_t max_channels);
+
+int
+safelink_init(int fd)
+{
+ safelink_fd = fd;
+ return OK;
+
+ r_safelink_count = 0;
+}
+
+void
+safelink_output(uint16_t *values, uint16_t num_values)
+{
+ uint8_t buf[SAFELINK_FRAME_SIZE];
+ buf[0] = SAFELINK_STX1;
+ buf[1] = SAFELINK_STX2;
+
+ for (unsigned i = 0; (i < num_values) && (i < PX4IO_SERVO_COUNT); i++) {
+ uint8_t *dp = &buf[2 + (2 * i)];
+ dp[0] = values[i] >> 8;
+ dp[1] = values[i];
+ }
+
+ buf[SAFELINK_FRAME_SIZE - 2] = 0x11;
+ buf[SAFELINK_FRAME_SIZE - 1] = 0xFF;
+}
+
+bool
+safelink_input(uint16_t *values, uint16_t *num_values, bool *failsafe, uint16_t max_channels)
+{
+ ssize_t ret;
+ hrt_abstime now;
+ bool decode_ret = false;
+
+ /*
+ * The S.BUS protocol doesn't provide reliable framing,
+ * so we detect frame boundaries by the inter-frame delay.
+ *
+ * The minimum frame spacing is 7ms; with 25 bytes at 100000bps
+ * frame transmission time is ~2ms.
+ *
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 3ms 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) > 3000) {
+ if (partial_frame_count > 0) {
+ safelink_frame_drops++;
+ partial_frame_count = 0;
+ }
+ }
+
+ /*
+ * Fetch bytes, but no more than we would need to complete
+ * the current frame.
+ */
+ ret = read(safelink_fd, &frame[partial_frame_count], SAFELINK_FRAME_SIZE - partial_frame_count);
+
+ /* if the read failed for any reason, just give up here */
+ if (ret < 1)
+ return false;
+
+ if (partial_frame_count < SAFELINK_FRAME_SIZE)
+ decode_ret = false;
+
+ safelink_decode(now, values, num_values, failsafe, max_channels);
+
+ /* return false as default */
+ return decode_ret;
+}
+
+static bool safelink_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *failsafe, uint16_t max_channels)
+{
+ /* check frame boundary markers to avoid out-of-sync cases */
+ if ((frame[0] != SAFELINK_STX1) || frame[1] != SAFELINK_STX2) {
+ safelink_frame_drops++;
+ return false;
+ }
+
+ /* XXX check checksum here */
+
+
+ /* if we make it down here, we have valid data */
+ last_frame_time = frame_time;
+
+ for (unsigned i = 0; (i < max_channels) && (i < PX4IO_SERVO_COUNT); i++) {
+ uint8_t *dp = &frame[2 + (2 * i)];
+ values[i] = (dp[0] << 8) | dp[1];
+ }
+}
+
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 0f0414148..1db22b7fc 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -52,7 +52,10 @@
#include "protocol.h"
#include "debug.h"
-#define SBUS_FRAME_SIZE 25
+#define SBUS_FRAME_SIZE 25
+#define SBUS2_FRAME_SIZE_RX_VOLTAGE 3
+#define SBUS2_FRAME_SIZE_GPS_DIGIT 3
+
#define SBUS_INPUT_CHANNELS 16
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
@@ -81,7 +84,15 @@ static int sbus_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
-static uint8_t frame[SBUS_FRAME_SIZE];
+static enum SBUS2_DECODE_STATE {
+ SBUS2_DECODE_STATE_DESYNC = 0xFFF,
+ SBUS2_DECODE_STATE_SBUS1_SYNC = 0x00,
+ SBUS2_DECODE_STATE_SBUS2_SYNC = 0x1FF,
+ SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE = 0x04,
+ SBUS2_DECODE_STATE_SBUS2_GPS = 0x14
+} sbus_decode_state;
+
+static uint8_t frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)];
static unsigned partial_frame_count;
@@ -113,6 +124,9 @@ sbus_init(const char *device)
} else {
debug("S.Bus: open failed");
}
+
+ sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
+
return sbus_fd;
}
@@ -137,7 +151,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
hrt_abstime now;
/*
- * The S.bus protocol doesn't provide reliable framing,
+ * The S.BUS protocol doesn't provide reliable framing,
* so we detect frame boundaries by the inter-frame delay.
*
* The minimum frame spacing is 7ms; with 25 bytes at 100000bps
@@ -177,18 +191,99 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
*/
partial_frame_count += ret;
- /*
- * If we don't have a full frame, return
+ /* this is set by the decoding state machine and will default to false
+ * once everything that was decodable has been decoded.
*/
- if (partial_frame_count < SBUS_FRAME_SIZE)
- return false;
+ bool decode_ret = true;
+
+ /* keep decoding until we have consumed the buffer or have given up */
+ for (unsigned d = 0; (d < 10) && (partial_frame_count > 0) && (decode_ret); d++) {
+ unsigned n_consumed = 0;
+
+ switch (sbus_decode_state) {
+ case SBUS2_DECODE_STATE_DESYNC:
+ /* fall through */
+ case SBUS2_DECODE_STATE_SBUS1_SYNC:
+ /* fall through */
+ case SBUS2_DECODE_STATE_SBUS2_SYNC:
+ {
+ /* decode whatever we got */
+ if (partial_frame_count < SBUS_FRAME_SIZE)
+ decode_ret = false;
+
+ /*
+ * Great, it looks like we might have a frame. Go ahead and
+ * decode it.
+ */
+ n_consumed = SBUS_FRAME_SIZE;
+ return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
+ }
+ break;
+ case SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE:
+ {
+ if (partial_frame_count < SBUS2_FRAME_SIZE_RX_VOLTAGE)
+ decode_ret = false;
+
+ /* find out which payload we're dealing with in this slot */
+ switch(frame[0]) {
+ case 0x03:
+ {
+ uint16_t rx_voltage = (frame[1] << 8) | frame[2];
+ n_consumed = 3;
+ }
+ break;
+ case 0x0F:
+ /* the battery slot is unused and followed by a normal frame */
+ sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
+ break;
+ default:
+ /* this is not what we expect it to be, go back to sync */
+ sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
+ /* throw unknown bytes away */
+ n_consumed = 3;
+ }
+ }
+ break;
+ case SBUS2_DECODE_STATE_SBUS2_GPS:
+ {
+ if (partial_frame_count < SBUS2_FRAME_SIZE_GPS_DIGIT)
+ decode_ret = false;
+
+ /* find out which payload we're dealing with in this slot */
+ switch(frame[0]) {
+ case 0x13:
+ {
+ uint16_t gps_something = (frame[1] << 8) | frame[2];
+ n_consumed = 3;
+ }
+ break;
+ case 0x0F:
+ /* the GPS slot is unused and followed by a normal frame */
+ sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
+ break;
+ default:
+ /* this is not what we expect it to be, go back to sync */
+ sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
+ /* throw unknown bytes away */
+ n_consumed = 3;
+ }
+ }
+ break;
+ default:
+ decode_ret = false;
+ }
- /*
- * Great, it looks like we might have a frame. Go ahead and
- * decode it.
- */
- partial_frame_count = 0;
- return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
+ /* move buffer to start after this step, keep decoding if any bytes remain */
+
+ /* XXX should be still more efficient than single-byte ringbuffer accesses */
+ uint8_t frame_buf[SBUS_FRAME_SIZE];
+ memcpy(&frame_buf[0], &frame[n_consumed], partial_frame_count - n_consumed);
+ memcpy(&frame[0], &frame_buf[0], partial_frame_count - n_consumed);
+ partial_frame_count = partial_frame_count - n_consumed;
+ }
+
+ /* return false as default */
+ return decode_ret;
}
/*
@@ -238,24 +333,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
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;
+ /* this is S.BUS 1 */
+ sbus_decode_state = SBUS2_DECODE_STATE_SBUS1_SYNC;
+ break;
+ case 0x04:
+ /* receiver voltage */
+ sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_RX_VOLTAGE;
+ break;
+ case 0x14:
+ /* GPS / baro */
+ sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_GPS;
+ break;
+ case 0x24:
+ /* Unknown SBUS2 data */
+ sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
+ break;
+ case 0x34:
+ /* Unknown SBUS2 data */
+ sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
+ break;
default:
- /* we expect one of the bits above, but there are some we don't know yet */
- break;
+ return false;
}
/* we have received something we think is a frame */
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-08 21:29:40 +0000