mirror of https://github.com/ArduPilot/ardupilot
128 lines
3.4 KiB
C++
128 lines
3.4 KiB
C++
/*
|
|
(c) 2017 night_ghost@ykoctpa.ru
|
|
|
|
*/
|
|
#pragma GCC optimize ("O2")
|
|
|
|
#include <AP_HAL/HAL.h>
|
|
#include <AP_Param_Helper/AP_Param_Helper.h>
|
|
|
|
#ifdef BOARD_SBUS_UART
|
|
|
|
#include <exti.h>
|
|
#include <timer.h>
|
|
#include <usart.h>
|
|
#include "RCInput.h"
|
|
#include <pwm_in.h>
|
|
#include "sbus.h"
|
|
#include "GPIO.h"
|
|
#include "ring_buffer_pulse.h"
|
|
|
|
#include "RC_SBUS_parser.h"
|
|
|
|
using namespace F4Light;
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
UARTDriver *SBUS_parser::uartSDriver;
|
|
|
|
void SBUS_parser::init(uint8_t ch){
|
|
|
|
memset((void *)&_val[0], 0, sizeof(_val));
|
|
memset((void *)&sbus, 0, sizeof(sbus));
|
|
|
|
_last_signal=0;
|
|
_last_change =0;
|
|
|
|
_ioc = Scheduler::register_io_completion(FUNCTOR_BIND_MEMBER(&SBUS_parser::_io_completion, void));
|
|
|
|
}
|
|
|
|
|
|
void SBUS_parser::late_init(uint8_t b){
|
|
|
|
if(hal_param_helper->_uart_sbus) {
|
|
#ifdef BOARD_SBUS_INVERTER
|
|
GPIO::_pinMode(BOARD_SBUS_INVERTER, OUTPUT);
|
|
GPIO::_write( BOARD_SBUS_INVERTER, HIGH); // do inverse
|
|
#endif
|
|
|
|
const usart_dev * uart = UARTS[hal_param_helper->_uart_sbus];
|
|
if(uart) {
|
|
uartSDriver = new UARTDriver(uart);
|
|
|
|
// initialize SBUS UART
|
|
uartSDriver->end();
|
|
uartSDriver->begin(100000, (UART_Parity_Even <<4) | UART_Stop_Bits_2);
|
|
if(uartSDriver->is_initialized() ) {
|
|
Revo_handler h = { .mp = FUNCTOR_BIND_MEMBER(&SBUS_parser::add_uart_input, void) };
|
|
uartSDriver->setCallback(h.h);
|
|
}
|
|
} else printf("\nWrong HAL_SBUS_UART selected!");
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
add some bytes of input in SBUS serial stream format, coping with partial packets - UART input callback
|
|
*/
|
|
void SBUS_parser::add_uart_input() {
|
|
Scheduler::do_io_completion(_ioc);
|
|
}
|
|
|
|
|
|
void SBUS_parser::_io_completion() {
|
|
|
|
while(uartSDriver->available()){
|
|
|
|
// at least 1 byte we have
|
|
const uint8_t frame_size = sizeof(sbus.frame);
|
|
|
|
//uint32_t now = systick_uptime();
|
|
uint32_t now = Scheduler::_micros();
|
|
if (now - sbus.last_input_uS > 5000) { // 25 bytes * 13 bits on 100 000 baud takes 3250uS
|
|
// resync based on time
|
|
sbus.partial_frame_count = 0;
|
|
}
|
|
sbus.last_input_uS = now;
|
|
|
|
if (sbus.partial_frame_count + 1 > frame_size) {
|
|
return; // we can't add bytes to buffer
|
|
}
|
|
|
|
|
|
sbus.frame[sbus.partial_frame_count] = uartSDriver->read();
|
|
sbus.partial_frame_count += 1;
|
|
|
|
if (sbus.partial_frame_count == frame_size) {
|
|
sbus.partial_frame_count = 0;
|
|
uint16_t values[18] {};
|
|
uint16_t num_values=0;
|
|
bool sbus_failsafe=false, sbus_frame_drop=false;
|
|
|
|
if (sbus_decode(sbus.frame, values, &num_values,
|
|
&sbus_failsafe, &sbus_frame_drop,
|
|
F4Light_RC_INPUT_NUM_CHANNELS) &&
|
|
num_values >= F4Light_RC_INPUT_MIN_CHANNELS)
|
|
{
|
|
|
|
for (uint8_t i=0; i<num_values; i++) {
|
|
if(_val[i] != values[i]) _last_change = systick_uptime();
|
|
_val[i] = values[i];
|
|
}
|
|
_channels = num_values;
|
|
|
|
if (!sbus_failsafe) {
|
|
_last_signal = systick_uptime();
|
|
}
|
|
}
|
|
|
|
sbus.partial_frame_count = 0; // clear count when packet done
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
#endif
|