zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Build S.BUS and DSM decoders in RC lib

sbg
Lorenz Meier 9 years ago
parent
ba4fdf197e
commit
d2b154cd07
6 changed files with 144 additions and 38 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      src/lib/rc/CMakeLists.txt
  2. 37
      src/lib/rc/dsm.c
  3. 52
      src/lib/rc/dsm.h
  4. 35
      src/lib/rc/sbus.c
  5. 55
      src/lib/rc/sbus.h
  6. 2
      src/lib/rc/sumd.h

1
src/lib/rc/CMakeLists.txt
Unescape View File

@ -37,6 +37,7 @@ px4_add_module(
SRCS SRCS
st24.c st24.c
sumd.c sumd.c
sbus.c
DEPENDS DEPENDS
platforms__common platforms__common
) )

37
src/lib/rc/dsm.c
Unescape View File

@ -46,9 +46,16 @@
#include <unistd.h> #include <unistd.h>
#include <termios.h> #include <termios.h>
#include "dsm.h"
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include "px4io.h" #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#include <px4io.h>
#endif
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#include <px4io.h>
#endif
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/ #define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/ #define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
@ -185,18 +192,18 @@ dsm_guess_format(bool reset)
if ((votes11 == 1) && (votes10 == 0)) { if ((votes11 == 1) && (votes10 == 0)) {
dsm_channel_shift = 11; dsm_channel_shift = 11;
debug("DSM: 11-bit format"); //debug("DSM: 11-bit format");
return; return;
} }
if ((votes10 == 1) && (votes11 == 0)) { if ((votes10 == 1) && (votes11 == 0)) {
dsm_channel_shift = 10; dsm_channel_shift = 10;
debug("DSM: 10-bit format"); //debug("DSM: 10-bit format");
return; return;
} }
/* call ourselves to reset our state ... we have to try again */ /* call ourselves to reset our state ... we have to try again */
debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); //debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
dsm_guess_format(true); dsm_guess_format(true);
} }
@ -237,11 +244,11 @@ dsm_init(const char *device)
/* reset the format detector */ /* reset the format detector */
dsm_guess_format(true); dsm_guess_format(true);
debug("DSM: ready"); //debug("DSM: ready");
} else { } else {
debug("DSM: open failed"); //debug("DSM: open failed");
} }
@ -257,11 +264,9 @@ dsm_init(const char *device)
void void
dsm_bind(uint16_t cmd, int pulses) dsm_bind(uint16_t cmd, int pulses)
{ {
#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) #if !defined(GPIO_USART1_RX_SPEKTRUM)
#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
#else #else
const uint32_t usart1RxAsOutp =
GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
if (dsm_fd < 0) { if (dsm_fd < 0) {
return; return;
@ -272,9 +277,9 @@ dsm_bind(uint16_t cmd, int pulses)
case dsm_bind_power_down: case dsm_bind_power_down:
/*power down DSM satellite*/ /*power down DSM satellite*/
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #if defined(CONFIG_ARCH_BOARD_PX4IO_V1)
POWER_RELAY1(0); POWER_RELAY1(0);
#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ #elif defined(CONFIG_ARCH_BOARD_PX4IO_V2)
POWER_SPEKTRUM(0); POWER_SPEKTRUM(0);
#endif #endif
break; break;
@ -282,9 +287,9 @@ dsm_bind(uint16_t cmd, int pulses)
case dsm_bind_power_up: case dsm_bind_power_up:
/*power up DSM satellite*/ /*power up DSM satellite*/
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #if defined(CONFIG_ARCH_BOARD_PX4IO_V1)
POWER_RELAY1(1); POWER_RELAY1(1);
#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ #elif defined(CONFIG_ARCH_BOARD_PX4IO_V2)
POWER_SPEKTRUM(1); POWER_SPEKTRUM(1);
#endif #endif
dsm_guess_format(true); dsm_guess_format(true);
@ -293,7 +298,7 @@ dsm_bind(uint16_t cmd, int pulses)
case dsm_bind_set_rx_out: case dsm_bind_set_rx_out:
/*Set UART RX pin to active output mode*/ /*Set UART RX pin to active output mode*/
stm32_configgpio(usart1RxAsOutp); stm32_configgpio(GPIO_USART1_RX_SPEKTRUM);
break; break;
case dsm_bind_send_pulses: case dsm_bind_send_pulses:
@ -301,9 +306,9 @@ dsm_bind(uint16_t cmd, int pulses)
/*Pulse RX pin a number of times*/ /*Pulse RX pin a number of times*/
for (int i = 0; i < pulses; i++) { for (int i = 0; i < pulses; i++) {
up_udelay(120); up_udelay(120);
stm32_gpiowrite(usart1RxAsOutp, false); stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, false);
up_udelay(120); up_udelay(120);
stm32_gpiowrite(usart1RxAsOutp, true); stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, true);
} }
break; break;

52
src/lib/rc/dsm.h
Unescape View File

@ -0,0 +1,52 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 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 dsm.h
*
* RC protocol definition for Spektrum RC
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#include <stdint.h>
__BEGIN_DECLS
__EXPORT int dsm_init(const char *device);
__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes);
__EXPORT void dsm_bind(uint16_t cmd, int pulses);
__END_DECLS

35
src/lib/rc/sbus.c
Unescape View File

@ -43,15 +43,9 @@
#include <unistd.h> #include <unistd.h>
#include <termios.h> #include <termios.h>
#include <systemlib/ppm_decode.h> #include "sbus.h"
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#define DEBUG
#include "px4io.h"
#include "protocol.h"
#include "debug.h"
#define SBUS_FRAME_SIZE 25 #define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16 #define SBUS_INPUT_CHANNELS 16
#define SBUS_FLAGS_BYTE 23 #define SBUS_FLAGS_BYTE 23
@ -77,8 +71,6 @@
#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) #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)) #define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
static int sbus_fd = -1;
static hrt_abstime last_rx_time; static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time; static hrt_abstime last_frame_time;
static hrt_abstime last_txframe_time = 0; static hrt_abstime last_txframe_time = 0;
@ -93,11 +85,9 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_
bool *sbus_frame_drop, uint16_t max_channels); bool *sbus_frame_drop, uint16_t max_channels);
int int
sbus_init(const char *device) sbus_init(const char *device, bool singlewire)
{ {
if (sbus_fd < 0) { int sbus_fd = open(device, O_RDWR | O_NONBLOCK);
sbus_fd = open(device, O_RDWR | O_NONBLOCK);
}
if (sbus_fd >= 0) { if (sbus_fd >= 0) {
struct termios t; struct termios t;
@ -108,21 +98,24 @@ sbus_init(const char *device)
t.c_cflag |= (CSTOPB | PARENB); t.c_cflag |= (CSTOPB | PARENB);
tcsetattr(sbus_fd, TCSANOW, &t); tcsetattr(sbus_fd, TCSANOW, &t);
if (singlewire) {
/* only defined in configs capable of IOCTL */
#ifdef SBUS_SERIAL_PORT
ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
#endif
}
/* initialise the decoder */ /* initialise the decoder */
partial_frame_count = 0; partial_frame_count = 0;
last_rx_time = hrt_absolute_time(); last_rx_time = hrt_absolute_time();
debug("S.Bus: ready");
} else {
debug("S.Bus: open failed");
} }
return sbus_fd; return sbus_fd;
} }
void void
sbus1_output(uint16_t *values, uint16_t num_values) sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values)
{ {
uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
uint8_t offset = 0; uint8_t offset = 0;
@ -161,14 +154,14 @@ sbus1_output(uint16_t *values, uint16_t num_values)
} }
} }
void void
sbus2_output(uint16_t *values, uint16_t num_values) sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values)
{ {
char b = 'B'; char b = 'B';
write(sbus_fd, &b, 1); write(sbus_fd, &b, 1);
} }
bool bool
sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
{ {
ssize_t ret; ssize_t ret;
hrt_abstime now; hrt_abstime now;
@ -331,7 +324,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
} }
/* decode switch channels if data fields are wide enough */ /* decode switch channels if data fields are wide enough */
if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) { if (max_values > 17 && chancount > 15) {
chancount = 18; chancount = 18;
/* channel 17 (index 16) */ /* channel 17 (index 16) */

55
src/lib/rc/sbus.h
Unescape View File

@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 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 sbus.h
*
* RC protocol definition for S.BUS
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#include <stdint.h>
#include <stdbool.h>
__BEGIN_DECLS
__EXPORT int sbus_init(const char *device, bool singlewire);
__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
uint16_t max_channels);
__EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values);
__EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values);
__END_DECLS

2
src/lib/rc/sumd.h
Unescape View File

@ -101,7 +101,7 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value);
__EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
uint16_t *channels, uint16_t max_chan_count); uint16_t *channels, uint16_t max_chan_count);
*/ */
int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
uint16_t *channels, uint16_t max_chan_count); uint16_t *channels, uint16_t max_chan_count);

Write Preview
Loading…
Cancel
Save