diff options
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r-- | src/modules/px4iofirmware/sbus.c | 37 |
1 files changed, 34 insertions, 3 deletions
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 6ead38d61..23dae7898 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<SBUS_FRAME_SIZE;++i) { + 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) { + value=(uint16_t)(((values[i]-SBUS_SCALE_OFFSET+.5f)/SBUS_SCALE_FACTOR) +.5f); + //protect from out of bounds values and limit to 11 bits; + 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) { |