379 lines
11 KiB
Arduino
Raw Normal View History

2015-12-30 01:41:12 +01:00
/*
This project is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Multiprotocol is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
*/
#if defined(SYMAX_NRF24L01_INO)
#include "iface_nrf24l01.h"
/***
Main protocol compatible with Syma X5C-1, X11, X11C, X12.
SymaX5C protocol option compatible with Syma X5C (original) and X2.
***/
#define SYMAX_BIND_COUNT 345 // 1.5 seconds
#define SYMAX_FIRST_PACKET_DELAY 12000
#define SYMAX_PACKET_PERIOD 4000 // Timeout for callback in uSec
#define SYMAX_INITIAL_WAIT 500
#define SYMAX_MAX_RF_CHANNELS 17
#define SYMAX_FLAG_FLIP 0x01
#define SYMAX_FLAG_VIDEO 0x02
#define SYMAX_FLAG_PICTURE 0x04
#define SYMAX_FLAG_HEADLESS 0x08
#define SYMAX_PAYLOADSIZE 10 // receive data pipes set to this size, but unused
#define SYMAX_MAX_PACKET_LENGTH 16 // X11,X12,X5C-1 10-byte, X5C 16-byte
enum {
SYMAX_INIT1 = 0,
SYMAX_BIND2,
SYMAX_BIND3,
SYMAX_DATA
};
/*
http://www.deviationtx.com/forum/protocol-development/3768-syma-x5c-1-x11-x12?start=140
TX address Channel Sequence
S1 3B B6 00 00 A2 15 35 1D 3D
D1 9A E9 02 00 A2 14 34 1C 3C
D2 46 18 00 00 A2 11 21 31 41
*/
uint8_t SYMAX_checksum(uint8_t *data)
{
uint8_t sum = data[0];
for (uint8_t i=1; i < packet_length-1; i++)
if ( sub_protocol==SYMAX5C )
sum += data[i];
else
sum ^= data[i];
return sum + ( sub_protocol==SYMAX5C ? 0 : 0x55 );
}
void SYMAX_read_controls()
{
// Protocol is registered AETRF, that is
// Aileron is channel 1, Elevator - 2, Throttle - 3, Rudder - 4, Flip control - 5
aileron = convert_channel_s8b(AILERON);
elevator = convert_channel_s8b(ELEVATOR);
throttle = convert_channel_8b(THROTTLE);
rudder = convert_channel_s8b(RUDDER);
// Channel 5
if (Servo_data[AUX1] > PPM_SWITCH)
flags = SYMAX_FLAG_FLIP;
else
flags=0;
// Channel 7
if (Servo_data[AUX3] > PPM_SWITCH)
flags |= SYMAX_FLAG_PICTURE;
// Channel 8
if (Servo_data[AUX4] > PPM_SWITCH)
flags |= SYMAX_FLAG_VIDEO;
// Channel 9
if (Servo_data[AUX5] > PPM_SWITCH)
flags |= SYMAX_FLAG_HEADLESS;
}
#define X5C_CHAN2TRIM(X) ((((X) & 0x80 ? 0xff - (X) : 0x80 + (X)) >> 2) + 0x20)
void SYMAX_build_packet_x5c(uint8_t bind)
{
if (bind)
{
memset(packet, 0, packet_length);
packet[7] = 0xae;
packet[8] = 0xa9;
packet[14] = 0xc0;
packet[15] = 0x17;
}
else
{
SYMAX_read_controls();
packet[0] = throttle;
packet[1] = rudder;
packet[2] = elevator ^ 0x80; // reversed from default
packet[3] = aileron;
packet[4] = X5C_CHAN2TRIM(rudder ^ 0x80);// drive trims for extra control range
packet[5] = X5C_CHAN2TRIM(elevator);
packet[6] = X5C_CHAN2TRIM(aileron ^ 0x80);
packet[7] = 0xae;
packet[8] = 0xa9;
packet[9] = 0x00;
packet[10] = 0x00;
packet[11] = 0x00;
packet[12] = 0x00;
packet[13] = 0x00;
packet[14] = (flags & SYMAX_FLAG_VIDEO ? 0x10 : 0x00)
| (flags & SYMAX_FLAG_PICTURE ? 0x08 : 0x00)
| (flags & SYMAX_FLAG_FLIP ? 0x01 : 0x00)
| 0x04;// (flags & SYMAX_FLAG_RATES ? 0x04 : 0x00);
packet[15] = SYMAX_checksum(packet);
}
}
void SYMAX_build_packet(uint8_t bind)
{
if (bind)
{
packet[0] = rx_tx_addr[4];
packet[1] = rx_tx_addr[3];
packet[2] = rx_tx_addr[2];
packet[3] = rx_tx_addr[1];
packet[4] = rx_tx_addr[0];
packet[5] = 0xaa;
packet[6] = 0xaa;
packet[7] = 0xaa;
packet[8] = 0x00;
}
else
{
SYMAX_read_controls();
packet[0] = throttle;
packet[1] = elevator;
packet[2] = rudder;
packet[3] = aileron;
packet[4] = (flags & SYMAX_FLAG_VIDEO ? 0x80 : 0x00) | (flags & SYMAX_FLAG_PICTURE ? 0x40 : 0x00);
packet[5] = (elevator >> 2) | 0xc0; //always high rates (bit 7 is rate control) (flags & SYMAX_FLAG_RATES ? 0x80 : 0x00) | 0x40; // use trims to extend controls
packet[6] = (rudder >> 2) | (flags & SYMAX_FLAG_FLIP ? 0x40 : 0x00);
packet[7] = (aileron >> 2) | (flags & SYMAX_FLAG_HEADLESS ? 0x80 : 0x00);
packet[8] = 0x00;
}
packet[9] = SYMAX_checksum(packet);
}
void SYMAX_send_packet(uint8_t bind)
{
if (sub_protocol==SYMAX5C)
SYMAX_build_packet_x5c(bind);
else
SYMAX_build_packet(bind);
// clear packet status bits and TX FIFO
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70);
NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x2e);
NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency[hopping_frequency_no]);
NRF24L01_FlushTx();
NRF24L01_WritePayload(packet, packet_length);
if (packet_counter++ % 2) // use each channel twice
hopping_frequency_no = (hopping_frequency_no + 1) % rf_ch_num;
NRF24L01_SetPower(); // Set tx_power
}
static void symax_init()
{
NRF24L01_Initialize();
//
NRF24L01_SetTxRxMode(TX_EN);
//
NRF24L01_ReadReg(NRF24L01_07_STATUS);
NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO));
NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknoledgement
NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x3F); // Enable all data pipes (even though not used?)
NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address
NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0xff); // 4mS retransmit t/o, 15 tries (retries w/o AA?)
NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x08);
if (sub_protocol==SYMAX5C)
{
NRF24L01_SetBitrate(NRF24L01_BR_1M);
packet_length = 16;
}
else
{
NRF24L01_SetBitrate(NRF24L01_BR_250K);
packet_length = 10;
}
//
NRF24L01_SetPower();
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
NRF24L01_WriteReg(NRF24L01_08_OBSERVE_TX, 0x00);
NRF24L01_WriteReg(NRF24L01_09_CD, 0x00);
NRF24L01_WriteReg(NRF24L01_0C_RX_ADDR_P2, 0xC3); // LSB byte of pipe 2 receive address
NRF24L01_WriteReg(NRF24L01_0D_RX_ADDR_P3, 0xC4);
NRF24L01_WriteReg(NRF24L01_0E_RX_ADDR_P4, 0xC5);
NRF24L01_WriteReg(NRF24L01_0F_RX_ADDR_P5, 0xC6);
NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, SYMAX_PAYLOADSIZE); // bytes of data payload for pipe 1
NRF24L01_WriteReg(NRF24L01_12_RX_PW_P1, SYMAX_PAYLOADSIZE);
NRF24L01_WriteReg(NRF24L01_13_RX_PW_P2, SYMAX_PAYLOADSIZE);
NRF24L01_WriteReg(NRF24L01_14_RX_PW_P3, SYMAX_PAYLOADSIZE);
NRF24L01_WriteReg(NRF24L01_15_RX_PW_P4, SYMAX_PAYLOADSIZE);
NRF24L01_WriteReg(NRF24L01_16_RX_PW_P5, SYMAX_PAYLOADSIZE);
NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR , sub_protocol==SYMAX5C ? (uint8_t *)"\x6D\x6A\x73\x73\x73" : (uint8_t *)"\xAB\xAC\xAD\xAE\xAF" ,5);
NRF24L01_ReadReg(NRF24L01_07_STATUS);
NRF24L01_FlushTx();
NRF24L01_ReadReg(NRF24L01_07_STATUS);
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x0e);
NRF24L01_ReadReg(NRF24L01_00_CONFIG);
NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0c);
NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0e); // power on
}
void symax_init1()
{
// duplicate stock tx sending strange packet (effect unknown)
uint8_t first_packet[] = {0xf9, 0x96, 0x82, 0x1b, 0x20, 0x08, 0x08, 0xf2, 0x7d, 0xef, 0xff, 0x00, 0x00, 0x00, 0x00};
uint8_t chans_bind[] = {0x4b, 0x30, 0x40, 0x20};
uint8_t chans_bind_x5c[] = {0x27, 0x1b, 0x39, 0x28, 0x24, 0x22, 0x2e, 0x36,
0x19, 0x21, 0x29, 0x14, 0x1e, 0x12, 0x2d, 0x18};
//uint8_t data_rx_tx_addr[] = {0x3b,0xb6,0x00,0x00,0xa2};
//uint8_t data_rx_tx_addr[] = {0x9A,0xe9,0x03,0x00,0xa2};//<<---- is ok
//uint8_t data_rx_tx_addr[] = {0x3b,0xb6,0x00,0x00,0xa2};//<<--- is ok
//uint8_t data_rx_tx_addr[] = {0x9A,0xe9,0x00,0x00,0xa2};
//uint8_t data_rx_tx_addr[] = {0x9A,0xe9,0x03,0x00,0xa2};//<<---- is ok
//uint8_t data_rx_tx_addr[] = {0x46,0x18,0x00,0x00,0xa2};
NRF24L01_FlushTx();
NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x08);
NRF24L01_WritePayload(first_packet, 15);
if (sub_protocol==SYMAX5C)
{
rf_ch_num = sizeof(chans_bind_x5c);
memcpy(hopping_frequency, chans_bind_x5c, rf_ch_num);
}
else
{
rx_tx_addr[4] = 0xa2; // this is constant in ID
rf_ch_num = sizeof(chans_bind);
memcpy(hopping_frequency, chans_bind, rf_ch_num);
}
hopping_frequency_no = 0;
packet_counter = 0;
}
// channels determined by last byte of tx address
void symax_set_channels(uint8_t address)
{
static const uint8_t start_chans_1[] = {0x0a, 0x1a, 0x2a, 0x3a};
static const uint8_t start_chans_2[] = {0x2a, 0x0a, 0x42, 0x22};
static const uint8_t start_chans_3[] = {0x1a, 0x3a, 0x12, 0x32};
//static const uint8_t start_chans_4[] = {0x15, 0x35, 0x1d, 0x3d};
//static const uint8_t start_chans_5[] = {0x14, 0x34, 0x1c, 0x3c};
//static const uint8_t start_chans_6[] = {0x11, 0x21, 0x31, 0x41};
uint8_t laddress = address & 0x1f;
uint8_t i;
uint32_t *pchans = (uint32_t *)hopping_frequency; // avoid compiler warning
rf_ch_num = 4;
if (laddress < 0x10)
{
if (laddress == 6)
laddress = 7;
for(i=0; i < rf_ch_num; i++)
hopping_frequency[i] = start_chans_1[i] + laddress;
}
else
if (laddress < 0x18)
{
for(i=0; i < rf_ch_num; i++)
hopping_frequency[i] = start_chans_2[i] + (laddress & 0x07);
if (laddress == 0x16)
{
hopping_frequency[0]++;
hopping_frequency[1]++;
}
}
else
if (laddress < 0x1e)
{
for(i=0; i < rf_ch_num; i++)
hopping_frequency[i] = start_chans_3[i] + (laddress & 0x07);
}
else
if (laddress == 0x1e)
*pchans = 0x38184121;
else
*pchans = 0x39194121;
}
void symax_init2()
{
uint8_t chans_data_x5c[] = {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24,
0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22};
if (sub_protocol==SYMAX5C)
{
rf_ch_num = sizeof(chans_data_x5c);
memcpy(hopping_frequency, chans_data_x5c, rf_ch_num);
}
else
{
symax_set_channels(rx_tx_addr[0]);
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
}
hopping_frequency_no = 0;
packet_counter = 0;
}
uint16_t symax_callback()
{
switch (phase)
{
case SYMAX_INIT1:
symax_init1();
phase = SYMAX_BIND2;
return SYMAX_FIRST_PACKET_DELAY;
break;
case SYMAX_BIND2:
counter = SYMAX_BIND_COUNT;
phase = SYMAX_BIND3;
SYMAX_send_packet(1);
break;
case SYMAX_BIND3:
if (counter == 0)
{
symax_init2();
phase = SYMAX_DATA;
BIND_DONE;
}
else
{
SYMAX_send_packet(1);
counter--;
}
break;
case SYMAX_DATA:
SYMAX_send_packet(0);
break;
}
return SYMAX_PACKET_PERIOD;
}
uint16_t initSymax()
{
packet_counter = 0;
flags = 0;
BIND_IN_PROGRESS; // autobind protocol
symax_init();
phase = SYMAX_INIT1;
return SYMAX_INITIAL_WAIT;
}
#endif