You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
306 lines
8.4 KiB
306 lines
8.4 KiB
/*
|
|
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/>.
|
|
*/
|
|
|
|
//#include "iface_cc2500.h"
|
|
#include "frskyd.h"
|
|
#include "config.h"
|
|
#include "Arduino.h"
|
|
|
|
|
|
Frsky_d::Frsky_d(CC2500 *cc2500)
|
|
{
|
|
this->calibration = 0x89;
|
|
this->state = FRSKY_BIND;
|
|
this->cc2500 = cc2500;
|
|
this->packet_count = 0;
|
|
|
|
// TODO real random randomSeed(analogRead(0));
|
|
randomSeed(42);
|
|
|
|
this->MProtocol_id_master = random(10);
|
|
this->MProtocol_id = this->MProtocol_id_master;
|
|
|
|
this->init_channel();
|
|
this->init_failsafe();
|
|
|
|
Serial.print(" multiprotocl id : ");
|
|
Serial.print(this->MProtocol_id);
|
|
Serial.print("\n");
|
|
}
|
|
void Frsky_d::init(bool bind)
|
|
{
|
|
this->init_cc2500(FRSKYD_cc2500_conf);
|
|
|
|
cc2500->write_register(CC2500_09_ADDR, bind ? 0x03 : rx_tx_addr[3]);
|
|
cc2500->write_register(CC2500_07_PKTCTRL1, 0x05);
|
|
cc2500->strobe(CC2500_SIDLE); // Go to idle...
|
|
//
|
|
cc2500->write_register(CC2500_0A_CHANNR, 0x00);
|
|
cc2500->write_register(CC2500_23_FSCAL3, 0x89);
|
|
cc2500->strobe(CC2500_SFRX);
|
|
//#######END INIT########
|
|
}
|
|
void Frsky_d::init_cc2500(const uint8_t *ptr)
|
|
{
|
|
for (uint8_t i = 0; i < 19; i++) {
|
|
uint8_t reg = FRSKY_common_startreg_cc2500_conf[i];
|
|
uint8_t val = ptr[i];
|
|
|
|
if (reg == CC2500_0C_FSCTRL0)
|
|
val = option;
|
|
|
|
cc2500->write_register(reg, val);
|
|
}
|
|
|
|
prev_option = option ; // Save option to monitor FSCTRL0 change
|
|
|
|
for (uint8_t i = 0; i < 17; i++) {
|
|
uint8_t reg = FRSKY_common_end_cc2500_conf[i][0];
|
|
uint8_t val = FRSKY_common_end_cc2500_conf[i][1];
|
|
cc2500->write_register(reg, val);
|
|
}
|
|
|
|
cc2500->set_wireless_mode(CC2500::wireless_tx);
|
|
cc2500->set_power();
|
|
cc2500->strobe(CC2500_SIDLE); // Go to idle...
|
|
}
|
|
|
|
void Frsky_d::build_bind_packet(void)
|
|
{
|
|
//11 03 01 d7 2d 00 00 1e 3c 5b 78 00 00 00 00 00 00 01
|
|
//11 03 01 19 3e 00 02 8e 2f bb 5c 00 00 00 00 00 00 01
|
|
packet[0] = 0x11;
|
|
packet[1] = 0x03;
|
|
packet[2] = 0x01;
|
|
packet[3] = rx_tx_addr[3];
|
|
packet[4] = rx_tx_addr[2];
|
|
uint16_t idx = ((state - FRSKY_BIND) % 10) * 5;
|
|
packet[5] = idx;
|
|
packet[6] = hopping_frequency[idx++];
|
|
packet[7] = hopping_frequency[idx++];
|
|
packet[8] = hopping_frequency[idx++];
|
|
packet[9] = hopping_frequency[idx++];
|
|
packet[10] = hopping_frequency[idx++];
|
|
packet[11] = 0x00;
|
|
packet[12] = 0x00;
|
|
packet[13] = 0x00;
|
|
packet[14] = 0x00;
|
|
packet[15] = 0x00;
|
|
packet[16] = 0x00;
|
|
packet[17] = 0x01;
|
|
}
|
|
|
|
void Frsky_d::frsky2way_data_frame()
|
|
{
|
|
//pachet[4] is telemetry user frame counter(hub)
|
|
//11 d7 2d 22 00 01 c9 c9 ca ca 88 88 ca ca c9 ca 88 88
|
|
//11 57 12 00 00 01 f2 f2 f2 f2 06 06 ca ca ca ca 18 18
|
|
packet[0] = 0x11; //Length
|
|
packet[1] = rx_tx_addr[3];
|
|
packet[2] = rx_tx_addr[2];
|
|
packet[3] = counter;//
|
|
#if defined TELEMETRY
|
|
packet[4] = telemetry_counter;
|
|
#else
|
|
packet[4] = 0x00;
|
|
#endif
|
|
|
|
packet[5] = 0x01;
|
|
//
|
|
packet[10] = 0;
|
|
packet[11] = 0;
|
|
packet[16] = 0;
|
|
packet[17] = 0;
|
|
|
|
for (uint8_t i = 0; i < 8; i++) {
|
|
uint16_t value;
|
|
value = convert_channel_frsky(i);
|
|
|
|
if (i < 4) {
|
|
packet[6 + i] = value & 0xff;
|
|
packet[10 + (i >> 1)] |= ((value >> 8) & 0x0f) << (4 * (i & 0x01));
|
|
} else {
|
|
packet[8 + i] = value & 0xff;
|
|
packet[16 + ((i - 4) >> 1)] |= ((value >> 8) & 0x0f) << (4 * ((i - 4) & 0x01));
|
|
}
|
|
}
|
|
}
|
|
|
|
uint16_t Frsky_d::init_frsky_2way(void)
|
|
{
|
|
Serial.print("init hop");
|
|
init_hop();
|
|
packet_count = 0;
|
|
#if defined TELEMETRY
|
|
init_frskyd_link_telemetry();
|
|
#endif
|
|
|
|
if (IS_BIND_IN_PROGRESS) {
|
|
init(true);
|
|
state = FRSKY_BIND;
|
|
} else {
|
|
state = FRSKY_BIND_DONE;
|
|
}
|
|
|
|
return 10000;
|
|
}
|
|
|
|
uint16_t Frsky_d::read_frsky_2way(void)
|
|
{
|
|
if (state < FRSKY_BIND_DONE) {
|
|
Serial.print("bind mode\n");
|
|
build_bind_packet();
|
|
cc2500->strobe(CC2500_SIDLE);
|
|
cc2500->write_register(CC2500_0A_CHANNR, 0x00);
|
|
cc2500->write_register(CC2500_23_FSCAL3, 0x89);
|
|
cc2500->strobe(CC2500_SFRX);//0x3A
|
|
cc2500->write_data(packet, packet[0] + 1);
|
|
|
|
if (IS_BIND_DONE) {
|
|
Serial.print("bind finished \n");
|
|
state = FRSKY_BIND_DONE;
|
|
} else {
|
|
//state += 1;
|
|
Serial.print("bind not done\n");
|
|
Serial.print("state :");
|
|
Serial.print(state);
|
|
Serial.print("\n");
|
|
}
|
|
|
|
return 9000;
|
|
}
|
|
|
|
if (state == FRSKY_BIND_DONE) {
|
|
Serial.print("bind done \n");
|
|
state = FRSKY_DATA2;
|
|
init(false);
|
|
counter = 0;
|
|
BIND_DONE;
|
|
} else if (state == FRSKY_DATA5) {
|
|
Serial.print("data 5 \n");
|
|
cc2500->strobe(CC2500_SRX);//0x34 RX enable
|
|
state = FRSKY_DATA1;
|
|
return 9200;
|
|
}
|
|
|
|
counter = (counter + 1) % 188;
|
|
|
|
if (state == FRSKY_DATA4) {
|
|
Serial.print("data 4 \n");
|
|
//telemetry receive
|
|
cc2500->set_wireless_mode(CC2500::wireless_rx);
|
|
cc2500->strobe(CC2500_SIDLE);
|
|
cc2500->write_register(CC2500_0A_CHANNR, hopping_frequency[counter % 47]);
|
|
cc2500->write_register(CC2500_23_FSCAL3, 0x89);
|
|
state++;
|
|
return 1300;
|
|
} else {
|
|
if (state == FRSKY_DATA1) {
|
|
Serial.print("data 1 \n");
|
|
len = cc2500->read_register(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
|
|
|
if (len && len <= (0x11 + 3)) { // 20bytes
|
|
cc2500->read_data(pkt, len); //received telemetry packets
|
|
#if defined(TELEMETRY)
|
|
|
|
if (pkt[len - 1] & 0x80) {
|
|
//with valid crc
|
|
packet_count = 0;
|
|
frsky_check_telemetry(pkt, len); //check if valid telemetry packets and buffer them.
|
|
}
|
|
|
|
#endif
|
|
} else {
|
|
packet_count++;
|
|
|
|
// restart sequence on missed packet - might need count or timeout instead of one missed
|
|
if (packet_count > 100) {
|
|
//~1sec
|
|
packet_count = 0;
|
|
#if defined TELEMETRY
|
|
telemetry_link = 0; //no link frames
|
|
pkt[6] = 0; //no user frames.
|
|
#endif
|
|
}
|
|
}
|
|
|
|
cc2500->set_wireless_mode(CC2500::wireless_tx);
|
|
cc2500->set_power(); // Set tx_power
|
|
}
|
|
|
|
cc2500->strobe(CC2500_SIDLE);
|
|
cc2500->write_register(CC2500_0A_CHANNR, hopping_frequency[counter % 47]);
|
|
|
|
if (prev_option != option) {
|
|
cc2500->write_register(CC2500_0C_FSCTRL0, option); // Frequency offset hack
|
|
prev_option = option ;
|
|
}
|
|
|
|
cc2500->write_register(CC2500_23_FSCAL3, 0x89);
|
|
cc2500->strobe(CC2500_SFRX);
|
|
frsky2way_data_frame();
|
|
cc2500->write_data(packet, packet[0] + 1);
|
|
state++;
|
|
}
|
|
|
|
return state == FRSKY_DATA4 ? 7500 : 9000;
|
|
}
|
|
|
|
uint16_t Frsky_d::convert_channel_frsky(uint8_t num)
|
|
{
|
|
// Channel value for FrSky (PPM is multiplied by 1.5)
|
|
uint16_t val = Channel_data[num];
|
|
return ((val*15)>>4)+1290;
|
|
}
|
|
|
|
void Frsky_d::init_hop(void)
|
|
{
|
|
uint8_t val;
|
|
uint8_t channel = rx_tx_addr[0]&0x07;
|
|
uint8_t channel_spacing = rx_tx_addr[1];
|
|
//Filter bad tables
|
|
if(channel_spacing<0x02)
|
|
channel_spacing+=0x02;
|
|
if(channel_spacing>0xE9)
|
|
channel_spacing-=0xE7;
|
|
if(channel_spacing%0x2F==0)
|
|
channel_spacing++;
|
|
|
|
hopping_frequency[0]=channel;
|
|
for(uint8_t i=1;i<50;i++) {
|
|
channel=(channel+channel_spacing) % 0xEB;
|
|
val=channel;
|
|
if((val==0x00) || (val==0x5A) || (val==0xDC))
|
|
val++;
|
|
hopping_frequency[i]=i>46?0:val;
|
|
}
|
|
}
|
|
|
|
void Frsky_d::init_channel(void)
|
|
{
|
|
for(uint8_t i=0;i<NUM_CHN;i++)
|
|
Channel_data[i]=1024;
|
|
|
|
Channel_data[THROTTLE]=204;
|
|
}
|
|
|
|
void Frsky_d::init_failsafe()
|
|
{
|
|
for(uint8_t i=0;i<NUM_CHN;i++)
|
|
failsafe_data[i]=1024;
|
|
|
|
failsafe_data[THROTTLE]=204;
|
|
fail_save = true;
|
|
}
|