forked from Archive/PX4-Autopilot
First attempt at sbus1 output.
This commit is contained in:
parent
43418a6749
commit
02d31522cd
|
@ -80,8 +80,10 @@ 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 uint8_t frame[SBUS_FRAME_SIZE];
|
static uint8_t frame[SBUS_FRAME_SIZE];
|
||||||
|
static uint8_t oframe[SBUS_FRAME_SIZE];
|
||||||
|
|
||||||
static unsigned partial_frame_count;
|
static unsigned partial_frame_count;
|
||||||
|
|
||||||
|
@ -122,10 +124,39 @@ sbus_init(const char *device)
|
||||||
void
|
void
|
||||||
sbus1_output(uint16_t *values, uint16_t num_values)
|
sbus1_output(uint16_t *values, uint16_t num_values)
|
||||||
{
|
{
|
||||||
char a = 'A';
|
//int. first byte of data is offset 1 in sbus
|
||||||
write(sbus_fd, &a, 1);
|
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
|
void
|
||||||
sbus2_output(uint16_t *values, uint16_t num_values)
|
sbus2_output(uint16_t *values, uint16_t num_values)
|
||||||
{
|
{
|
||||||
|
|
Loading…
Reference in New Issue