fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-04 08:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

sbus1 output. Cleaned up. Safer bounds checking.

Browse Source
This commit is contained in:
Daniel Shiels
2014-11-08 22:36:45 +11:00
parent 02d31522cd
commit 60ecd8868d
1 changed files with 27 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/sbus.c
+27 -20
View File
@@ -57,6 +57,7 @@
#define SBUS_FLAGS_BYTE 23 #define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3 #define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2 #define SBUS_FRAMELOST_BIT 2
#define SBUS1_FRAME_DELAY 14000
/* /*
Measured values with Futaba FX-30/R6108SB: 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_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;
static uint8_t frame[SBUS_FRAME_SIZE]; static uint8_t frame[SBUS_FRAME_SIZE];
static uint8_t oframe[SBUS_FRAME_SIZE]; static uint8_t oframe[SBUS_FRAME_SIZE];
@@ -125,35 +126,41 @@ void
sbus1_output(uint16_t *values, uint16_t num_values) sbus1_output(uint16_t *values, uint16_t num_values)
{ {
//int. first byte of data is offset 1 in sbus //int. first byte of data is offset 1 in sbus
uint8_t byteindex=1; uint8_t byteindex = 1;
uint8_t offset=0; uint8_t offset = 0;
uint16_t value; uint16_t value;
hrt_abstime now; hrt_abstime now;
//oframe[0]=0xf0; oframe[0] = 0x0f;
oframe[0]=0x0f;
now = hrt_absolute_time(); now = hrt_absolute_time();
if ((now - last_txframe_time) > 14000) {
if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
last_txframe_time = now; last_txframe_time = now;
for (uint16_t i=1;i<SBUS_FRAME_SIZE;++i) { for (uint16_t i = 1; i < SBUS_FRAME_SIZE; ++i) {
oframe[i]=0; oframe[i] = 0;
} }
// 16 is sbus number of servos/channels minus 2 single bit channels. // 16 is sbus number of servos/channels minus 2 single bit channels.
// currently ignoring single bit channels. // currently ignoring single bit channels.
for (uint16_t i=0;(i<num_values)&&(i<16);++i) { for (uint16_t i = 0; (i < num_values) && (i < 16); ++i) {
value=(uint16_t)(((values[i]-SBUS_SCALE_OFFSET+.5f)/SBUS_SCALE_FACTOR) +.5f); 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;
value&=0x07ff; if (value > 0x07ff ) {
while (offset>=8) { value = 0x07ff;
++byteindex;
offset-=8;
} }
oframe[byteindex] |= (value<<(offset))&0xff;
oframe[byteindex+1]|=(value>>(8-offset))&0xff; while (offset >= 8) {
oframe[byteindex+2]|=(value>>(16-offset))&0xff; ++byteindex;
offset+=11; 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); write(sbus_fd, oframe, SBUS_FRAME_SIZE);
} }
} }