dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sbus: added support for changing SBUS output rate

This commit is contained in:
Andrew Tridgell 2016-04-12 18:39:58 +10:00 committed by Lorenz Meier
parent 05790ff540
commit 58122ed33d
2 changed files with 27 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/lib/rc/sbus.c
View File

@ -57,7 +57,16 @@
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
#define SBUS1_FRAME_DELAY 14000
// testing with a SBUS->PWM adapter shows that
// above 300Hz SBUS becomes unreliable. 333 would
// be the theoretical achievable, but at 333Hz some
// frames are lost
#define SBUS1_MAX_RATE_HZ 300
#define SBUS1_MIN_RATE_HZ 50
// this is the rate of the old code
#define SBUS1_DEFAULT_RATE_HZ 72
#define SBUS_SINGLE_CHAR_LEN_US (1/((100000/10)) * 1000 * 1000)
@ -108,6 +117,7 @@ static enum SBUS2_DECODE_STATE {
static uint8_t sbus_frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)];
static unsigned partial_frame_count;
static unsigned sbus1_frame_delay = (1000U*1000U)/SBUS1_DEFAULT_RATE_HZ;
static unsigned sbus_frame_drops;
@ -179,7 +189,7 @@ sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values)
now = hrt_absolute_time();
if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
if ((now - last_txframe_time) > sbus1_frame_delay) {
last_txframe_time = now;
uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f };
@ -627,3 +637,17 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num
return true;
}
/*
set output rate of SBUS in Hz
*/
void sbus1_set_output_rate_hz(uint16_t rate_hz)
{
if (rate_hz > SBUS1_MAX_RATE_HZ) {
rate_hz = SBUS1_MAX_RATE_HZ;
}
if (rate_hz < SBUS1_MIN_RATE_HZ) {
rate_hz = SBUS1_MIN_RATE_HZ;
}
sbus1_frame_delay = (1000U*1000U) / rate_hz;
}

1
src/lib/rc/sbus.h
View File

@ -76,6 +76,7 @@ __EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *v
uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels);
__EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values);
__EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values);
__EXPORT void sbus1_set_output_rate_hz(uint16_t rate_hz);
/**
* The number of incomplete frames we encountered