Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_BoardConfig: use hal.rcout->enable_sbus_out()

Browse Source
This commit is contained in:
Andrew Tridgell 2016-04-15 16:39:38 +10:00
parent 6f284d673a
commit 2304c41f44
1 changed files with 19 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AP_BoardConfig/AP_BoardConfig.cpp
View File

@ -161,38 +161,26 @@ void AP_BoardConfig::init()
} }
if (_sbus_out_rate.get() >= 1) { if (_sbus_out_rate.get() >= 1) {
int fd = open("/dev/px4io", 0); static const struct {
if (fd == -1) { uint8_t value;
hal.console->printf("SBUS: Unable to open px4io for sbus\n"); uint16_t rate;
} else { } rates[] = {
static const struct { { 1, 50 },
uint8_t value; { 2, 75 },
uint16_t rate; { 3, 100 },
} rates[] = { { 4, 150 },
{ 1, 50 }, { 5, 200 },
{ 2, 75 }, { 6, 250 },
{ 3, 100 }, { 7, 300 }
{ 4, 150 }, };
{ 5, 200 }, uint16_t rate = 300;
{ 6, 250 }, for (i=0; i<ARRAY_SIZE(rates); i++) {
{ 7, 300 } if (rates[i].value == _sbus_out_rate) {
}; rate = rates[i].rate;
uint16_t rate = 300;
for (i=0; i<ARRAY_SIZE(rates); i++) {
if (rates[i].value == _sbus_out_rate) {
rate = rates[i].rate;
}
} }
for (uint8_t tries=0; tries<10; tries++) { }
if (ioctl(fd, SBUS_SET_PROTO_VERSION, 1) != 0) { if (!hal.rcout->enable_sbus_out(rate)) {
continue; hal.console->printf("Failed to enable SBUS out\n");
}
if (ioctl(fd, PWM_SERVO_SET_SBUS_RATE, rate) != 0) {
continue;
}
break;
}
close(fd);
} }
} }