mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-02-04 22:58:10 +00:00
353 lines
9.5 KiB
C++
353 lines
9.5 KiB
C++
/*
|
|
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/>.
|
|
*/
|
|
// compatible with E012 and E015
|
|
|
|
#if defined(E01X_NRF24L01_INO)
|
|
|
|
#include "iface_nrf24l01.h"
|
|
|
|
//Protocols constants
|
|
#define E01X_BIND_COUNT 500
|
|
#define E01X_INITIAL_WAIT 500
|
|
#define E01X_ADDRESS_LENGTH 5
|
|
|
|
#define E012_PACKET_PERIOD 4525
|
|
#define E012_RF_BIND_CHANNEL 0x3c
|
|
#define E012_NUM_RF_CHANNELS 4
|
|
#define E012_PACKET_SIZE 15
|
|
|
|
#define E015_PACKET_PERIOD 4500 // stock Tx=9000, but let's send more packets ...
|
|
#define E015_RF_CHANNEL 0x2d // 2445 MHz
|
|
#define E015_PACKET_SIZE 10
|
|
#define E015_BIND_PACKET_SIZE 9
|
|
|
|
#define E016H_PACKET_PERIOD 4080
|
|
#define E016H_PACKET_SIZE 10
|
|
#define E016H_BIND_CHANNEL 80
|
|
#define E016H_NUM_CHANNELS 4
|
|
|
|
//Channels
|
|
#define E01X_ARM_SW CH5_SW
|
|
#define E016H_STOP_SW CH5_SW
|
|
#define E01X_FLIP_SW CH6_SW
|
|
#define E01X_LED_SW CH7_SW
|
|
#define E01X_HEADLESS_SW CH8_SW
|
|
#define E01X_RTH_SW CH9_SW
|
|
|
|
// E012 flags packet[1]
|
|
#define E012_FLAG_FLIP 0x40
|
|
#define E012_FLAG_HEADLESS 0x10
|
|
#define E012_FLAG_RTH 0x04
|
|
// E012 flags packet[7]
|
|
#define E012_FLAG_EXPERT 0x02
|
|
|
|
// E015 flags packet[6]
|
|
#define E015_FLAG_DISARM 0x80
|
|
#define E015_FLAG_ARM 0x40
|
|
// E015 flags packet[7]
|
|
#define E015_FLAG_FLIP 0x80
|
|
#define E015_FLAG_HEADLESS 0x10
|
|
#define E015_FLAG_RTH 0x08
|
|
#define E015_FLAG_LED 0x04
|
|
#define E015_FLAG_EXPERT 0x02
|
|
#define E015_FLAG_INTERMEDIATE 0x01
|
|
|
|
// E016H flags packet[1]
|
|
#define E016H_FLAG_CALIBRATE 0x80
|
|
#define E016H_FLAG_STOP 0x20
|
|
#define E016H_FLAG_FLIP 0x04
|
|
// E016H flags packet[3]
|
|
#define E016H_FLAG_HEADLESS 0x10
|
|
#define E016H_FLAG_RTH 0x04
|
|
// E016H flags packet[7]
|
|
#define E016H_FLAG_TAKEOFF 0x80
|
|
#define E016H_FLAG_HIGHRATE 0x08
|
|
|
|
static void __attribute__((unused)) E015_check_arming()
|
|
{
|
|
uint8_t arm_channel = E01X_ARM_SW;
|
|
|
|
if (arm_channel != arm_channel_previous)
|
|
{
|
|
arm_channel_previous = arm_channel;
|
|
if (arm_channel)
|
|
{
|
|
armed = 1;
|
|
arm_flags ^= E015_FLAG_ARM;
|
|
}
|
|
else
|
|
{
|
|
armed = 0;
|
|
arm_flags ^= E015_FLAG_DISARM;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void __attribute__((unused)) E01X_send_packet(uint8_t bind)
|
|
{
|
|
uint8_t can_flip = 0, calibrate = 1;
|
|
if(sub_protocol==E012)
|
|
{
|
|
packet_length=E012_PACKET_SIZE;
|
|
packet[0] = rx_tx_addr[1];
|
|
if(bind)
|
|
{
|
|
packet[1] = 0xaa;
|
|
memcpy(&packet[2], hopping_frequency, E012_NUM_RF_CHANNELS);
|
|
memcpy(&packet[6], rx_tx_addr, E01X_ADDRESS_LENGTH);
|
|
rf_ch_num=E012_RF_BIND_CHANNEL;
|
|
}
|
|
else
|
|
{
|
|
packet[1] = 0x01
|
|
| GET_FLAG(E01X_RTH_SW, E012_FLAG_RTH)
|
|
| GET_FLAG(E01X_HEADLESS_SW, E012_FLAG_HEADLESS)
|
|
| GET_FLAG(E01X_FLIP_SW, E012_FLAG_FLIP);
|
|
packet[2] = convert_channel_16b_limit(AILERON, 0xc8, 0x00); // aileron
|
|
packet[3] = convert_channel_16b_limit(ELEVATOR, 0x00, 0xc8); // elevator
|
|
packet[4] = convert_channel_16b_limit(RUDDER, 0xc8, 0x00); // rudder
|
|
packet[5] = convert_channel_16b_limit(THROTTLE, 0x00, 0xc8); // throttle
|
|
packet[6] = 0xaa;
|
|
packet[7] = E012_FLAG_EXPERT; // rate (0-2)
|
|
packet[8] = 0x00;
|
|
packet[9] = 0x00;
|
|
packet[10]= 0x00;
|
|
rf_ch_num=hopping_frequency[hopping_frequency_no++];
|
|
hopping_frequency_no %= E012_NUM_RF_CHANNELS;
|
|
}
|
|
packet[11] = 0x00;
|
|
packet[12] = 0x00;
|
|
packet[13] = 0x56;
|
|
packet[14] = rx_tx_addr[2];
|
|
}
|
|
else if(sub_protocol==E015)
|
|
{ // E015
|
|
if(bind)
|
|
{
|
|
packet[0] = 0x18;
|
|
packet[1] = 0x04;
|
|
packet[2] = 0x06;
|
|
// data phase address
|
|
memcpy(&packet[3], rx_tx_addr, E01X_ADDRESS_LENGTH);
|
|
// checksum
|
|
packet[8] = packet[3];
|
|
for(uint8_t i=4; i<8; i++)
|
|
packet[8] += packet[i];
|
|
packet_length=E015_BIND_PACKET_SIZE;
|
|
}
|
|
else
|
|
{
|
|
E015_check_arming();
|
|
packet[0] = convert_channel_16b_limit(THROTTLE, 0, 225); // throttle
|
|
packet[1] = convert_channel_16b_limit(RUDDER, 225, 0); // rudder
|
|
packet[2] = convert_channel_16b_limit(AILERON, 0, 225); // aileron
|
|
packet[3] = convert_channel_16b_limit(ELEVATOR, 225, 0); // elevator
|
|
packet[4] = 0x20; // elevator trim
|
|
packet[5] = 0x20; // aileron trim
|
|
packet[6] = arm_flags;
|
|
packet[7] = E015_FLAG_EXPERT
|
|
| GET_FLAG(E01X_FLIP_SW, E015_FLAG_FLIP)
|
|
| GET_FLAG(E01X_LED_SW, E015_FLAG_LED)
|
|
| GET_FLAG(E01X_HEADLESS_SW,E015_FLAG_HEADLESS)
|
|
| GET_FLAG(E01X_RTH_SW, E015_FLAG_RTH);
|
|
packet[8] = 0;
|
|
// checksum
|
|
packet[9] = packet[0];
|
|
for(uint8_t i=1; i<9; i++)
|
|
packet[9] += packet[i];
|
|
packet_length=E015_PACKET_SIZE;
|
|
}
|
|
}
|
|
else
|
|
{ // E016H
|
|
packet_length=E016H_PACKET_SIZE;
|
|
if(bind)
|
|
{
|
|
rf_ch_num=E016H_BIND_CHANNEL;
|
|
memcpy(packet, &rx_tx_addr[1], 4);
|
|
memcpy(&packet[4], hopping_frequency, 4);
|
|
packet[8] = 0x23;
|
|
}
|
|
else
|
|
{
|
|
// trim commands
|
|
packet[0] = 0;
|
|
// aileron
|
|
uint16_t val = convert_channel_16b_limit(AILERON, 0, 0x3ff);
|
|
can_flip |= (val < 0x100) || (val > 0x300);
|
|
packet[1] = val >> 8;
|
|
packet[2] = val & 0xff;
|
|
if(val < 0x300) calibrate = 0;
|
|
// elevator
|
|
val = convert_channel_16b_limit(ELEVATOR, 0x3ff, 0);
|
|
can_flip |= (val < 0x100) || (val > 0x300);
|
|
packet[3] = val >> 8;
|
|
packet[4] = val & 0xff;
|
|
if(val < 0x300) calibrate = 0;
|
|
// throttle
|
|
val = convert_channel_16b_limit(THROTTLE, 0, 0x3ff);
|
|
packet[5] = val >> 8;
|
|
packet[6] = val & 0xff;
|
|
if(val > 0x100) calibrate = 0;
|
|
// rudder
|
|
val = convert_channel_16b_limit(RUDDER, 0, 0x3ff);
|
|
packet[7] = val >> 8;
|
|
packet[8] = val & 0xff;
|
|
if(val > 0x100) calibrate = 0;
|
|
// flags
|
|
packet[1] |= GET_FLAG(E016H_STOP_SW, E016H_FLAG_STOP)
|
|
| (can_flip ? GET_FLAG(E01X_FLIP_SW, E016H_FLAG_FLIP) : 0)
|
|
| (calibrate ? E016H_FLAG_CALIBRATE : 0);
|
|
packet[3] |= GET_FLAG(E01X_HEADLESS_SW, E016H_FLAG_HEADLESS)
|
|
| GET_FLAG(E01X_RTH_SW, E016H_FLAG_RTH);
|
|
packet[7] |= E016H_FLAG_HIGHRATE;
|
|
// frequency hopping
|
|
rf_ch_num=hopping_frequency[hopping_frequency_no++ & 0x03];
|
|
}
|
|
// checksum
|
|
packet[9] = packet[0];
|
|
for (uint8_t i=1; i < E016H_PACKET_SIZE-1; i++)
|
|
packet[9] += packet[i];
|
|
}
|
|
|
|
// Power on, TX mode, CRC enabled
|
|
if(sub_protocol==E016H)
|
|
XN297_Configure( _BV(NRF24L01_00_EN_CRC) | _BV(NRF24L01_00_CRCO) | _BV(NRF24L01_00_PWR_UP));
|
|
else //E012 & E015
|
|
HS6200_Configure(_BV(NRF24L01_00_EN_CRC) | _BV(NRF24L01_00_CRCO) | _BV(NRF24L01_00_PWR_UP));
|
|
NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_ch_num);
|
|
|
|
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70);
|
|
NRF24L01_FlushTx();
|
|
|
|
if(sub_protocol==E016H)
|
|
XN297_WritePayload(packet, packet_length);
|
|
else
|
|
HS6200_WritePayload(packet, packet_length);
|
|
|
|
// Check and adjust transmission power. We do this after
|
|
// transmission to not bother with timeout after power
|
|
// settings change - we have plenty of time until next
|
|
// packet.
|
|
NRF24L01_SetPower();
|
|
}
|
|
|
|
static void __attribute__((unused)) E01X_init()
|
|
{
|
|
NRF24L01_Initialize();
|
|
NRF24L01_SetTxRxMode(TX_EN);
|
|
if(sub_protocol==E012)
|
|
HS6200_SetTXAddr((uint8_t *)"\x55\x42\x9C\x8F\xC9", E01X_ADDRESS_LENGTH);
|
|
else if(sub_protocol==E015)
|
|
HS6200_SetTXAddr((uint8_t *)"\x62\x54\x79\x38\x53", E01X_ADDRESS_LENGTH);
|
|
else //E016H
|
|
XN297_SetTXAddr((uint8_t *)"\x5a\x53\x46\x30\x31", 5); // bind address
|
|
NRF24L01_FlushTx();
|
|
NRF24L01_FlushRx();
|
|
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
|
|
NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknowldgement on all data pipes
|
|
NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03);
|
|
NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x00); // no retransmits
|
|
NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1 Mbps
|
|
NRF24L01_SetPower();
|
|
NRF24L01_Activate(0x73); // Activate feature register
|
|
NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x00); // Disable dynamic payload length on all pipes
|
|
NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x01); // Set feature bits on
|
|
NRF24L01_Activate(0x73);
|
|
}
|
|
|
|
uint16_t E01X_callback()
|
|
{
|
|
if(IS_BIND_IN_PROGRESS)
|
|
{
|
|
if (bind_counter == 0)
|
|
{
|
|
if(sub_protocol==E016H)
|
|
XN297_SetTXAddr(rx_tx_addr, E01X_ADDRESS_LENGTH);
|
|
else
|
|
HS6200_SetTXAddr(rx_tx_addr, E01X_ADDRESS_LENGTH);
|
|
BIND_DONE;
|
|
}
|
|
else
|
|
{
|
|
E01X_send_packet(1);
|
|
bind_counter--;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
#ifdef MULTI_SYNC
|
|
telemetry_set_input_sync(packet_period);
|
|
#endif
|
|
E01X_send_packet(0);
|
|
}
|
|
return packet_period;
|
|
}
|
|
|
|
static void __attribute__((unused)) E012_initialize_txid()
|
|
{
|
|
// rf channels
|
|
uint32_t lfsr=random(0xfefefefe);
|
|
for(uint8_t i=0; i<E012_NUM_RF_CHANNELS; i++)
|
|
{
|
|
hopping_frequency[i] = 0x10 + ((lfsr & 0xff) % 0x32);
|
|
lfsr>>=8;
|
|
}
|
|
}
|
|
|
|
static void __attribute__((unused)) E016H_initialize_txid()
|
|
{
|
|
// tx id
|
|
rx_tx_addr[0] = 0xa5;
|
|
rx_tx_addr[1] = 0x00;
|
|
|
|
// rf channels
|
|
uint32_t lfsr=random(0xfefefefe);
|
|
for(uint8_t i=0; i<E016H_NUM_CHANNELS; i++)
|
|
{
|
|
hopping_frequency[i] = (lfsr & 0xFF) % 80;
|
|
lfsr>>=8;
|
|
}
|
|
}
|
|
|
|
uint16_t initE01X()
|
|
{
|
|
BIND_IN_PROGRESS;
|
|
if(sub_protocol==E012)
|
|
{
|
|
E012_initialize_txid();
|
|
packet_period=E012_PACKET_PERIOD;
|
|
}
|
|
else if(sub_protocol==E015)
|
|
{
|
|
packet_period=E015_PACKET_PERIOD;
|
|
rf_ch_num=E015_RF_CHANNEL;
|
|
armed = 0;
|
|
arm_flags = 0;
|
|
arm_channel_previous = E01X_ARM_SW;
|
|
}
|
|
else
|
|
{ // E016H
|
|
E016H_initialize_txid();
|
|
packet_period=E016H_PACKET_PERIOD;
|
|
}
|
|
E01X_init();
|
|
bind_counter = E01X_BIND_COUNT;
|
|
hopping_frequency_no = 0;
|
|
return E01X_INITIAL_WAIT;
|
|
}
|
|
|
|
#endif
|