Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_HAL_F4Light/RCInput.cpp

352 lines
8.4 KiB
C++
Raw Blame History

/*
(c) 2017 night_ghost@ykoctpa.ru
*/
#include <AP_HAL/HAL.h>
#include <AP_Param_Helper/AP_Param_Helper.h>
#include <exti.h>
#include <timer.h>
#include "RCInput.h"
#include <pwm_in.h>
#include <AP_HAL/utility/dsm.h>
#include "sbus.h"
#include "GPIO.h"
#include "ring_buffer_pulse.h"
#include "RC_PPM_parser.h"
#ifdef BOARD_SPEKTRUM_RX_PIN
#include "RC_DSM_parser.h"
#endif
#ifdef BOARD_SBUS_UART
#include "RC_SBUS_parser.h"
#endif
// Constructors ////////////////////////////////////////////////////////////////
using namespace F4Light;
/*
DSM satellite connection
1 2 3 4
pins * * * * * * *
use gnd vcc 26 103 xxx xxx xxx
DSM GND rx en
*/
extern const AP_HAL::HAL& hal;
/*
input_channels:
4, // PB14 T12/1 - PPM
5, // PB15 T12/2 - PPM2
12, // PC6 T8/1 - 6_tx
13, // PC7 T8/2 - 6_rx
14, // PC8 T8/3 - Soft_scl / soft_TX
15, // PC9 T8/4 - Soft_sda / soft_RX
*/
_parser * RCInput::parsers[MAX_RC_PARSERS] IN_CCM; // individual parsers on each PPM pin and DSM/SBUS USART
uint8_t RCInput::num_parsers IN_CCM;
uint8_t RCInput::_valid_channels IN_CCM; // = 0;
uint64_t RCInput::_last_read IN_CCM; // = 0;
uint16_t RCInput::_override[F4Light_RC_INPUT_NUM_CHANNELS] IN_CCM;
bool RCInput::_override_valid;
bool RCInput::is_PPM IN_CCM;
uint8_t RCInput::_last_read_from IN_CCM;
uint16_t RCInput::max_num_pulses IN_CCM;
bool RCInput::fs_flag IN_CCM;
bool RCInput::aibao_fs_flag IN_CCM;
bool RCInput::rc_failsafe_enabled IN_CCM;
/* constrain captured pulse to be between min and max pulsewidth. */
static inline uint16_t constrain_pulse(uint16_t p) {
if (p > RC_INPUT_MAX_PULSEWIDTH) return RC_INPUT_MAX_PULSEWIDTH;
if (p < RC_INPUT_MIN_PULSEWIDTH) return RC_INPUT_MIN_PULSEWIDTH;
return p;
}
RCInput::RCInput()
{ }
void RCInput::init() {
caddr_t ptr;
memset((void *)&_override[0], 0, sizeof(_override));
/* OPLINK AIR port pinout
1 2 3 4 5 6 7
PD2 PA15
gnd +5 26 103
used as
for DSM: rx pow
for RFM int cs
*/
is_PPM=true;
_last_read_from=0;
max_num_pulses=0;
clear_overrides();
pwmInit(is_PPM); // PPM sum mode
uint8_t pp=0;
ptr = sbrk_ccm(sizeof(PPM_parser)); // allocate memory in CCM
parsers[pp++] = new(ptr) PPM_parser;
ptr = sbrk_ccm(sizeof(PPM_parser)); // allocate memory in CCM
parsers[pp++] = new(ptr) PPM_parser;
#ifdef BOARD_SPEKTRUM_RX_PIN
ptr = sbrk_ccm(sizeof(DSM_parser)); // allocate memory in CCM
parsers[pp++] =new(ptr) DSM_parser;
#endif
#ifdef BOARD_NRF_CS_PIN
ptr = sbrk_ccm(sizeof(NRF_parser)); // allocate memory in CCM
parsers[pp++] =new(ptr) NRF_parser;
#endif
#ifdef BOARD_SBUS_UART
ptr = sbrk_ccm(sizeof(SBUS_parser)); // allocate memory in CCM
parsers[pp++] =new(ptr) SBUS_parser;
#endif
num_parsers = pp; // counter
for(uint8_t i=0; i<num_parsers;i++) {
parsers[i]->init(i);
}
}
void RCInput::late_init(uint8_t b) {
for(uint8_t i=0; i<num_parsers;i++) {
parsers[i]->late_init(b);
}
if(b & BOARD_RC_FAILSAFE) rc_failsafe_enabled=true;
}
// we can have 4 individual sources of data - internal DSM from UART5, SBUS from UART1 and 2 PPM parsers
bool RCInput::new_input()
{
if(_override_valid) return true;
uint8_t inp=hal_param_helper->_rc_input;
if(inp && inp < num_parsers+1){
inp-=1;
return parsers[inp]->get_last_signal() > _last_read;
}
for(uint8_t i=0; i<num_parsers;i++) {
if(parsers[i]->get_last_signal() >_last_read) return true;
}
return false;
}
uint8_t RCInput::num_channels()
{
return _valid_channels;
}
uint16_t RCInput::last_4=0;
//#define LOST_TIME 50 // this is wrong! Any packet lost and viola...
#define LOST_TIME 500
#define FRAME_TIME 50 // time between packets
uint16_t RCInput::_read_ppm(uint8_t ch,uint8_t n){
const _parser *p = parsers[n];
_valid_channels = p->get_valid_channels();
return p->get_val(ch);
}
uint16_t RCInput::read(uint8_t ch)
{
uint16_t data=0;
uint64_t pulse=0;
uint64_t last=0;
if(ch>=F4Light_RC_INPUT_NUM_CHANNELS) return 0;
uint64_t now=systick_uptime();
uint8_t got=0;
uint32_t dead_time = hal_param_helper->_rc_fs * 1000UL; // time in seconds
uint8_t inp=hal_param_helper->_rc_input;
if(inp && inp < num_parsers+1 ){
inp-=1;
const _parser *p = parsers[inp];
pulse = p->get_last_signal();
last = p->get_last_change();
data = p->get_val(ch);
_valid_channels = p->get_valid_channels();
got = inp+1;
} else if(now - _last_read > FRAME_TIME) { // seems that we loose 1 frame on current RC receiver so should select new one
uint32_t best_t=(uint32_t) -1;
for(uint8_t i=0; i<num_parsers;i++) {
const _parser *p = parsers[i];
uint64_t pt = p->get_last_signal();
uint64_t lt = p->get_last_change();
uint32_t dt = now-pt; // time from signal
if( pt >_last_read && // data is newer than last
dt<best_t && // and most recent
((now - lt ) < dead_time || !rc_failsafe_enabled)) // and time from last change less than DEAD_TIME
{
best_t = dt;
data = p->get_val(ch);
_valid_channels = p->get_valid_channels();
pulse = pt;
last = lt;
got = i+1;
}
}
// now we have a most recent data
}
if(got) {
_last_read_from = got;
_last_read = pulse;
} else {
if(_last_read_from) { // read from the last channel
uint8_t n = _last_read_from-1;
const _parser *p = parsers[n];
pulse = p->get_last_signal();
last = p->get_last_change();
data = p->get_val(ch);
_valid_channels = p->get_valid_channels();
_last_read = pulse;
} else { // no data at all
if( ch == 2) data = 899; // to know the source
else data = 1000;
}
}
/* Check for override */
uint16_t over = _override[ch];
if(over) return over;
if( ch == 4) {
last_4 = data;
}
if(ch == 2) { // throttle
if( (now-pulse) > LOST_TIME || // last pulse is very old
((now-last) > dead_time && rc_failsafe_enabled) ) // pulses OK but last change is very old
{
data = 900;
if(!fs_flag) {
fs_flag=true;
printf("\n failsafe! now=%lld last pulse=%lld last change=%lld\n",now, pulse, last);
}
} else {
fs_flag=false;
}
if(hal_param_helper->_aibao_fs) {
/*
Receiver-DEVO-RX719-for-Walkera-Aibao
failsafe: mode below 1000 and throttle at 1500
*/
if(last_4 < 990 && data >1300 && data < 1700){
if(!aibao_fs_flag){
aibao_fs_flag=true;
printf("\nAibao failsafe! ch4=%d ch2=%d\n",last_4, data);
}
data = 901; // to know the source
} else {
aibao_fs_flag=false;
}
}
}
return data;
}
uint8_t RCInput::read(uint16_t* periods, uint8_t len)
{
for (uint8_t i = 0; i < len; i++){
periods[i] = read(i);
}
return _valid_channels;
}
bool RCInput::set_overrides(int16_t *overrides, uint8_t len)
{
bool res = false;
for (int i = 0; i < len; i++) {
res |= set_override(i, overrides[i]);
}
return res;
}
bool RCInput::set_override(uint8_t channel, int16_t override)
{
if (override < 0) return false; /* -1: no change. */
if (channel < F4Light_RC_INPUT_NUM_CHANNELS) {
_override[channel] = override;
if (override != 0) {
_override_valid = true;
return true;
}
}
return false;
}
void RCInput::clear_overrides()
{
for (int i = 0; i < F4Light_RC_INPUT_NUM_CHANNELS; i++) {
set_override(i, 0);
}
}
bool RCInput::rc_bind(int dsmMode){
#ifdef BOARD_SPEKTRUM_RX_PIN
return parsers[2]->bind(dsmMode); // only DSM
#else
return false;
#endif
}
Reference in New Issue View Git Blame Copy Permalink