From 02d31522cde1de13ab397f4fbd77c0cf9b7f712d Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Wed, 29 Oct 2014 22:00:30 +1100 Subject: [PATCH 1/4] First attempt at sbus1 output. --- src/modules/px4iofirmware/sbus.c | 37 +++++++++++++++++++++++++++++--- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 6ead38d617..23dae78985 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -80,8 +80,10 @@ 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]; +static uint8_t oframe[SBUS_FRAME_SIZE]; static unsigned partial_frame_count; @@ -122,10 +124,39 @@ sbus_init(const char *device) void sbus1_output(uint16_t *values, uint16_t num_values) { - char a = 'A'; - write(sbus_fd, &a, 1); -} + //int. first byte of data is offset 1 in sbus + uint8_t byteindex=1; + uint8_t offset=0; + uint16_t value; + hrt_abstime now; + //oframe[0]=0xf0; + oframe[0]=0x0f; + now = hrt_absolute_time(); + if ((now - last_txframe_time) > 14000) { + last_txframe_time = now; + + for (uint16_t i=1;i=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) { From 60ecd8868db3539e552bad5f51473d1baf5d6ecd Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Sat, 8 Nov 2014 22:36:45 +1100 Subject: [PATCH 2/4] sbus1 output. Cleaned up. Safer bounds checking. --- src/modules/px4iofirmware/sbus.c | 47 ++++++++++++++++++-------------- 1 file changed, 27 insertions(+), 20 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 23dae78985..2f234234ea 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -57,6 +57,7 @@ #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: @@ -80,7 +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 hrt_abstime last_txframe_time = 0; static uint8_t frame[SBUS_FRAME_SIZE]; static uint8_t oframe[SBUS_FRAME_SIZE]; @@ -125,35 +126,41 @@ void sbus1_output(uint16_t *values, uint16_t num_values) { //int. first byte of data is offset 1 in sbus - uint8_t byteindex=1; - uint8_t offset=0; + uint8_t byteindex = 1; + uint8_t offset = 0; uint16_t value; hrt_abstime now; - //oframe[0]=0xf0; - oframe[0]=0x0f; + oframe[0] = 0x0f; - now = hrt_absolute_time(); - if ((now - last_txframe_time) > 14000) { + now = hrt_absolute_time(); + + if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) { last_txframe_time = now; - for (uint16_t i=1;i=8) { - ++byteindex; - offset-=8; + if (value > 0x07ff ) { + value = 0x07ff; } - oframe[byteindex] |= (value<<(offset))&0xff; - oframe[byteindex+1]|=(value>>(8-offset))&0xff; - oframe[byteindex+2]|=(value>>(16-offset))&0xff; - offset+=11; - } + + 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); } } From 34e9d9efce10ce487b4c5af0cf93a442ab3b8594 Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Mon, 10 Nov 2014 15:54:28 +1100 Subject: [PATCH 3/4] Code style cleanup. --- src/modules/px4iofirmware/sbus.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 2f234234ea..90c0e05038 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -125,8 +125,7 @@ sbus_init(const char *device) void sbus1_output(uint16_t *values, uint16_t num_values) { - //int. first byte of data is offset 1 in sbus - uint8_t byteindex = 1; + uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; uint16_t value; hrt_abstime now; @@ -141,11 +140,13 @@ sbus1_output(uint16_t *values, uint16_t num_values) oframe[i] = 0; } - // 16 is sbus number of servos/channels minus 2 single bit channels. - // currently ignoring single bit channels. - for (uint16_t i = 0; (i < num_values) && (i < 16); ++i) { + /* 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; + + /*protect from out of bounds values and limit to 11 bits*/ if (value > 0x07ff ) { value = 0x07ff; } From 3eeec2cce05c8f32a5c2d70b956786d41114df3c Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Mon, 10 Nov 2014 16:34:28 +1100 Subject: [PATCH 4/4] Cleaned up sbus output frame initialisation. --- src/modules/px4iofirmware/sbus.c | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 90c0e05038..d76ec55f05 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -84,7 +84,6 @@ static hrt_abstime last_frame_time; static hrt_abstime last_txframe_time = 0; static uint8_t frame[SBUS_FRAME_SIZE]; -static uint8_t oframe[SBUS_FRAME_SIZE]; static unsigned partial_frame_count; @@ -128,17 +127,13 @@ sbus1_output(uint16_t *values, uint16_t num_values) uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; uint16_t value; - hrt_abstime now; - oframe[0] = 0x0f; + hrt_abstime now; now = hrt_absolute_time(); if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) { last_txframe_time = now; - - for (uint16_t i = 1; i < SBUS_FRAME_SIZE; ++i) { - oframe[i] = 0; - } + uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f }; /* 16 is sbus number of servos/channels minus 2 single bit channels. * currently ignoring single bit channels. */