APM:Libraries
RC_SBUS_parser.cpp
Go to the documentation of this file.
1 /*
2 (c) 2017 night_ghost@ykoctpa.ru
3 
4 */
5 #pragma GCC optimize ("O2")
6 
7 #include <AP_HAL/HAL.h>
9 
10 #ifdef BOARD_SBUS_UART
11 
12 #include <exti.h>
13 #include <timer.h>
14 #include <usart.h>
15 #include "RCInput.h"
16 #include <pwm_in.h>
17 #include "sbus.h"
18 #include "GPIO.h"
19 #include "ring_buffer_pulse.h"
20 
21 #include "RC_SBUS_parser.h"
22 
23 using namespace F4Light;
24 
25 
26 extern const AP_HAL::HAL& hal;
27 
29 
30 void SBUS_parser::init(uint8_t ch){
31 
32  memset((void *)&_val[0], 0, sizeof(_val));
33  memset((void *)&sbus, 0, sizeof(sbus));
34 
35  _last_signal=0;
36  _last_change =0;
37 
39 
40 }
41 
42 
43 void SBUS_parser::late_init(uint8_t b){
44 
45  if(hal_param_helper->_uart_sbus) {
46 #ifdef BOARD_SBUS_INVERTER
48  GPIO::_write( BOARD_SBUS_INVERTER, HIGH); // do inverse
49 #endif
50 
51  const usart_dev * uart = UARTS[hal_param_helper->_uart_sbus];
52  if(uart) {
53  uartSDriver = new UARTDriver(uart);
54 
55  // initialize SBUS UART
56  uartSDriver->end();
58  if(uartSDriver->is_initialized() ) {
61  }
62  } else printf("\nWrong HAL_SBUS_UART selected!");
63  }
64 }
65 
66 
67 /*
68  add some bytes of input in SBUS serial stream format, coping with partial packets - UART input callback
69  */
72 }
73 
74 
76 
77  while(uartSDriver->available()){
78 
79  // at least 1 byte we have
80  const uint8_t frame_size = sizeof(sbus.frame);
81 
82  //uint32_t now = systick_uptime();
83  uint32_t now = Scheduler::_micros();
84  if (now - sbus.last_input_uS > 5000) { // 25 bytes * 13 bits on 100 000 baud takes 3250uS
85  // resync based on time
86  sbus.partial_frame_count = 0;
87  }
88  sbus.last_input_uS = now;
89 
90  if (sbus.partial_frame_count + 1 > frame_size) {
91  return; // we can't add bytes to buffer
92  }
93 
94 
95  sbus.frame[sbus.partial_frame_count] = uartSDriver->read();
96  sbus.partial_frame_count += 1;
97 
98  if (sbus.partial_frame_count == frame_size) {
99  sbus.partial_frame_count = 0;
100  uint16_t values[18] {};
101  uint16_t num_values=0;
102  bool sbus_failsafe=false, sbus_frame_drop=false;
103 
104  if (sbus_decode(sbus.frame, values, &num_values,
105  &sbus_failsafe, &sbus_frame_drop,
107  num_values >= F4Light_RC_INPUT_MIN_CHANNELS)
108  {
109 
110  for (uint8_t i=0; i<num_values; i++) {
111  if(_val[i] != values[i]) _last_change = systick_uptime();
112  _val[i] = values[i];
113  }
114  _channels = num_values;
115 
116  if (!sbus_failsafe) {
118  }
119  }
120 
121  sbus.partial_frame_count = 0; // clear count when packet done
122  }
123  }
124 }
125 
126 
127 #endif
int printf(const char *fmt,...)
Definition: stdio.c:113
AP_HAL::MemberProc mp
Definition: handler.h:18
static UARTDriver * uartSDriver
static void _pinMode(uint8_t pin, uint8_t output)
Definition: GPIO.cpp:21
Handler h
Definition: handler.h:20
#define BOARD_SBUS_INVERTER
Definition: board.h:164
Simple circular buffer for PEM input.
static void _write(uint8_t pin, uint8_t value)
Definition: GPIO.h:165
static uint64_t systick_uptime(void)
Returns the system uptime, in milliseconds.
Definition: systick.h:44
const AP_HAL::HAL & hal
Definition: UARTDriver.cpp:37
Definition: GPIO.h:35
void begin(uint32_t b)
Definition: UARTDriver.h:31
timer interface.
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
void setCallback(Handler cb)
Definition: UARTDriver.h:45
const usart_dev *const UARTS[]
Definition: usart.c:136
struct F4Light::SBUS_parser::SBUS sbus
#define HIGH
Definition: board.h:34
void late_init(uint8_t ch)
int16_t read() override
Definition: UARTDriver.cpp:103
bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
Definition: sbus.cpp:97
#define F4Light_RC_INPUT_MIN_CHANNELS
Definition: Config.h:11
void init(uint8_t ch)
static void do_io_completion(uint8_t id)
Definition: Scheduler.h:422
uint32_t available() override
Definition: UARTDriver.cpp:94
volatile uint8_t _channels
Definition: RC_parser.h:24
static uint32_t _micros()
Definition: Scheduler.h:232
volatile uint16_t _val[F4Light_RC_INPUT_NUM_CHANNELS]
Definition: RC_parser.h:22
volatile uint64_t _last_signal
Definition: RC_parser.h:21
#define F4Light_RC_INPUT_NUM_CHANNELS
Definition: Config.h:12
#define FUNCTOR_BIND_MEMBER(func, rettype,...)
Definition: functor.h:31
uint64_t _last_change
Definition: RC_parser.h:23
static uint8_t register_io_completion(Handler handle)
Definition: Scheduler.cpp:1219