Browse Source

HAL_Linux: support SRXL R/C input

master
Andrew Tridgell 8 years ago
parent
commit
710d08da6d
  1. 25
      libraries/AP_HAL_Linux/RCInput.cpp
  2. 3
      libraries/AP_HAL_Linux/RCInput.h
  3. 6
      libraries/AP_HAL_Linux/RCInput_115200.cpp

25
libraries/AP_HAL_Linux/RCInput.cpp

@ -14,6 +14,7 @@ @@ -14,6 +14,7 @@
#include <AP_HAL/utility/dsm.h>
#include <AP_HAL/utility/sumd.h>
#include <AP_HAL/utility/st24.h>
#include <AP_HAL/utility/srxl.h>
#include "RCInput.h"
#include "sbus.h"
@ -464,6 +465,30 @@ void RCInput::add_st24_input(const uint8_t *bytes, size_t nbytes) @@ -464,6 +465,30 @@ void RCInput::add_st24_input(const uint8_t *bytes, size_t nbytes)
}
}
/*
add some bytes of input in SRXL serial stream format, coping with partial packets
*/
void RCInput::add_srxl_input(const uint8_t *bytes, size_t nbytes)
{
uint16_t values[LINUX_RC_INPUT_NUM_CHANNELS];
uint8_t channel_count;
uint64_t now = AP_HAL::micros64();
while (nbytes > 0) {
if (srxl_decode(now, *bytes++, &channel_count, values, LINUX_RC_INPUT_NUM_CHANNELS) == 0) {
if (channel_count > LINUX_RC_INPUT_NUM_CHANNELS) {
return;
}
for (uint8_t i=0; i<channel_count; i++) {
_pwm_values[i] = values[i];
}
_num_channels = channel_count;
new_rc_input = true;
}
nbytes--;
}
}
/*
add some bytes of input in SBUS serial stream format, coping with partial packets

3
libraries/AP_HAL_Linux/RCInput.h

@ -39,6 +39,9 @@ public: @@ -39,6 +39,9 @@ public:
// add some st24 input bytes, for RCInput over a serial port
void add_st24_input(const uint8_t *bytes, size_t nbytes);
// add some srxl input bytes, for RCInput over a serial port
void add_srxl_input(const uint8_t *bytes, size_t nbytes);
protected:
void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1);

6
libraries/AP_HAL_Linux/RCInput_115200.cpp

@ -31,9 +31,6 @@ @@ -31,9 +31,6 @@
#include <errno.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <AP_HAL/utility/sumd.h>
#include <AP_HAL/utility/st24.h>
#include <AP_HAL/utility/dsm.h>
extern const AP_HAL::HAL& hal;
@ -105,6 +102,9 @@ void RCInput_115200::_timer_tick(void) @@ -105,6 +102,9 @@ void RCInput_115200::_timer_tick(void)
return;
}
// process as srxl
add_srxl_input(bytes, nread);
// process as DSM
add_dsm_input(bytes, nread);

Loading…
Cancel
Save