mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-02-04 20:48:12 +00:00
984aa3f413
- Change how PPM is handled with a resolution of 2048 and scaled to match serial input range. PPM is now fully scaled for all protocols which was not the case before. If you are using PPM, you might have to adjust the end points depending on the protocols. - Change all range conversions to use 2048 where possible - Updated all protocols with new range functions - Protocols which are taking advantage of 2048 are Assan, FrSky V/D/X, DSM, Devo, WK2x01 - Renamed AUX xto CHx for code readbility
502 lines
12 KiB
C++
502 lines
12 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(WK2x01_CYRF6936_INO)
|
|
|
|
#include "iface_cyrf6936.h"
|
|
|
|
#define WK_BIND_COUNT 2980
|
|
#define WK_NUM_WAIT_LOOPS (100 / 5) //each loop is ~5us. Do not wait more than 100us
|
|
|
|
enum {
|
|
WK_BIND=0,
|
|
WK_BOUND_1,
|
|
WK_BOUND_2,
|
|
WK_BOUND_3,
|
|
WK_BOUND_4,
|
|
WK_BOUND_5,
|
|
WK_BOUND_6,
|
|
WK_BOUND_7,
|
|
WK_BOUND_8,
|
|
};
|
|
|
|
static const uint8_t WK_sopcodes[8] = {
|
|
/* Note these are in order transmitted (LSB 1st) */
|
|
0xDF,0xB1,0xC0,0x49,0x62,0xDF,0xC1,0x49 //0x49C1DF6249C0B1DF
|
|
};
|
|
static const uint8_t init_2801[] = {0xc5, 0x34, 0x60, 0x00, 0x25};
|
|
static const uint8_t init_2601[] = {0xb9, 0x45, 0xb0, 0xf1, 0x3a};
|
|
static const uint8_t init_2401[] = {0xa5, 0x23, 0xd0, 0xf0, 0x00};
|
|
|
|
uint8_t WK_last_beacon;
|
|
|
|
static void __attribute__((unused)) WK_add_pkt_crc(uint8_t init)
|
|
{
|
|
uint8_t add = init;
|
|
uint8_t xou = init;
|
|
for (uint8_t i = 0; i < 14; i++)
|
|
{
|
|
add += packet[i];
|
|
xou ^= packet[i];
|
|
}
|
|
packet[14] = xou;
|
|
packet[15] = add;
|
|
}
|
|
|
|
static void __attribute__((unused)) WK_build_bind_pkt(const uint8_t *init)
|
|
{
|
|
packet[0] = init[0];
|
|
packet[1] = init[1];
|
|
packet[2] = hopping_frequency[0];
|
|
packet[3] = hopping_frequency[1];
|
|
packet[4] = init[2];
|
|
packet[5] = hopping_frequency[2];
|
|
packet[6] = 0xff;
|
|
packet[7] = 0x00;
|
|
packet[8] = 0x00;
|
|
packet[9] = 0x32;
|
|
if (sub_protocol == WK2401)
|
|
packet[10] = 0x10 | (rx_tx_addr[0] & 0x0e);
|
|
else
|
|
packet[10] = rx_tx_addr[0];
|
|
packet[11] = rx_tx_addr[1];
|
|
packet[12] = rx_tx_addr[2] | packet_count;
|
|
packet[13] = init[3];
|
|
WK_add_pkt_crc(init[4]);
|
|
}
|
|
|
|
static int16_t __attribute__((unused)) WK_get_channel(uint8_t ch, int32_t scale, int16_t center, int16_t range)
|
|
{
|
|
int16_t value = convert_channel_16b_nolimit(CH_AETR[ch],-scale,scale)+center;
|
|
if (value < center - range) value = center - range;
|
|
if (value > center + range) value = center + range;
|
|
return value;
|
|
}
|
|
|
|
static void __attribute__((unused)) WK_build_data_pkt_2401()
|
|
{
|
|
uint16_t msb = 0;
|
|
uint8_t offset = 0;
|
|
for (uint8_t i = 0; i < 4; i++)
|
|
{
|
|
if (i == 2)
|
|
offset = 1;
|
|
int16_t value = WK_get_channel(i, 0x800, 0, 0xA00); //12 bits, allow value to go to 125%
|
|
uint16_t base = abs(value) >> 2; //10 bits is the base value
|
|
uint16_t trim = abs(value) & 0x03; //lowest 2 bits represent trim
|
|
if (base >= 0x200)
|
|
{ //if value is > 100%, remainder goes to trim
|
|
trim = 4 *(base - 0x200);
|
|
base = 0x1ff;
|
|
}
|
|
base = (value >= 0) ? 0x200 + base : 0x200 - base;
|
|
trim = (value >= 0) ? 0x200 + trim : 0x200 - trim;
|
|
|
|
packet[2*i+offset] = base & 0xff;
|
|
packet[2*i+offset+1] = trim & 0xff;
|
|
msb = (msb << 4) | ((base >> 6) & 0x0c) | ((trim >> 8) & 0x03);
|
|
}
|
|
packet[4] = msb >> 8; //Ele/Ail MSB
|
|
packet[9] = msb & 0xff; //Thr/Rud MSB
|
|
packet[10] = 0xe0 | (rx_tx_addr[0] & 0x0e);
|
|
packet[11] = rx_tx_addr[1];
|
|
packet[12] = rx_tx_addr[2] | packet_count;
|
|
packet[13] = 0xf0; //FIXME - What is this?
|
|
WK_add_pkt_crc(0x00);
|
|
}
|
|
|
|
#define PCT(pct, max) (((int32_t)(max) * (int32_t)(pct) + 1L) / 1000L)
|
|
#define MAXTHR 426 //Measured to provide equal value at +/-0
|
|
static void __attribute__((unused)) WK_channels_6plus1_2601(uint8_t frame, int16_t *_v1, int16_t *_v2)
|
|
{
|
|
int16_t thr = WK_get_channel(2, 1000, 0, 1000);
|
|
int16_t v1;
|
|
uint8_t thr_rev = 0, pitch_rev = 0;
|
|
if(thr > 0)
|
|
{
|
|
if(thr >= 780)
|
|
{ //78%
|
|
v1 = 0; //thr = 60% * (x - 78%) / 22% + 40%
|
|
thr = PCT(1000-MAXTHR,512) * (thr-PCT(780,1000)) / PCT(220,1000) + PCT(MAXTHR,512);
|
|
}
|
|
else
|
|
{
|
|
v1 = 1023 - 1023 * thr / 780;
|
|
thr = PCT(MAXTHR, 512); //40%
|
|
}
|
|
}
|
|
else
|
|
{
|
|
thr = -thr;
|
|
thr_rev = 1;
|
|
if(thr >= 780)
|
|
{ //78%
|
|
v1 = 1023; //thr = 60% * (x - 78%) / 22% + 40%
|
|
thr = PCT(1000-MAXTHR,512) * (thr-PCT(780,1000)) / PCT(220,1000) + PCT(MAXTHR,512);
|
|
}
|
|
else
|
|
{
|
|
v1 = 1023 * thr / 780;
|
|
thr = PCT(MAXTHR, 512); //40%
|
|
}
|
|
}
|
|
if (thr >= 512)
|
|
thr = 511;
|
|
packet[2] = thr & 0xff;
|
|
packet[4] = (packet[4] & 0xF3) | ((thr >> 6) & 0x04);
|
|
|
|
int16_t pitch= WK_get_channel(5, 0x400, 0, 0x400);
|
|
if (pitch < 0)
|
|
{
|
|
pitch_rev = 1;
|
|
pitch = -pitch;
|
|
}
|
|
if (frame == 1)
|
|
{
|
|
//Pitch curve and range
|
|
if (thr > PCT(MAXTHR, 512))
|
|
*_v2 = pitch - pitch * 16 * (thr - PCT(MAXTHR, 512)) / PCT(1000 - MAXTHR, 512) / 100;
|
|
else
|
|
*_v2 = pitch;
|
|
*_v1 = 0;
|
|
}
|
|
else
|
|
if (frame == 2)
|
|
{
|
|
//Throttle curve & Expo
|
|
*_v1 = v1;
|
|
*_v2 = 512;
|
|
}
|
|
packet[7] = (thr_rev << 5) | (pitch_rev << 2); //reverse bits
|
|
packet[8] = 0;
|
|
}
|
|
|
|
static void __attribute__((unused)) WK_channels_5plus1_2601(uint8_t frame, int16_t *v1, int16_t *v2)
|
|
{
|
|
(void)v1;
|
|
//Zero out pitch, provide ail, ele, thr, rud, gyr + gear
|
|
if (frame == 1)
|
|
*v2 = 0; //Pitch curve and range
|
|
packet[7] = 0;
|
|
packet[8] = 0;
|
|
}
|
|
static void __attribute__((unused)) WK_channels_heli_2601(uint8_t frame, int16_t *v1, int16_t *v2)
|
|
{
|
|
//pitch is controlled by rx
|
|
//we can only control fmode, pit-reverse and pit/thr rate
|
|
uint8_t pit_rev = 0;
|
|
if (sub_protocol==W6_HEL_I)
|
|
pit_rev = 1;
|
|
int16_t pit_rate = WK_get_channel(5, 0x400, 0, 0x400);
|
|
uint8_t fmode = 1;
|
|
if (pit_rate < 0)
|
|
{
|
|
pit_rate = -pit_rate;
|
|
fmode = 0;
|
|
}
|
|
if (frame == 1)
|
|
{
|
|
//Pitch curve and range
|
|
*v1 = pit_rate;
|
|
*v2 = (int16_t)(option) * 0x400 / 100 + 0x400;
|
|
}
|
|
packet[7] = (pit_rev << 2); //reverse bits
|
|
packet[8] = fmode ? 0x02 : 0x00;
|
|
}
|
|
|
|
static void __attribute__((unused)) WK_build_data_pkt_2601()
|
|
{
|
|
uint8_t msb = 0;
|
|
uint8_t frame = (packet_count % 3);
|
|
for (uint8_t i = 0; i < 4; i++)
|
|
{
|
|
int16_t value = WK_get_channel(i, 0x190, 0, 0x1FF);
|
|
uint16_t mag = value < 0 ? -value : value;
|
|
packet[i] = mag & 0xff;
|
|
msb = (msb << 2) | ((mag >> 8) & 0x01) | (value < 0 ? 0x02 : 0x00);
|
|
}
|
|
packet[4] = msb;
|
|
int16_t v1 = 0x200, v2 = 0x200;
|
|
if (frame == 0)
|
|
{
|
|
//Gyro & Rudder mix
|
|
v1 = WK_get_channel(6, 0x200, 0x200, 0x200);
|
|
v2 = 0;
|
|
}
|
|
if (sub_protocol == W6_5_1)
|
|
WK_channels_5plus1_2601(frame, &v1, &v2);
|
|
else if (sub_protocol == W6_6_1)
|
|
WK_channels_6plus1_2601(frame, &v1, &v2);
|
|
else
|
|
WK_channels_heli_2601(frame, &v1, &v2);
|
|
if (v1 > 1023)
|
|
v1 = 1023;
|
|
if (v2 > 1023)
|
|
v2 = 1023;
|
|
packet[5] = v2 & 0xff;
|
|
packet[6] = v1 & 0xff;
|
|
//packet[7] handled by channel code
|
|
packet[8] |= (WK_get_channel(4, 0x190, 0, 0x1FF) > 0 ? 1 : 0);
|
|
packet[9] = ((v1 >> 4) & 0x30) | ((v2 >> 2) & 0xc0) | 0x04 | frame;
|
|
packet[10] = rx_tx_addr[0];
|
|
packet[11] = rx_tx_addr[1];
|
|
packet[12] = rx_tx_addr[2] | packet_count;
|
|
packet[13] = 0xff;
|
|
|
|
WK_add_pkt_crc(0x3A);
|
|
}
|
|
|
|
static void __attribute__((unused)) WK_build_data_pkt_2801()
|
|
{
|
|
uint16_t msb = 0;
|
|
uint8_t offset = 0;
|
|
uint8_t sign = 0;
|
|
for (uint8_t i = 0; i < 8; i++)
|
|
{
|
|
if (i == 4) { offset = 1; }
|
|
int16_t value = WK_get_channel(i, 0x1C2, 0, 0x3FF);
|
|
uint16_t mag = value < 0 ? -value : value;
|
|
packet[i+offset] = mag & 0xff;
|
|
msb = (msb << 2) | ((mag >> 8) & 0x03);
|
|
if (value < 0) { sign |= 1 << i; }
|
|
}
|
|
packet[4] = msb >> 8;
|
|
packet[9] = msb & 0xff;
|
|
packet[10] = rx_tx_addr[0];
|
|
packet[11] = rx_tx_addr[1];
|
|
packet[12] = rx_tx_addr[2] | packet_count;
|
|
packet[13] = sign;
|
|
WK_add_pkt_crc(0x25);
|
|
}
|
|
|
|
static void __attribute__((unused)) WK_build_beacon_pkt_2801()
|
|
{
|
|
WK_last_beacon ^= 1;
|
|
uint8_t en = 0;
|
|
uint8_t bind_state;
|
|
|
|
#ifdef ENABLE_PPM
|
|
if(mode_select && option==0 && IS_BIND_DONE) //PPM mode and option not already set and bind is finished
|
|
{
|
|
BIND_SET_INPUT;
|
|
BIND_SET_PULLUP; // set pullup
|
|
if(IS_BIND_BUTTON_on)
|
|
{
|
|
eeprom_write_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select),0x01); // Set fixed id mode for the current model
|
|
option=1;
|
|
}
|
|
BIND_SET_OUTPUT;
|
|
}
|
|
#endif //ENABLE_PPM
|
|
if(prev_option!=option && IS_BIND_DONE)
|
|
{
|
|
set_rx_tx_addr(MProtocol_id);
|
|
rx_tx_addr[2]=rx_tx_addr[3]<<4; // Make use of RX_Num
|
|
bind_counter = WK_BIND_COUNT / 8 + 1;
|
|
}
|
|
if (option)
|
|
{
|
|
if (bind_counter)
|
|
bind_state = 0xe4;
|
|
else
|
|
bind_state = 0x1b;
|
|
}
|
|
else
|
|
bind_state = 0x99;
|
|
|
|
for (uint8_t i = 0; i < 4; i++)
|
|
{
|
|
#ifdef FAILSAFE_ENABLE
|
|
uint16_t failsafe=Failsafe_data[CH_AETR[i + WK_last_beacon * 4]];
|
|
if(failsafe!=FAILSAFE_CHANNEL_HOLD && IS_FAILSAFE_VALUES_on)
|
|
{
|
|
packet[i+1] = failsafe>>3; //0..255
|
|
en |= 1 << i;
|
|
}
|
|
else
|
|
#endif
|
|
packet[i+1] = 0;
|
|
}
|
|
packet[0] = en;
|
|
packet[5] = packet[4];
|
|
packet[4] = WK_last_beacon << 6;
|
|
packet[6] = hopping_frequency[0];
|
|
packet[7] = hopping_frequency[1];
|
|
packet[8] = hopping_frequency[2];
|
|
packet[9] = bind_state;
|
|
packet[10] = rx_tx_addr[0];
|
|
packet[11] = rx_tx_addr[1];
|
|
packet[12] = rx_tx_addr[2] | packet_count;
|
|
packet[13] = 0x00; //Does this matter? in the docs it is the same as the data packet
|
|
WK_add_pkt_crc(0x1C);
|
|
}
|
|
|
|
static void __attribute__((unused)) wk2x01_cyrf_init() {
|
|
/* Initialize CYRF chip */
|
|
CYRF_SetPower(0x28);
|
|
CYRF_WriteRegister(CYRF_06_RX_CFG, 0x4A);
|
|
CYRF_WriteRegister(CYRF_0B_PWR_CTRL, 0x00);
|
|
CYRF_WriteRegister(CYRF_0C_XTAL_CTRL, 0xC0);
|
|
CYRF_WriteRegister(CYRF_0D_IO_CFG, 0x04);
|
|
CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x2C);
|
|
CYRF_WriteRegister(CYRF_10_FRAMING_CFG, 0xEE);
|
|
CYRF_WriteRegister(CYRF_1B_TX_OFFSET_LSB, 0x55);
|
|
CYRF_WriteRegister(CYRF_1C_TX_OFFSET_MSB, 0x05);
|
|
CYRF_WriteRegister(CYRF_1D_MODE_OVERRIDE, 0x18);
|
|
CYRF_WriteRegister(CYRF_32_AUTO_CAL_TIME, 0x3C);
|
|
CYRF_WriteRegister(CYRF_35_AUTOCAL_OFFSET, 0x14);
|
|
CYRF_WriteRegister(CYRF_1E_RX_OVERRIDE, 0x90);
|
|
CYRF_WriteRegister(CYRF_1F_TX_OVERRIDE, 0x00);
|
|
CYRF_WriteRegister(CYRF_01_TX_LENGTH, 0x10);
|
|
CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x2C);
|
|
CYRF_WriteRegister(CYRF_28_CLK_EN, 0x02);
|
|
CYRF_WriteRegister(CYRF_27_CLK_OVERRIDE, 0x02);
|
|
CYRF_ConfigSOPCode(WK_sopcodes);
|
|
CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x28);
|
|
CYRF_WriteRegister(CYRF_1E_RX_OVERRIDE, 0x10);
|
|
CYRF_WriteRegister(CYRF_0E_GPIO_CTRL, 0x20);
|
|
CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x2C);
|
|
}
|
|
|
|
static void __attribute__((unused)) WK_BuildPacket_2801()
|
|
{
|
|
switch(phase) {
|
|
case WK_BIND:
|
|
bind_counter--;
|
|
WK_build_bind_pkt(init_2801);
|
|
if (bind_counter == 0)
|
|
{
|
|
BIND_DONE;
|
|
phase++;
|
|
}
|
|
break;
|
|
case WK_BOUND_1:
|
|
case WK_BOUND_2:
|
|
case WK_BOUND_3:
|
|
case WK_BOUND_4:
|
|
case WK_BOUND_5:
|
|
case WK_BOUND_6:
|
|
case WK_BOUND_7:
|
|
WK_build_data_pkt_2801();
|
|
phase++;
|
|
break;
|
|
case WK_BOUND_8:
|
|
WK_build_beacon_pkt_2801();
|
|
phase = WK_BOUND_1;
|
|
if (bind_counter)
|
|
{
|
|
bind_counter--;
|
|
if (bind_counter == 0)
|
|
BIND_DONE;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void __attribute__((unused)) WK_BuildPacket_2601()
|
|
{
|
|
if (bind_counter)
|
|
{
|
|
bind_counter--;
|
|
WK_build_bind_pkt(init_2601);
|
|
if (bind_counter == 0)
|
|
BIND_DONE;
|
|
}
|
|
else
|
|
WK_build_data_pkt_2601();
|
|
}
|
|
|
|
static void __attribute__((unused)) WK_BuildPacket_2401()
|
|
{
|
|
if (bind_counter)
|
|
{
|
|
bind_counter--;
|
|
WK_build_bind_pkt(init_2401);
|
|
if(bind_counter == 0)
|
|
BIND_DONE;
|
|
}
|
|
else
|
|
WK_build_data_pkt_2401();
|
|
}
|
|
|
|
uint16_t WK_cb()
|
|
{
|
|
if (packet_sent == 0)
|
|
{
|
|
packet_sent = 1;
|
|
if(sub_protocol == WK2801)
|
|
WK_BuildPacket_2801();
|
|
else if(sub_protocol == WK2401)
|
|
WK_BuildPacket_2401();
|
|
else
|
|
WK_BuildPacket_2601();
|
|
packet_count = (packet_count + 1) % 12;
|
|
CYRF_WriteDataPacket(packet);
|
|
return 1600;
|
|
}
|
|
packet_sent = 0;
|
|
uint8_t start=micros();
|
|
while ((uint8_t)micros()-start < 100) // Wait max 100µs
|
|
if(CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02)
|
|
break;
|
|
if((packet_count & 0x03) == 0)
|
|
{
|
|
hopping_frequency_no++;
|
|
hopping_frequency_no%=3;
|
|
CYRF_ConfigRFChannel(hopping_frequency[hopping_frequency_no]);
|
|
//Keep transmit power updated
|
|
CYRF_SetPower(0x28);
|
|
}
|
|
return 1200;
|
|
}
|
|
|
|
uint16_t WK_setup()
|
|
{
|
|
wk2x01_cyrf_init();
|
|
CYRF_SetTxRxMode(TX_EN);
|
|
|
|
hopping_frequency_no=0;
|
|
CYRF_FindBestChannels(hopping_frequency, 3, 4, 4, 80);
|
|
CYRF_ConfigRFChannel(hopping_frequency[0]);
|
|
|
|
packet_count = 0;
|
|
packet_sent = 0;
|
|
WK_last_beacon = 0;
|
|
prev_option=option;
|
|
if(sub_protocol!=WK2801 || option==0)
|
|
{
|
|
CYRF_GetMfgData(cyrfmfg_id);
|
|
rx_tx_addr[2]=(hopping_frequency[0] ^ cyrfmfg_id[0] ^ cyrfmfg_id[3])<<4;
|
|
rx_tx_addr[1]=hopping_frequency[1] ^ cyrfmfg_id[1] ^ cyrfmfg_id[4];
|
|
rx_tx_addr[0]=hopping_frequency[2] ^ cyrfmfg_id[2] ^ cyrfmfg_id[5];
|
|
if(sub_protocol == WK2401)
|
|
rx_tx_addr[0] |= 0x01; //ID must be odd for 2401
|
|
|
|
bind_counter = WK_BIND_COUNT;
|
|
phase = WK_BIND;
|
|
BIND_IN_PROGRESS;
|
|
}
|
|
else
|
|
{
|
|
rx_tx_addr[2]=rx_tx_addr[3]<<4; // Make use of RX_Num
|
|
bind_counter = 0;
|
|
phase = WK_BOUND_1;
|
|
BIND_DONE;
|
|
}
|
|
return 2800;
|
|
}
|
|
|
|
#endif
|