mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-02-04 18:48:11 +00:00
359 lines
9.1 KiB
C++
359 lines
9.1 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/>.
|
|
|
|
Thanks to Goebish ,Ported from his deviation firmware
|
|
*/
|
|
|
|
#if defined(V761_NRF24L01_INO)
|
|
|
|
#include "iface_xn297.h"
|
|
|
|
//#define V761_FORCE_ID
|
|
|
|
#define V761_PACKET_PERIOD 7060 // Timeout for callback in uSec
|
|
#define V761_INITIAL_WAIT 500
|
|
#define V761_PACKET_SIZE 8
|
|
#define V761_RXPAYLOAD_SIZE 3
|
|
#define V761_BIND_COUNT 200
|
|
#define V761_BIND_FREQ 0x28
|
|
#define V761_RF_NUM_CHANNELS 3
|
|
#define TOPRC_BIND_FREQ 0x2A
|
|
#define TOPRC_PACKET_PERIOD 14120 // Timeout for callback in uSec
|
|
|
|
#define V761_WRITE_TIME 450
|
|
#define V761_TELEM_PACKET_PERIOD 14088 // Timeout for callback in uSec
|
|
//#define V761_TELEM_DEBUG
|
|
|
|
enum
|
|
{
|
|
V761_BIND1 = 0,
|
|
V761_BIND2,
|
|
V761_RX_CHECK,
|
|
V761_DATA,
|
|
V761_RX
|
|
};
|
|
|
|
static void __attribute__((unused)) V761_set_checksum()
|
|
{
|
|
uint8_t checksum = packet[0];
|
|
for(uint8_t i=1; i<V761_PACKET_SIZE-2; i++)
|
|
checksum += packet[i];
|
|
if(phase == V761_BIND1)
|
|
{
|
|
packet[6] = checksum ^ 0xff;
|
|
packet[7] = packet[6];
|
|
}
|
|
else
|
|
{
|
|
checksum += packet[6];
|
|
packet[7] = checksum ^ 0xff;
|
|
}
|
|
}
|
|
|
|
static void __attribute__((unused)) V761_send_packet()
|
|
{
|
|
static bool calib=false, prev_ch6=false;
|
|
|
|
if(phase != V761_DATA)
|
|
{
|
|
memcpy(packet, rx_tx_addr, 4);
|
|
packet[4] = hopping_frequency[1];
|
|
packet[5] = hopping_frequency[2];
|
|
if(phase == V761_BIND2)
|
|
packet[6] = 0xF0; // ?
|
|
}
|
|
else
|
|
{
|
|
#ifdef V761_TELEM_DEBUG
|
|
debug_time("");
|
|
debugln(" Ph:%d",hopping_frequency_no);
|
|
#endif
|
|
XN297_Hopping(hopping_frequency_no++);
|
|
if(hopping_frequency_no >= V761_RF_NUM_CHANNELS)
|
|
{
|
|
hopping_frequency_no = 0;
|
|
packet_sent++;
|
|
packet_sent &= 0x03;
|
|
}
|
|
|
|
packet[0] = convert_channel_8b(THROTTLE); // Throttle
|
|
packet[2] = convert_channel_8b(ELEVATOR)>>1; // Elevator
|
|
|
|
if(sub_protocol == V761_4CH || sub_protocol == V761_TOPRC)
|
|
{
|
|
packet[1] = convert_channel_8b(AILERON)>>1; // Aileron
|
|
packet[3] = convert_channel_8b(RUDDER)>>1; // Rudder
|
|
}
|
|
else
|
|
{
|
|
packet[1] = convert_channel_8b(RUDDER)>>1; // Rudder
|
|
packet[3] = convert_channel_8b(AILERON)>>1; // Aileron
|
|
}
|
|
|
|
packet[5] = packet_sent<<6; // 0X, 4X, 8X, CX
|
|
packet[4] = 0x20; // Trims 00..20..40, 0X->20 4X->TrAil 8X->TrEle CX->TrRud
|
|
|
|
if(CH5_SW) // Mode Expert Gyro off
|
|
flags = 0x0c;
|
|
else
|
|
if(Channel_data[CH5] < CHANNEL_MIN_COMMAND)
|
|
flags = 0x08; // Beginer mode (Gyro on, yaw and pitch rate limited)
|
|
else
|
|
flags = 0x0a; // Mid Mode ( Gyro on no rate limits)
|
|
|
|
if(!prev_ch6 && CH6_SW) // -100% -> 100% launch gyro calib
|
|
calib=!calib;
|
|
prev_ch6 = CH6_SW;
|
|
if(calib)
|
|
flags |= 0x01; // Gyro calibration
|
|
|
|
packet[5] |= flags;
|
|
|
|
packet[6] = GET_FLAG(CH7_SW, 0x20) // Flip
|
|
|GET_FLAG(CH8_SW, 0x08) // RTH activation
|
|
|GET_FLAG(CH9_SW, 0x10); // RTH on/off
|
|
if(sub_protocol == V761_3CH)
|
|
packet[6] |= 0x80; // Unknown, set on original V761-1 dump but not on eachine dumps, keeping for compatibility
|
|
}
|
|
V761_set_checksum();
|
|
|
|
#if 0
|
|
debug("H:%02X P:",hopping_frequency_no);
|
|
for(uint8_t i=0;i<V761_PACKET_SIZE;i++)
|
|
debug(" %02X",packet[i]);
|
|
debugln("");
|
|
#endif
|
|
// Send
|
|
XN297_SetPower();
|
|
XN297_SetTxRxMode(TX_EN);
|
|
XN297_WritePayload(packet, V761_PACKET_SIZE);
|
|
}
|
|
|
|
static void __attribute__((unused)) V761_RF_init()
|
|
{
|
|
XN297_Configure(XN297_CRCEN, XN297_SCRAMBLED, XN297_1M);
|
|
}
|
|
|
|
static void __attribute__((unused)) V761_initialize_txid()
|
|
{
|
|
#ifdef V761_FORCE_ID
|
|
if(sub_protocol == V761_TOPRC)
|
|
{ //Dump from air on TopRCHobby TX
|
|
memcpy(rx_tx_addr,(uint8_t *)"\xD5\x01\x00\x00",4);
|
|
memcpy(hopping_frequency,(uint8_t *)"\x2E\x41",2);
|
|
}
|
|
else
|
|
{
|
|
switch(RX_num%5)
|
|
{
|
|
case 1: //Dump from air on Protonus TX
|
|
memcpy(rx_tx_addr,(uint8_t *)"\xE8\xE4\x45\x09",4);
|
|
memcpy(hopping_frequency,(uint8_t *)"\x0D\x21",2);
|
|
break;
|
|
case 2: //Dump from air on mshagg2 TX
|
|
memcpy(rx_tx_addr,(uint8_t *)"\xAE\xD1\x45\x09",4);
|
|
memcpy(hopping_frequency,(uint8_t *)"\x13\x1D",2);
|
|
break;
|
|
case 3: //Dump from air on MikeHRC Eachine TX
|
|
memcpy(rx_tx_addr,(uint8_t *)"\x08\x03\x00\xA0",4);
|
|
memcpy(hopping_frequency,(uint8_t *)"\x0D\x21",2);
|
|
break;
|
|
case 4: //Dump from air on Crashanium Eachine TX
|
|
memcpy(rx_tx_addr,(uint8_t *)"\x58\x08\x00\xA0",4);
|
|
memcpy(hopping_frequency,(uint8_t *)"\x0D\x31",2);
|
|
break;
|
|
default: //Dump from SPI
|
|
memcpy(rx_tx_addr,(uint8_t *)"\x6f\x2c\xb1\x93",4);
|
|
memcpy(hopping_frequency,(uint8_t *)"\x14\x1e",2);
|
|
break;
|
|
}
|
|
}
|
|
#else
|
|
//Tested with Eachine RX
|
|
rx_tx_addr[0]+=RX_num;
|
|
hopping_frequency[0]=(rx_tx_addr[0]&0x0F)+0x05;
|
|
hopping_frequency[1]=hopping_frequency[0]+0x05+(RX_num%0x2D);
|
|
#endif
|
|
|
|
hopping_frequency[2]=hopping_frequency[0]+0x37;
|
|
|
|
debugln("ID: %02X %02X %02X %02X , HOP: %02X %02X %02X",rx_tx_addr[0],rx_tx_addr[1],rx_tx_addr[2],rx_tx_addr[3],hopping_frequency[0],hopping_frequency[1],hopping_frequency[2]);
|
|
}
|
|
|
|
uint16_t V761_callback()
|
|
{
|
|
static bool rx = false;
|
|
static uint8_t packet_telem = 0;
|
|
|
|
switch(phase)
|
|
{
|
|
case V761_BIND1:
|
|
if(bind_counter)
|
|
bind_counter--;
|
|
packet_sent ++;
|
|
XN297_RFChannel(sub_protocol == V761_TOPRC ? TOPRC_BIND_FREQ : V761_BIND_FREQ);
|
|
XN297_SetTXAddr(rx_id, 4);
|
|
V761_send_packet();
|
|
if(packet_sent >= 20)
|
|
{
|
|
packet_sent = 0;
|
|
phase = V761_BIND2;
|
|
}
|
|
return 15730;
|
|
case V761_BIND2:
|
|
if(bind_counter)
|
|
bind_counter--;
|
|
packet_sent ++;
|
|
XN297_Hopping(0);
|
|
XN297_SetTXAddr(rx_tx_addr, 4);
|
|
V761_send_packet();
|
|
if(bind_counter == 0)
|
|
{
|
|
packet_sent = 0;
|
|
BIND_DONE;
|
|
phase = V761_DATA;
|
|
#ifdef V761_HUB_TELEMETRY
|
|
XN297_SetRXAddr(rx_tx_addr, V761_RXPAYLOAD_SIZE);
|
|
#endif
|
|
}
|
|
else if(packet_sent >= 20)
|
|
{
|
|
packet_sent = 0;
|
|
phase = V761_BIND1;
|
|
}
|
|
return 15730;
|
|
#ifdef V761_HUB_TELEMETRY
|
|
case V761_RX_CHECK:
|
|
rx = XN297_IsRX(); // Needed for the NRF24L01 since otherwise the bit gets cleared
|
|
XN297_SetTxRxMode(TXRX_OFF);
|
|
if(packet_count > 4*63) // Around 3.5sec with no telemetry
|
|
{
|
|
telemetry_lost = 1;
|
|
packet_period = V761_PACKET_PERIOD;
|
|
}
|
|
else
|
|
{
|
|
packet_count++;
|
|
if(!telemetry_lost && !rx && (packet_count%64) == 0)
|
|
{// Should have received a telem packet but... Send telem to the radio to keep it alive
|
|
telemetry_link = 1;
|
|
#ifdef V761_TELEM_DEBUG
|
|
debugln("Miss");
|
|
#endif
|
|
}
|
|
}
|
|
phase++;
|
|
#endif
|
|
case V761_DATA:
|
|
#ifdef MULTI_SYNC
|
|
telemetry_set_input_sync(packet_period);
|
|
#endif
|
|
V761_send_packet();
|
|
#ifdef V761_HUB_TELEMETRY
|
|
if(sub_protocol == V761_TOPRC)
|
|
break;
|
|
if(rx)
|
|
{ // Check if a packet has been received
|
|
#ifdef V761_TELEM_DEBUG
|
|
debug("RX ");
|
|
#endif
|
|
if(XN297_ReadPayload(packet_in, V761_RXPAYLOAD_SIZE))
|
|
{ // packet with good CRC and length
|
|
#ifdef V761_TELEM_DEBUG
|
|
debug("OK:");
|
|
for(uint8_t i=0;i<V761_RXPAYLOAD_SIZE;i++)
|
|
debug(" %02X",packet_in[i]);
|
|
debug(" pps:%d", packet_count);
|
|
#endif
|
|
// packet_in[] = AA 00 55 -> battery ok
|
|
// packet_in[] = 55 00 AA -> low battery
|
|
crc8 = 0;
|
|
for(uint8_t i=0;i<V761_RXPAYLOAD_SIZE;i++)
|
|
crc8 ^= packet_in[i];
|
|
if(crc8 == 0xFF)
|
|
{
|
|
v_lipo1 = packet_in[0] >> 1;
|
|
telemetry_link = 1;
|
|
telemetry_lost = 0;
|
|
packet_count = 0;
|
|
packet_period = V761_TELEM_PACKET_PERIOD;
|
|
}
|
|
#ifdef V761_TELEM_DEBUG
|
|
else // Bad packet
|
|
debug(" NOK");
|
|
#endif
|
|
}
|
|
#ifdef V761_TELEM_DEBUG
|
|
else // Bad packet
|
|
debug("NOK");
|
|
debugln("");
|
|
#endif
|
|
rx = false;
|
|
}
|
|
phase++;
|
|
return V761_WRITE_TIME;
|
|
case V761_RX:
|
|
{ // Wait for packet to be sent before switching to receive mode
|
|
uint16_t start=(uint16_t)micros();
|
|
while ((uint16_t)((uint16_t)micros()-(uint16_t)start) < 500)
|
|
if(XN297_IsPacketSent())
|
|
break;
|
|
}
|
|
XN297_SetTxRxMode(RX_EN);
|
|
phase = V761_RX_CHECK;
|
|
return packet_period - V761_WRITE_TIME;
|
|
#else
|
|
break;
|
|
#endif
|
|
}
|
|
return packet_period;
|
|
}
|
|
|
|
void V761_init(void)
|
|
{
|
|
V761_initialize_txid();
|
|
V761_RF_init();
|
|
if(sub_protocol == V761_TOPRC)
|
|
{
|
|
memcpy(rx_id,(uint8_t*)"\x20\x21\x05\x0A",4);
|
|
packet_period = TOPRC_PACKET_PERIOD;
|
|
}
|
|
else
|
|
{
|
|
memcpy(rx_id,(uint8_t*)"\x34\x43\x10\x10",4);
|
|
packet_period = V761_PACKET_PERIOD;
|
|
}
|
|
|
|
if(IS_BIND_IN_PROGRESS)
|
|
{
|
|
bind_counter = V761_BIND_COUNT;
|
|
phase = V761_BIND1;
|
|
}
|
|
else
|
|
{
|
|
XN297_SetTXAddr(rx_tx_addr, 4);
|
|
#ifdef V761_HUB_TELEMETRY
|
|
XN297_SetRXAddr(rx_tx_addr, V761_RXPAYLOAD_SIZE);
|
|
#endif
|
|
phase = V761_DATA;
|
|
}
|
|
|
|
hopping_frequency_no = 0;
|
|
packet_sent = 0;
|
|
#ifdef V761_HUB_TELEMETRY
|
|
packet_count = 0;
|
|
telemetry_lost = 1;
|
|
RX_RSSI = 100; // Dummy value
|
|
#endif
|
|
}
|
|
|
|
#endif |