diff options
author | Lorenz Meier <lm@qgroundcontrol.org> | 2014-11-10 13:16:21 +0100 |
---|---|---|
committer | Lorenz Meier <lm@qgroundcontrol.org> | 2014-11-10 13:16:21 +0100 |
commit | 4e8e6e653beb21808f532b83c6c6e827103ea379 (patch) | |
tree | c1ca7066f271fa06a250befa9146fd1c3cbb6035 /src/modules | |
parent | ea0a59f80650c7bd7c72889f1c7bb72bfd3fdbd5 (diff) | |
parent | 3eeec2cce05c8f32a5c2d70b956786d41114df3c (diff) | |
download | px4-firmware-4e8e6e653beb21808f532b83c6c6e827103ea379.tar.gz px4-firmware-4e8e6e653beb21808f532b83c6c6e827103ea379.tar.bz2 px4-firmware-4e8e6e653beb21808f532b83c6c6e827103ea379.zip |
Merge pull request #1439 from samsonact/master
sbus1_output
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/px4iofirmware/sbus.c | 40 |
1 files changed, 37 insertions, 3 deletions
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 6ead38d61..d76ec55f0 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,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]; @@ -122,10 +124,42 @@ sbus_init(const char *device) 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) { |