mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-02-04 19:48:11 +00:00
257 lines
7.5 KiB
C++
257 lines
7.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/>.
|
|
*/
|
|
|
|
#if defined(YD717_NRF24L01_INO)
|
|
|
|
#include "iface_nrf24l01.h"
|
|
|
|
#define YD717_BIND_COUNT 60
|
|
#define YD717_PACKET_PERIOD 8000 // Timeout for callback in uSec, 8ms=8000us for YD717
|
|
#define YD717_INITIAL_WAIT 50000 // Initial wait before starting callbacks
|
|
#define YD717_PACKET_CHKTIME 500 // Time to wait if packet not yet acknowledged or timed out
|
|
|
|
// Stock tx fixed frequency is 0x3C. Receiver only binds on this freq.
|
|
#define RF_CHANNEL 0x3C
|
|
|
|
#define YD717_FLAG_FLIP 0x0F
|
|
#define YD717_FLAG_LIGHT 0x80
|
|
#define YD717_FLAG_PICTURE 0x40
|
|
#define YD717_FLAG_VIDEO 0x20
|
|
#define YD717_FLAG_HEADLESS 0x10
|
|
|
|
#define YD717_PAYLOADSIZE 8 // receive data pipes set to this size, but unused
|
|
//#define YD717_MAX_PACKET_SIZE 9 // YD717 packets have 8-byte payload, Syma X4 is 9
|
|
|
|
enum {
|
|
YD717_INIT1 = 0,
|
|
YD717_BIND2,
|
|
YD717_BIND3,
|
|
YD717_DATA
|
|
};
|
|
|
|
void yd717_send_packet(uint8_t bind)
|
|
{
|
|
uint8_t rudder_trim, elevator_trim, aileron_trim;
|
|
if (bind)
|
|
{
|
|
packet[0]= rx_tx_addr[0]; // send data phase address in first 4 bytes
|
|
packet[1]= rx_tx_addr[1];
|
|
packet[2]= rx_tx_addr[2];
|
|
packet[3]= rx_tx_addr[3];
|
|
packet[4] = 0x56;
|
|
packet[5] = 0xAA;
|
|
packet[6] = (sub_protocol == NIHUI) ? 0x00 : 0x32;
|
|
packet[7] = 0x00;
|
|
}
|
|
else
|
|
{
|
|
// Throttle
|
|
packet[0] = convert_channel_8b(THROTTLE);
|
|
// Rudder
|
|
if( sub_protocol==XINXUN )
|
|
{
|
|
rudder = convert_channel_8b(RUDDER);
|
|
rudder_trim = (0xff - rudder) >> 1;
|
|
}
|
|
else
|
|
{
|
|
rudder = 0xff - convert_channel_8b(RUDDER);
|
|
rudder_trim = rudder >> 1;
|
|
}
|
|
packet[1] = rudder;
|
|
// Elevator
|
|
elevator = convert_channel_8b(ELEVATOR);
|
|
elevator_trim = elevator >> 1;
|
|
packet[3] = elevator;
|
|
// Aileron
|
|
aileron = 0xff - convert_channel_8b(AILERON);
|
|
aileron_trim = aileron >> 1;
|
|
packet[4] = aileron;
|
|
// Trims
|
|
if( sub_protocol == YD717 )
|
|
{
|
|
packet[2] = elevator_trim;
|
|
packet[5] = aileron_trim;
|
|
packet[6] = rudder_trim;
|
|
}
|
|
else
|
|
{
|
|
packet[2] = rudder_trim;
|
|
packet[5] = elevator_trim;
|
|
packet[6] = aileron_trim;
|
|
}
|
|
// Flags
|
|
// Channel 5
|
|
if (Servo_data[AUX1] > PPM_SWITCH)
|
|
flags = YD717_FLAG_FLIP;
|
|
else
|
|
flags=0;
|
|
// Channel 6
|
|
if (Servo_data[AUX2] > PPM_SWITCH)
|
|
flags |= YD717_FLAG_LIGHT;
|
|
// Channel 7
|
|
if (Servo_data[AUX3] > PPM_SWITCH)
|
|
flags |= YD717_FLAG_PICTURE;
|
|
// Channel 8
|
|
if (Servo_data[AUX4] > PPM_SWITCH)
|
|
flags |= YD717_FLAG_VIDEO;
|
|
// Channel 9
|
|
if (Servo_data[AUX5] > PPM_SWITCH)
|
|
flags |= YD717_FLAG_HEADLESS;
|
|
packet[7] = flags;
|
|
}
|
|
|
|
// clear packet status bits and TX FIFO
|
|
NRF24L01_WriteReg(NRF24L01_07_STATUS, (BV(NRF24L01_07_TX_DS) | BV(NRF24L01_07_MAX_RT)));
|
|
NRF24L01_FlushTx();
|
|
|
|
if( sub_protocol == YD717 )
|
|
NRF24L01_WritePayload(packet, 8);
|
|
else
|
|
{
|
|
packet[8] = packet[0]; // checksum
|
|
for(uint8_t i=1; i < 8; i++)
|
|
packet[8] += packet[i];
|
|
packet[8] = ~packet[8];
|
|
NRF24L01_WritePayload(packet, 9);
|
|
}
|
|
|
|
NRF24L01_SetPower(); // Set tx_power
|
|
}
|
|
|
|
void yd717_init()
|
|
{
|
|
NRF24L01_Initialize();
|
|
|
|
// CRC, radio on
|
|
NRF24L01_SetTxRxMode(TX_EN);
|
|
NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_PWR_UP));
|
|
NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x3F); // Auto Acknoledgement on all data pipes
|
|
NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x3F); // Enable all data pipes
|
|
NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address
|
|
NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x1A); // 500uS retransmit t/o, 10 tries
|
|
NRF24L01_WriteReg(NRF24L01_05_RF_CH, RF_CHANNEL); // Channel 3C
|
|
NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps
|
|
NRF24L01_SetPower();
|
|
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
|
|
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, YD717_PAYLOADSIZE); // bytes of data payload for pipe 1
|
|
NRF24L01_WriteReg(NRF24L01_12_RX_PW_P1, YD717_PAYLOADSIZE);
|
|
NRF24L01_WriteReg(NRF24L01_13_RX_PW_P2, YD717_PAYLOADSIZE);
|
|
NRF24L01_WriteReg(NRF24L01_14_RX_PW_P3, YD717_PAYLOADSIZE);
|
|
NRF24L01_WriteReg(NRF24L01_15_RX_PW_P4, YD717_PAYLOADSIZE);
|
|
NRF24L01_WriteReg(NRF24L01_16_RX_PW_P5, YD717_PAYLOADSIZE);
|
|
NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
|
|
|
|
NRF24L01_Activate(0x73); // Activate feature register
|
|
NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x3F); // Enable dynamic payload length on all pipes
|
|
NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x07); // Set feature bits on
|
|
NRF24L01_Activate(0x73);
|
|
|
|
// set tx id
|
|
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 5);
|
|
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
|
|
}
|
|
|
|
void YD717_init1()
|
|
{
|
|
// for bind packets set address to prearranged value known to receiver
|
|
uint8_t bind_rx_tx_addr[] = {0x65, 0x65, 0x65, 0x65, 0x65};
|
|
if( sub_protocol==SYMAX2 )
|
|
for(uint8_t i=0; i < 5; i++)
|
|
bind_rx_tx_addr[i] = 0x60;
|
|
else
|
|
if( sub_protocol==NIHUI )
|
|
for(uint8_t i=0; i < 5; i++)
|
|
bind_rx_tx_addr[i] = 0x64;
|
|
|
|
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, bind_rx_tx_addr, 5);
|
|
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, bind_rx_tx_addr, 5);
|
|
}
|
|
|
|
void YD717_init2()
|
|
{
|
|
// set rx/tx address for data phase
|
|
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 5);
|
|
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
|
|
}
|
|
|
|
uint16_t yd717_callback()
|
|
{
|
|
switch (phase)
|
|
{
|
|
case YD717_INIT1:
|
|
yd717_send_packet(0); // receiver doesn't re-enter bind mode if connection lost...check if already bound
|
|
phase = YD717_BIND3;
|
|
break;
|
|
case YD717_BIND2:
|
|
if (counter == 0)
|
|
{
|
|
if (NRF24L01_packet_ack() == PKT_PENDING)
|
|
return YD717_PACKET_CHKTIME; // packet send not yet complete
|
|
YD717_init2(); // change to data phase rx/tx address
|
|
yd717_send_packet(0);
|
|
phase = YD717_BIND3;
|
|
}
|
|
else
|
|
{
|
|
if (NRF24L01_packet_ack() == PKT_PENDING)
|
|
return YD717_PACKET_CHKTIME; // packet send not yet complete;
|
|
yd717_send_packet(1);
|
|
counter--;
|
|
}
|
|
break;
|
|
case YD717_BIND3:
|
|
switch (NRF24L01_packet_ack())
|
|
{
|
|
case PKT_PENDING:
|
|
return YD717_PACKET_CHKTIME; // packet send not yet complete
|
|
case PKT_ACKED:
|
|
phase = YD717_DATA;
|
|
BIND_DONE; // bind complete
|
|
break;
|
|
case PKT_TIMEOUT:
|
|
YD717_init1(); // change to bind rx/tx address
|
|
counter = YD717_BIND_COUNT;
|
|
phase = YD717_BIND2;
|
|
yd717_send_packet(1);
|
|
}
|
|
break;
|
|
case YD717_DATA:
|
|
if (NRF24L01_packet_ack() == PKT_PENDING)
|
|
return YD717_PACKET_CHKTIME; // packet send not yet complete
|
|
yd717_send_packet(0);
|
|
break;
|
|
}
|
|
return YD717_PACKET_PERIOD; // Packet every 8ms
|
|
}
|
|
|
|
uint16_t initYD717()
|
|
{
|
|
rx_tx_addr[4] = 0xC1; // always uses first data port
|
|
flags = 0;
|
|
yd717_init();
|
|
phase = YD717_INIT1;
|
|
BIND_IN_PROGRESS; // autobind protocol
|
|
|
|
// Call callback in 50ms
|
|
return YD717_INITIAL_WAIT;
|
|
}
|
|
|
|
#endif
|