philipp schoenberger
7 years ago
12 changed files with 681 additions and 157 deletions
-
41remote/src/cc2500.cpp
-
19remote/src/cc2500.h
-
69remote/src/config.h
-
306remote/src/frskyd.cpp
-
81remote/src/frskyd.h
-
16remote/src/iface_cc2500.h
-
13remote/src/io_data.cpp
-
7remote/src/io_data.h
-
40remote/src/main.cpp
-
14remote/src/pins.h
-
19remote/src/spi.cpp
-
7remote/src/spi.h
@ -1,4 +1,73 @@ |
|||
#ifndef CONFIG_H |
|||
#define CONFIG_H |
|||
|
|||
const uint8_t FRSKYD_cc2500_conf[] = { |
|||
/*02_IOCFG0*/ 0x06, |
|||
/*00_IOCFG2*/ 0x06, |
|||
/*17_MCSM1*/ 0x0c, |
|||
/*18_MCSM0*/ 0x18, |
|||
/*06_PKTLEN*/ 0x19, |
|||
/*07_PKTCTRL1*/ 0x04, |
|||
/*08_PKTCTRL0*/ 0x05, |
|||
/*3E_PATABLE*/ 0xff, |
|||
/*0B_FSCTRL1*/ 0x08, |
|||
/*0C_FSCTRL0*/ 0x00, |
|||
/*0D_FREQ2*/ 0x5c, |
|||
/*0E_FREQ1*/ 0x76, |
|||
/*0F_FREQ0*/ 0x27, |
|||
/*10_MDMCFG4*/ 0xAA, |
|||
/*11_MDMCFG3*/ 0x39, |
|||
/*12_MDMCFG2*/ 0x11, |
|||
/*13_MDMCFG1*/ 0x23, |
|||
/*14_MDMCFG0*/ 0x7a, |
|||
/*15_DEVIATN*/ 0x42 |
|||
}; |
|||
|
|||
const uint8_t FRSKY_common_end_cc2500_conf[][2] = { |
|||
{ CC2500_19_FOCCFG, 0x16 }, |
|||
{ CC2500_1A_BSCFG, 0x6c }, |
|||
{ CC2500_1B_AGCCTRL2, 0x43 }, |
|||
{ CC2500_1C_AGCCTRL1, 0x40 }, |
|||
{ CC2500_1D_AGCCTRL0, 0x91 }, |
|||
{ CC2500_21_FREND1, 0x56 }, |
|||
{ CC2500_22_FREND0, 0x10 }, |
|||
{ CC2500_23_FSCAL3, 0xa9 }, |
|||
{ CC2500_24_FSCAL2, 0x0A }, |
|||
{ CC2500_25_FSCAL1, 0x00 }, |
|||
{ CC2500_26_FSCAL0, 0x11 }, |
|||
{ CC2500_29_FSTEST, 0x59 }, |
|||
{ CC2500_2C_TEST2, 0x88 }, |
|||
{ CC2500_2D_TEST1, 0x31 }, |
|||
{ CC2500_2E_TEST0, 0x0B }, |
|||
{ CC2500_03_FIFOTHR, 0x07 }, |
|||
{ CC2500_09_ADDR, 0x00 } |
|||
}; |
|||
|
|||
const uint8_t FRSKY_common_startreg_cc2500_conf[] = { |
|||
CC2500_02_IOCFG0, |
|||
CC2500_00_IOCFG2, |
|||
CC2500_17_MCSM1, |
|||
CC2500_18_MCSM0, |
|||
CC2500_06_PKTLEN, |
|||
CC2500_07_PKTCTRL1, |
|||
CC2500_08_PKTCTRL0, |
|||
CC2500_3E_PATABLE, |
|||
CC2500_0B_FSCTRL1, |
|||
CC2500_0C_FSCTRL0, // replaced by option value |
|||
CC2500_0D_FREQ2, |
|||
CC2500_0E_FREQ1, |
|||
CC2500_0F_FREQ0, |
|||
CC2500_10_MDMCFG4, |
|||
CC2500_11_MDMCFG3, |
|||
CC2500_12_MDMCFG2, |
|||
CC2500_13_MDMCFG1, |
|||
CC2500_14_MDMCFG0, |
|||
CC2500_15_DEVIATN |
|||
}; |
|||
//AETR |
|||
#define AILERON 0 |
|||
#define ELEVATOR 1 |
|||
#define THROTTLE 2 |
|||
#define RUDDER 3 |
|||
|
|||
#endif |
|||
@ -0,0 +1,306 @@ |
|||
/*
|
|||
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; |
|||
} |
|||
@ -0,0 +1,81 @@ |
|||
#include <Arduino.h> |
|||
|
|||
#ifndef FRSKYD_H |
|||
#define FRSKYD_H |
|||
#include <cc2500.h> |
|||
|
|||
#define BIND_IN_PROGRESS protocol_flags &= ~_BV(7) |
|||
#define BIND_DONE protocol_flags |= _BV(7) |
|||
#define IS_BIND_DONE ( ( protocol_flags & _BV(7) ) !=0 ) |
|||
#define IS_BIND_IN_PROGRESS ( ( protocol_flags & _BV(7) ) ==0 ) |
|||
|
|||
class Frsky_d |
|||
{ |
|||
private: |
|||
uint8_t hopping_frequency[50]; |
|||
uint8_t calibration; |
|||
enum frsky_d_state { |
|||
FRSKY_BIND = 0, |
|||
FRSKY_BIND_DONE = 1000, |
|||
FRSKY_DATA1, |
|||
FRSKY_DATA2, |
|||
FRSKY_DATA3, |
|||
FRSKY_DATA4, |
|||
FRSKY_DATA5 |
|||
}; |
|||
int state; |
|||
|
|||
CC2500 * cc2500; |
|||
|
|||
uint16_t packet_count; |
|||
uint8_t packet[40]; |
|||
uint8_t len; |
|||
|
|||
uint8_t rx_tx_addr[5]; |
|||
uint8_t option; |
|||
uint8_t prev_option; |
|||
uint16_t counter; |
|||
|
|||
uint8_t pkt[29]; |
|||
|
|||
#define NUM_CHN 16 |
|||
// Servo data |
|||
uint8_t Channel_AUX; |
|||
|
|||
uint16_t Channel_data[NUM_CHN]; |
|||
uint16_t failsafe_data[NUM_CHN]; |
|||
|
|||
uint8_t protocol_flags=0,protocol_flags2=0; |
|||
|
|||
volatile uint32_t gWDT_entropy = 0; |
|||
uint32_t MProtocol_id_master; |
|||
uint32_t MProtocol_id; |
|||
|
|||
bool fail_save = false; |
|||
|
|||
public: |
|||
Frsky_d(CC2500 *cc2500); |
|||
void init(bool bind); |
|||
void build_bind_packet(void); |
|||
uint16_t init_frsky_2way(void); |
|||
|
|||
uint16_t read_frsky_2way(void); |
|||
void frsky2way_init(bool bind); |
|||
void frsky_init_hop(void); |
|||
|
|||
void init_cc2500(const uint8_t *ptr); |
|||
void frsky2way_data_frame(); |
|||
|
|||
uint16_t convert_channel_frsky(uint8_t num); |
|||
|
|||
void init_hop(void); |
|||
|
|||
void init_channel(void); |
|||
void init_failsafe(void); |
|||
|
|||
|
|||
}; |
|||
|
|||
|
|||
|
|||
#endif /*FRSKYD_H*/ |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue