mirror of https://github.com/ArduPilot/ardupilot
229 lines
6.7 KiB
C++
229 lines
6.7 KiB
C++
/*
|
|
(c) 2017 night_ghost@ykoctpa.ru
|
|
|
|
*/
|
|
|
|
#pragma GCC optimize ("O2")
|
|
|
|
#include <AP_HAL/HAL.h>
|
|
|
|
#include <exti.h>
|
|
#include <timer.h>
|
|
#include "RCInput.h"
|
|
#include <pwm_in.h>
|
|
#include <AP_HAL/utility/dsm.h>
|
|
#include <AP_HAL/utility/sumd.h>
|
|
#include "sbus.h"
|
|
#include "GPIO.h"
|
|
#include "ring_buffer_pulse.h"
|
|
|
|
#include "RC_DSM_parser.h"
|
|
|
|
using namespace F4Light;
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
#if defined(BOARD_DSM_USART)
|
|
UARTDriver DSM_parser::uartSDriver(BOARD_DSM_USART); // UART connected to DSM pin
|
|
#elif defined(BOARD_USART5_RX_PIN)
|
|
UARTDriver DSM_parser::uartSDriver(_UART5);
|
|
#endif
|
|
|
|
|
|
|
|
void DSM_parser::init(uint8_t ch) {
|
|
#if defined(BOARD_SPEKTRUM_RX_PIN)
|
|
|
|
memset((void *)&_val[0], 0, sizeof(_val));
|
|
memset((void *)&dsm, 0, sizeof(dsm));
|
|
|
|
_last_signal=0;
|
|
_last_change =0;
|
|
|
|
uint32_t sig = board_get_rtc_register(RTC_DSM_BIND_REG);
|
|
if( (sig & ~DSM_BIND_SIGN_MASK) == DSM_BIND_SIGNATURE) {
|
|
board_set_rtc_register(0, RTC_DSM_BIND_REG);
|
|
_rc_bind(sig & DSM_BIND_SIGN_MASK);
|
|
}
|
|
|
|
GPIO::_pinMode(BOARD_SPEKTRUM_RX_PIN, INPUT_PULLUP);
|
|
#ifdef BOARD_SPEKTRUM_PWR_PIN
|
|
GPIO::_pinMode(BOARD_SPEKTRUM_PWR_PIN, OUTPUT);
|
|
GPIO::_write(BOARD_SPEKTRUM_PWR_PIN, BOARD_SPEKTRUM_PWR_ON);
|
|
#endif
|
|
|
|
_ioc = Scheduler::register_io_completion(FUNCTOR_BIND_MEMBER(&DSM_parser::_io_completion, void));
|
|
|
|
uartSDriver.end(); // just for case
|
|
|
|
// initialize DSM UART
|
|
uartSDriver.begin(115200);
|
|
Revo_handler h = { .mp = FUNCTOR_BIND_MEMBER(&DSM_parser::add_dsm_uart_input, void) };
|
|
uartSDriver.setCallback(h.h);
|
|
}
|
|
|
|
/*
|
|
add some bytes of input in DSM serial stream format, coping with partial packets - UART input callback
|
|
*/
|
|
void DSM_parser::add_dsm_uart_input() {
|
|
if(_ioc) {
|
|
Scheduler::do_io_completion(_ioc);
|
|
}
|
|
}
|
|
|
|
void DSM_parser::_io_completion(){
|
|
|
|
while(uartSDriver.available()){
|
|
|
|
// at least 1 byte we have
|
|
const uint8_t dsm_frame_size = sizeof(dsm.frame);
|
|
|
|
uint32_t now = AP_HAL::millis();
|
|
if (now - dsm.last_input_ms > 5) {
|
|
// resync based on time
|
|
dsm.partial_frame_count = 0;
|
|
}
|
|
dsm.last_input_ms = now;
|
|
|
|
if (dsm.partial_frame_count + 1 > dsm_frame_size) {
|
|
return; // we can't add bytes to buffer
|
|
}
|
|
|
|
|
|
char c= uartSDriver.read();
|
|
|
|
if(state != S_DSM) { // try to decode SUMD data
|
|
uint16_t values[F4Light_RC_INPUT_NUM_CHANNELS];
|
|
uint8_t rssi;
|
|
uint8_t rx_count;
|
|
uint16_t channel_count;
|
|
|
|
if (sumd_decode(c, &rssi, &rx_count, &channel_count, values, F4Light_RC_INPUT_NUM_CHANNELS) == 0) {
|
|
if (channel_count > F4Light_RC_INPUT_NUM_CHANNELS) {
|
|
continue;
|
|
}
|
|
state=S_SUMD;
|
|
for (uint8_t i=0; i<channel_count; i++) {
|
|
if (values[i] != 0) {
|
|
if(_val[i] != values[i]) _last_change = systick_uptime();
|
|
_val[i] = values[i];
|
|
}
|
|
}
|
|
_channels = channel_count + 1;
|
|
_last_signal = systick_uptime();
|
|
_val[channel_count] = rssi; // say about RSSI in last channel
|
|
}
|
|
}
|
|
|
|
if(state!=S_SUMD) {
|
|
dsm.frame[dsm.partial_frame_count] = c;
|
|
dsm.partial_frame_count += 1;
|
|
|
|
if (dsm.partial_frame_count == dsm_frame_size) {
|
|
dsm.partial_frame_count = 0;
|
|
uint16_t values[F4Light_RC_INPUT_NUM_CHANNELS] {};
|
|
uint16_t num_values=0;
|
|
if (dsm_decode(AP_HAL::micros64(), dsm.frame, values, &num_values, 16) &&
|
|
num_values >= 5 && num_values <F4Light_RC_INPUT_NUM_CHANNELS) {
|
|
state=S_DSM;
|
|
for (uint8_t i=0; i<num_values; i++) {
|
|
if (values[i] != 0) {
|
|
if(_val[i] != values[i]) _last_change = systick_uptime();
|
|
_val[i] = values[i];
|
|
}
|
|
}
|
|
/*
|
|
the apparent number of channels can change on DSM,
|
|
as they are spread across multiple frames. We just
|
|
use the max num_values we get
|
|
*/
|
|
uint8_t nc = num_values+1;
|
|
if (nc > _channels) {
|
|
_channels = nc;
|
|
}
|
|
/*
|
|
DSM frame from datasheet:
|
|
typedef stuct {
|
|
UINT8 fades;
|
|
UINT8 system;
|
|
UINT16 servo[7];
|
|
} INT_REMOTE_STR;
|
|
|
|
so we get RSSI from 1st byte and store it to last channel
|
|
*/
|
|
_val[_channels-1] = dsm.frame[0];
|
|
|
|
_last_signal = systick_uptime();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif // defined(BOARD_SPEKTRUM_RX_PIN)
|
|
}
|
|
|
|
|
|
|
|
#ifdef BOARD_SPEKTRUM_RX_PIN
|
|
void DSM_parser::_rc_bind(uint16_t dsmMode){
|
|
/*
|
|
To put a receiver into bind mode, within 200ms of power application the host device needs to issue a
|
|
series of falling pulses. The number of pulses issued selects which bind types will be accepted from
|
|
the transmitter. Selecting the 11ms mode automatically allows either 11ms or 22ms protocols to be
|
|
used. Selecting DSMX will automatically allow DSM2 to be used if the transmitter is unable to
|
|
support DSMX. For this reason, we recommend that 11ms DSMX be used (9 (“internal”) or 10
|
|
(“external”) pulses).
|
|
DSMX Bind Modes:
|
|
Pulses Mode Protocol Type
|
|
7 Internal DSMx 22ms
|
|
8 External DSMx 22ms
|
|
9 Internal DSMx 11ms
|
|
10 External DSMx 11ms
|
|
|
|
see https://github.com/SpektrumFPV/SpektrumDocumentation/blob/master/Telemetry/Remote%20Receiver%20Interfacing.pdf
|
|
|
|
*/
|
|
|
|
|
|
Scheduler::_delay(72);
|
|
|
|
for (int i = 0; i < dsmMode; i++) { /*Pulse RX pin a number of times*/
|
|
Scheduler::_delay_microseconds(120);
|
|
GPIO::_write(BOARD_SPEKTRUM_RX_PIN, 0);
|
|
Scheduler::_delay_microseconds(120);
|
|
GPIO::_write(BOARD_SPEKTRUM_RX_PIN, 1);
|
|
}
|
|
|
|
Scheduler::_delay(50);
|
|
}
|
|
|
|
|
|
|
|
bool DSM_parser::bind(int dsmMode) const {
|
|
#ifdef BOARD_SPEKTRUM_PWR_PIN
|
|
uartSDriver.end();
|
|
|
|
GPIO::_write(BOARD_SPEKTRUM_PWR_PIN, BOARD_SPEKTRUM_PWR_OFF); /* power down DSM satellite */
|
|
|
|
Scheduler::_delay(500);
|
|
|
|
|
|
GPIO::_write(BOARD_SPEKTRUM_PWR_PIN, BOARD_SPEKTRUM_PWR_ON); /* power up DSM satellite*/
|
|
|
|
GPIO::_pinMode(BOARD_SPEKTRUM_RX_PIN, OUTPUT); /*Set UART RX pin to active output mode*/
|
|
|
|
_rc_bind(dsmMode);
|
|
|
|
uartSDriver.begin(115200); /*Restore USART RX pin to RS232 receive mode*/
|
|
|
|
#else
|
|
// store request to bing in BACKUP RAM
|
|
board_set_rtc_register(DSM_BIND_SIGNATURE | ( dsmMode & DSM_BIND_SIGN_MASK), RTC_DSM_BIND_REG);
|
|
|
|
#endif
|
|
return true;
|
|
}
|
|
|
|
#endif // BOARD_SPEKTRUM_RX_PIN
|