/* (c) 2017 night_ghost@ykoctpa.ru */ #include #include #include #include #include "RCInput.h" #include #include #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; iinit(i); } } void RCInput::late_init(uint8_t b) { for(uint8_t i=0; ilate_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; iget_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; iget_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 dtget_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 }