mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-07-12 09:47:54 +00:00
teste flysky afhds2a STM2
This commit is contained in:
parent
950ee9ddc2
commit
7c17fec35f
3
.gitignore
vendored
3
.gitignore
vendored
@ -1 +1,4 @@
|
|||||||
*.ffs_db
|
*.ffs_db
|
||||||
|
*.ffs_db
|
||||||
|
*.ffs_db
|
||||||
|
*.hc
|
@ -15,6 +15,7 @@
|
|||||||
/********************/
|
/********************/
|
||||||
/** A7105 routines **/
|
/** A7105 routines **/
|
||||||
/********************/
|
/********************/
|
||||||
|
#ifdef A7105_INSTALLED
|
||||||
#include "iface_a7105.h"
|
#include "iface_a7105.h"
|
||||||
|
|
||||||
void A7105_WriteData(uint8_t len, uint8_t channel)
|
void A7105_WriteData(uint8_t len, uint8_t channel)
|
||||||
@ -36,7 +37,7 @@ void A7105_ReadData(uint8_t len=16) {
|
|||||||
A7105_CSN_off;
|
A7105_CSN_off;
|
||||||
SPI_Write(0x45);
|
SPI_Write(0x45);
|
||||||
for (i=0;i<len;i++)
|
for (i=0;i<len;i++)
|
||||||
packet[i]=SPI_SDIO_Read();
|
packet[i]=SPI_SDI_Read();
|
||||||
A7105_CSN_on;
|
A7105_CSN_on;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -48,11 +49,12 @@ void A7105_WriteReg(uint8_t address, uint8_t data) {
|
|||||||
A7105_CSN_on;
|
A7105_CSN_on;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t A7105_ReadReg(uint8_t address) {
|
uint8_t A7105_ReadReg(uint8_t address)
|
||||||
|
{
|
||||||
uint8_t result;
|
uint8_t result;
|
||||||
A7105_CSN_off;
|
A7105_CSN_off;
|
||||||
SPI_Write(address |=0x40); //bit 6 =1 for reading
|
SPI_Write(address |=0x40); //bit 6 =1 for reading
|
||||||
result = SPI_SDIO_Read();
|
result = SPI_SDI_Read();
|
||||||
A7105_CSN_on;
|
A7105_CSN_on;
|
||||||
return(result);
|
return(result);
|
||||||
}
|
}
|
||||||
@ -165,29 +167,44 @@ const uint8_t PROGMEM AFHDS2A_regs[] = {
|
|||||||
0x1e, 0xFF, 0x00, 0xFF, 0x00, 0x00, 0x3b, 0x00, 0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00, // 20 - 2f
|
0x1e, 0xFF, 0x00, 0xFF, 0x00, 0x00, 0x3b, 0x00, 0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00, // 20 - 2f
|
||||||
0x01, 0x0f // 30 - 31
|
0x01, 0x0f // 30 - 31
|
||||||
};
|
};
|
||||||
|
static const uint8_t PROGMEM JOYSWAY_regs[] = {
|
||||||
|
0x00, 0x62, -1, 0x0f, 0x00, -1 , -1 , 0x00, 0x00, 0x05, 0x00, 0x01, 0x00, 0xf5, 0x00, 0x15,
|
||||||
|
0x9e, 0x4b, 0x00, 0x03, 0x56, 0x2b, 0x12, 0x4a, 0x02, 0x80, 0x80, 0x00, 0x0e, 0x91, 0x03, 0x0f,
|
||||||
|
0x16, 0x2a, 0x00, -1, -1, -1, 0x3a, 0x06, 0x1f, 0x47, 0x80, 0x01, 0x05, 0x45, 0x18, 0x00,
|
||||||
|
0x01, 0x0f, 0x00
|
||||||
|
};
|
||||||
|
|
||||||
#define ID_NORMAL 0x55201041
|
#define ID_NORMAL 0x55201041
|
||||||
#define ID_PLUS 0xAA201041
|
#define ID_PLUS 0xAA201041
|
||||||
void A7105_Init(uint8_t protocol)
|
void A7105_Init(uint8_t protocol)
|
||||||
{
|
{
|
||||||
void *A7105_Regs;
|
uint8_t *A7105_Regs;
|
||||||
|
|
||||||
if(protocol==INIT_FLYSKY || protocol==INIT_FLYSKY_AFHDS2A)
|
if(protocol==INIT_FLYSKY || protocol==INIT_FLYSKY_AFHDS2A)
|
||||||
{
|
{
|
||||||
A7105_WriteID(0x5475c52A);//0x2Ac57554
|
A7105_WriteID(0x5475c52A);//0x2Ac57554
|
||||||
if(protocol==INIT_FLYSKY_AFHDS2A)
|
if(protocol==INIT_FLYSKY_AFHDS2A)
|
||||||
A7105_Regs=(void *)AFHDS2A_regs;
|
A7105_Regs=(uint8_t*)AFHDS2A_regs;
|
||||||
else
|
else
|
||||||
A7105_Regs=(void *)FLYSKY_A7105_regs;
|
A7105_Regs=(uint8_t*)FLYSKY_A7105_regs;
|
||||||
|
}
|
||||||
|
else if(protocol==INIT_JOYSWAY)
|
||||||
|
{
|
||||||
|
A7105_WriteID(0x5475c52a);//0x2Ac57554
|
||||||
|
A7105_Regs=(uint8_t*)JOYSWAY_regs;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{ // HUBSAN
|
||||||
A7105_WriteID(ID_NORMAL);
|
A7105_WriteID(ID_NORMAL);
|
||||||
A7105_Regs=(void *)HUBSAN_A7105_regs;
|
A7105_Regs=(uint8_t*)HUBSAN_A7105_regs;
|
||||||
}
|
}
|
||||||
for (uint8_t i = 0; i < 0x33; i++){
|
for (uint8_t i = 0; i < 0x33; i++){
|
||||||
if( pgm_read_byte_near((uint16_t)(A7105_Regs)+i != 0xFF)
|
if( pgm_read_byte_near(&A7105_Regs[i]) != 0xFF)
|
||||||
A7105_WriteReg(i, pgm_read_byte_near((uint16_t)(A7105_Regs)+i));
|
A7105_WriteReg(i, pgm_read_byte_near(&A7105_Regs[i]));
|
||||||
}
|
}
|
||||||
|
if(protocol==INIT_JOYSWAY)
|
||||||
|
A7105_Strobe(A7105_PLL);
|
||||||
|
else
|
||||||
A7105_Strobe(A7105_STANDBY);
|
A7105_Strobe(A7105_STANDBY);
|
||||||
|
|
||||||
//IF Filter Bank Calibration
|
//IF Filter Bank Calibration
|
||||||
@ -195,6 +212,13 @@ void A7105_Init(uint8_t protocol)
|
|||||||
while(A7105_ReadReg(A7105_02_CALC)); // Wait for calibration to end
|
while(A7105_ReadReg(A7105_02_CALC)); // Wait for calibration to end
|
||||||
// A7105_ReadReg(A7105_22_IF_CALIB_I);
|
// A7105_ReadReg(A7105_22_IF_CALIB_I);
|
||||||
// A7105_ReadReg(A7105_24_VCO_CURCAL);
|
// A7105_ReadReg(A7105_24_VCO_CURCAL);
|
||||||
|
if(protocol==INIT_JOYSWAY) {
|
||||||
|
A7105_Strobe(A7105_STANDBY);
|
||||||
|
//VCO Current Calibration
|
||||||
|
A7105_WriteReg(A7105_24_VCO_CURCAL,0x13); //Recommended calibration from A7105 Datasheet
|
||||||
|
A7105_WriteReg(A7105_25_VCO_SBCAL_I,0x09); //Recommended calibration from A7105 Datasheet
|
||||||
|
A7105_WriteID(binding_idx);
|
||||||
|
}
|
||||||
|
|
||||||
if(protocol==INIT_FLYSKY || protocol==INIT_FLYSKY_AFHDS2A)
|
if(protocol==INIT_FLYSKY || protocol==INIT_FLYSKY_AFHDS2A)
|
||||||
{
|
{
|
||||||
@ -205,25 +229,40 @@ void A7105_Init(uint8_t protocol)
|
|||||||
}
|
}
|
||||||
|
|
||||||
//VCO Bank Calibrate channel 0
|
//VCO Bank Calibrate channel 0
|
||||||
|
if(protocol==INIT_JOYSWAY) {
|
||||||
|
A7105_Strobe(A7105_PLL);
|
||||||
|
A7105_WriteReg(A7105_02_CALC,1);
|
||||||
|
}
|
||||||
|
else {
|
||||||
A7105_WriteReg(A7105_0F_CHANNEL, 0);
|
A7105_WriteReg(A7105_0F_CHANNEL, 0);
|
||||||
A7105_WriteReg(A7105_02_CALC,2);
|
A7105_WriteReg(A7105_02_CALC,2);
|
||||||
|
}
|
||||||
while(A7105_ReadReg(A7105_02_CALC)); // Wait for calibration to end
|
while(A7105_ReadReg(A7105_02_CALC)); // Wait for calibration to end
|
||||||
// A7105_ReadReg(A7105_25_VCO_SBCAL_I);
|
// A7105_ReadReg(A7105_25_VCO_SBCAL_I);
|
||||||
|
|
||||||
|
if(protocol==INIT_JOYSWAY) {
|
||||||
|
A7105_Strobe(A7105_STANDBY);
|
||||||
|
//VCO Current Calibration
|
||||||
|
A7105_WriteReg(A7105_24_VCO_CURCAL,0x13); //Recommended calibration from A7105 Datasheet
|
||||||
|
A7105_WriteReg(A7105_25_VCO_SBCAL_I,0x09); //Recommended calibration from A7105 Datasheet
|
||||||
|
}
|
||||||
|
else {
|
||||||
//VCO Bank Calibrate channel A0
|
//VCO Bank Calibrate channel A0
|
||||||
A7105_WriteReg(A7105_0F_CHANNEL, 0xa0);
|
A7105_WriteReg(A7105_0F_CHANNEL, 0xa0);
|
||||||
A7105_WriteReg(A7105_02_CALC, 2);
|
A7105_WriteReg(A7105_02_CALC, 2);
|
||||||
while(A7105_ReadReg(A7105_02_CALC)); // Wait for calibration to end
|
while(A7105_ReadReg(A7105_02_CALC)); // Wait for calibration to end
|
||||||
// A7105_ReadReg(A7105_25_VCO_SBCAL_I);
|
// A7105_ReadReg(A7105_25_VCO_SBCAL_I);
|
||||||
|
|
||||||
//Reset VCO Band calibration
|
//Reset VCO Band calibration
|
||||||
if(protocol==INIT_FLYSKY)
|
if(protocol==INIT_FLYSKY)
|
||||||
A7105_WriteReg(A7105_25_VCO_SBCAL_I,0x08);
|
A7105_WriteReg(A7105_25_VCO_SBCAL_I,0x08);
|
||||||
if(protocol==INIT_FLYSKY_AFHDS2A)
|
if(protocol==INIT_FLYSKY_AFHDS2A)
|
||||||
A7105_WriteReg(A7105_25_VCO_SBCAL_I,0x0A);
|
A7105_WriteReg(A7105_25_VCO_SBCAL_I,0x0A);
|
||||||
|
}
|
||||||
|
|
||||||
A7105_SetTxRxMode(TX_EN);
|
A7105_SetTxRxMode(TX_EN);
|
||||||
A7105_SetPower();
|
A7105_SetPower();
|
||||||
|
|
||||||
A7105_Strobe(A7105_STANDBY);
|
A7105_Strobe(A7105_STANDBY);
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
@ -19,78 +19,6 @@
|
|||||||
|
|
||||||
#define EVEN_ODD 0x00
|
#define EVEN_ODD 0x00
|
||||||
//#define EVEN_ODD 0x01
|
//#define EVEN_ODD 0x01
|
||||||
static const uint8_t A7105_regs[] = {
|
|
||||||
0x00, 0x62, -1, 0x0f, 0x00, -1 , -1 , 0x00, 0x00, 0x05, 0x00, 0x01, 0x00, 0xf5, 0x00, 0x15,
|
|
||||||
0x9e, 0x4b, 0x00, 0x03, 0x56, 0x2b, 0x12, 0x4a, 0x02, 0x80, 0x80, 0x00, 0x0e, 0x91, 0x03, 0x0f,
|
|
||||||
0x16, 0x2a, 0x00, -1, -1, -1, 0x3a, 0x06, 0x1f, 0x47, 0x80, 0x01, 0x05, 0x45, 0x18, 0x00,
|
|
||||||
0x01, 0x0f, 0x00
|
|
||||||
};
|
|
||||||
|
|
||||||
static uint8_t next_ch;
|
|
||||||
|
|
||||||
static int joysway_init()
|
|
||||||
{
|
|
||||||
int i;
|
|
||||||
uint8_t if_calibration1;
|
|
||||||
//uint8_t vco_calibration0;
|
|
||||||
//uint8_t vco_calibration1;
|
|
||||||
|
|
||||||
phase = 0;
|
|
||||||
next_ch = 0x30;
|
|
||||||
|
|
||||||
for (i = 0; i < 0x33; i++)
|
|
||||||
if((uint8_t)A7105_regs[i] != -1)
|
|
||||||
A7105_WriteReg(i, A7105_regs[i]);
|
|
||||||
A7105_WriteID(0x5475c52a);
|
|
||||||
|
|
||||||
A7105_Strobe(A7105_PLL);
|
|
||||||
|
|
||||||
//IF Filter Bank Calibration
|
|
||||||
A7105_WriteReg(0x02, 1);
|
|
||||||
A7105_ReadReg(0x02);
|
|
||||||
uint32_t ms = micros();
|
|
||||||
while(micros() - ms < 500) {
|
|
||||||
if(! A7105_ReadReg(0x02))
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
if (micros() - ms >= 500)
|
|
||||||
return 0;
|
|
||||||
A7105_Strobe(A7105_STANDBY);
|
|
||||||
if_calibration1 = A7105_ReadReg(0x22);
|
|
||||||
if(if_calibration1 & A7105_MASK_FBCF) {
|
|
||||||
//Calibration failed...what do we do?
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
//VCO Current Calibration
|
|
||||||
A7105_WriteReg(0x24, 0x13); //Recomended calibration from A7105 Datasheet
|
|
||||||
A7105_WriteReg(0x25, 0x09); //Recomended calibration from A7105 Datasheet
|
|
||||||
|
|
||||||
A7105_WriteID(MProtocol_id_master);
|
|
||||||
A7105_Strobe(A7105_PLL);
|
|
||||||
A7105_WriteReg(0x02, 1);
|
|
||||||
ms = micros();
|
|
||||||
while(micros() - ms < 500) {
|
|
||||||
if(! A7105_ReadReg(0x02))
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
if (micros() - ms >= 500)
|
|
||||||
return 0;
|
|
||||||
A7105_Strobe(A7105_STANDBY);
|
|
||||||
if_calibration1 = A7105_ReadReg(0x22);
|
|
||||||
if(if_calibration1 & A7105_MASK_FBCF) {
|
|
||||||
//Calibration failed...what do we do?
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
A7105_WriteReg(0x24, 0x13); //Recomended calibration from A7105 Datasheet
|
|
||||||
A7105_WriteReg(0x25, 0x09); //Recomended calibration from A7105 Datasheet
|
|
||||||
|
|
||||||
A7105_SetTxRxMode(TX_EN);
|
|
||||||
A7105_SetPower();
|
|
||||||
|
|
||||||
A7105_Strobe(A7105_STANDBY);
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void joysway_build_packet()
|
static void joysway_build_packet()
|
||||||
{
|
{
|
||||||
@ -101,16 +29,15 @@ static void joysway_build_packet()
|
|||||||
//Center = 0x5d9
|
//Center = 0x5d9
|
||||||
//1 % = 5
|
//1 % = 5
|
||||||
packet[0] = phase == 0 ? 0xdd : 0xff;
|
packet[0] = phase == 0 ? 0xdd : 0xff;
|
||||||
packet[1] = (MProtocol_id_master >> 24) & 0xff;
|
packet[1] = (binding_idx >> 24) & 0xff;
|
||||||
packet[2] = (MProtocol_id_master >> 16) & 0xff;
|
packet[2] = (binding_idx >> 16) & 0xff;
|
||||||
packet[3] = (MProtocol_id_master >> 8) & 0xff;
|
packet[3] = (binding_idx >> 8) & 0xff;
|
||||||
packet[4] = (MProtocol_id_master >> 0) & 0xff;
|
packet[4] = (binding_idx >> 0) & 0xff;
|
||||||
packet[5] = 0x00;
|
packet[5] = 0x00;
|
||||||
static const int chmap[4] = {6, 7, 10, 11};
|
static const int chmap[4] = {6, 7, 10, 11};
|
||||||
for (i = 0; i < 4; i++) {
|
for (i = 0; i < 4; i++) {
|
||||||
// if (i >= Model.num_channels) { packet[chmap[i]] = 0x64; continue; }
|
// if (i >= Model.num_channels) { packet[chmap[i]] = 0x64; continue; }
|
||||||
uint32_t value = map(limit_channel_100(i),servo_min_100,servo_max_100,0,204);
|
packet[chmap[i]] = map(limit_channel_100(i),servo_min_100,servo_max_100,0,204);
|
||||||
packet[chmap[i]] = value;
|
|
||||||
}
|
}
|
||||||
packet[8] = 0x64;
|
packet[8] = 0x64;
|
||||||
packet[9] = 0x64;
|
packet[9] = 0x64;
|
||||||
@ -124,39 +51,35 @@ static void joysway_build_packet()
|
|||||||
|
|
||||||
static uint16_t joysway_cb()
|
static uint16_t joysway_cb()
|
||||||
{
|
{
|
||||||
uint8_t ch;
|
|
||||||
if (phase == 254) {
|
if (phase == 254) {
|
||||||
phase = 0;
|
phase = 0;
|
||||||
A7105_WriteID(0x5475c52a);
|
A7105_WriteID(0x5475c52a);
|
||||||
ch = 0x0a;
|
hopping_frequency_no = 0x0a;
|
||||||
} else if (phase == 2) {
|
} else if (phase == 2) {
|
||||||
A7105_WriteID(MProtocol_id_master);
|
A7105_WriteID(binding_idx);
|
||||||
ch = 0x30;
|
hopping_frequency_no = 0x30;
|
||||||
} else {
|
} else {
|
||||||
if ((phase & 0x01) ^ EVEN_ODD) {
|
if ((phase & 0x01) ^ EVEN_ODD) {
|
||||||
ch = 0x30;
|
hopping_frequency_no = 0x30;
|
||||||
} else {
|
} else {
|
||||||
ch = next_ch;
|
hopping_frequency_no = rf_ch_num;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (! ((phase & 0x01) ^ EVEN_ODD)) {
|
if (! ((phase & 0x01) ^ EVEN_ODD)) {
|
||||||
next_ch++;
|
rf_ch_num++;
|
||||||
if (next_ch == 0x45)
|
if (rf_ch_num == 0x45)
|
||||||
next_ch = 0x30;
|
rf_ch_num = 0x30;
|
||||||
}
|
}
|
||||||
joysway_build_packet();
|
joysway_build_packet();
|
||||||
A7105_Strobe(A7105_STANDBY);
|
A7105_Strobe(A7105_STANDBY);
|
||||||
A7105_WriteData(16, ch);
|
A7105_WriteData(16, hopping_frequency_no);
|
||||||
phase++;
|
phase++;
|
||||||
return 6000;
|
return 6000;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t JOYSWAY_Setup() {
|
static uint16_t JOYSWAY_Setup() {
|
||||||
while(1) {
|
binding_idx = MProtocol_id_master | 0xf8000000;
|
||||||
A7105_Reset();
|
A7105_Init(INIT_JOYSWAY);
|
||||||
if (joysway_init())
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
return 2400;
|
return 2400;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -12,9 +12,23 @@
|
|||||||
You should have received a copy of the GNU General Public License
|
You should have received a copy of the GNU General Public License
|
||||||
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
#ifndef STM32_BOARD
|
||||||
|
/************************************/
|
||||||
/************************************/
|
/************************************/
|
||||||
/** Arduino replacement routines **/
|
/** Arduino replacement routines **/
|
||||||
/************************************/
|
/************************************/
|
||||||
|
// replacement map()
|
||||||
|
int16_t map( int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max)
|
||||||
|
{
|
||||||
|
// return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||||
|
long y ;
|
||||||
|
x -= in_min ;
|
||||||
|
y = out_max - out_min ;
|
||||||
|
y *= x ;
|
||||||
|
x = y / (in_max - in_min) ;
|
||||||
|
return x + out_min ;
|
||||||
|
}
|
||||||
|
|
||||||
// replacement millis() and micros()
|
// replacement millis() and micros()
|
||||||
// These work polled, no interrupts
|
// These work polled, no interrupts
|
||||||
// micros() MUST be called at least once every 32 milliseconds
|
// micros() MUST be called at least once every 32 milliseconds
|
||||||
@ -106,7 +120,7 @@ void delayMicroseconds(unsigned int us)
|
|||||||
return;
|
return;
|
||||||
us <<= 2; // * 4
|
us <<= 2; // * 4
|
||||||
us -= 2; // - 2
|
us -= 2; // - 2
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
__asm__ __volatile__ (
|
__asm__ __volatile__ (
|
||||||
"1: sbiw %0,1" "\n\t" // 2 cycles
|
"1: sbiw %0,1" "\n\t" // 2 cycles
|
||||||
"nop \n"
|
"nop \n"
|
||||||
@ -123,11 +137,12 @@ void delayMicroseconds(unsigned int us)
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifndef XMEGA
|
#ifndef ORANGE_TX
|
||||||
void init()
|
void init()
|
||||||
{
|
{
|
||||||
// this needs to be called before setup() or some functions won't work there
|
// this needs to be called before setup() or some functions won't work there
|
||||||
sei();
|
sei();
|
||||||
}
|
}
|
||||||
#endif //XMEGA
|
#endif //ORANGE_TX
|
||||||
|
|
||||||
|
#endif //STM32_BOARD
|
||||||
|
@ -1,3 +1,4 @@
|
|||||||
|
|
||||||
/*
|
/*
|
||||||
This project is free software: you can redistribute it and/or modify
|
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
|
it under the terms of the GNU General Public License as published by
|
||||||
@ -17,6 +18,7 @@
|
|||||||
//CC2500 SPI routines
|
//CC2500 SPI routines
|
||||||
//-------------------------------
|
//-------------------------------
|
||||||
//-------------------------------
|
//-------------------------------
|
||||||
|
#ifdef CC2500_INSTALLED
|
||||||
#include "iface_cc2500.h"
|
#include "iface_cc2500.h"
|
||||||
|
|
||||||
//----------------------------
|
//----------------------------
|
||||||
@ -152,4 +154,4 @@ void CC2500_SetPower()
|
|||||||
prev_power=power;
|
prev_power=power;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
@ -67,7 +67,12 @@ enum H8_3D_FLAGS {
|
|||||||
|
|
||||||
enum H8_3D_FLAGS_2 {
|
enum H8_3D_FLAGS_2 {
|
||||||
// flags going to packet[18]
|
// flags going to packet[18]
|
||||||
H8_3D_FLAG_CALIBRATE = 0x20, // accelerometer calibration
|
H8_3D_FLAG_CAM_UP = 0x04,
|
||||||
|
H8_3D_FLAG_CAM_DOWN = 0x08,
|
||||||
|
H8_3D_FLAG_CALIBRATE2 = 0x10, // acc calib. (H11D, H20)
|
||||||
|
H8_3D_FLAG_CALIBRATE = 0x20, // acc calib. (H8 3D), headless calib (H20)
|
||||||
|
H8_3D_FLAG_SNAPSHOT = 0x40,
|
||||||
|
H8_3D_FLAG_VIDEO = 0x80,
|
||||||
};
|
};
|
||||||
|
|
||||||
static void __attribute__((unused)) CG023_send_packet(uint8_t bind)
|
static void __attribute__((unused)) CG023_send_packet(uint8_t bind)
|
||||||
@ -119,8 +124,10 @@ static void __attribute__((unused)) CG023_send_packet(uint8_t bind)
|
|||||||
| GET_FLAG(Servo_AUX2,H8_3D_FLAG_LIGTH) //H22 light
|
| GET_FLAG(Servo_AUX2,H8_3D_FLAG_LIGTH) //H22 light
|
||||||
| GET_FLAG(Servo_AUX3,H8_3D_FLAG_HEADLESS)
|
| GET_FLAG(Servo_AUX3,H8_3D_FLAG_HEADLESS)
|
||||||
| GET_FLAG(Servo_AUX4,H8_3D_FLAG_RTH); // 180/360 flip mode on H8 3D
|
| GET_FLAG(Servo_AUX4,H8_3D_FLAG_RTH); // 180/360 flip mode on H8 3D
|
||||||
if(Servo_AUX5)
|
packet[18] = GET_FLAG(Servo_AUX5,H8_3D_FLAG_CALIBRATE)
|
||||||
packet[18] = H8_3D_FLAG_CALIBRATE;
|
| GET_FLAG(Servo_AUX5,H8_3D_FLAG_CALIBRATE2)
|
||||||
|
| GET_FLAG(Servo_AUX6, H8_3D_FLAG_SNAPSHOT)
|
||||||
|
| GET_FLAG(Servo_AUX7, H8_3D_FLAG_VIDEO);
|
||||||
}
|
}
|
||||||
uint8_t sum = packet[9];
|
uint8_t sum = packet[9];
|
||||||
for (uint8_t i=10; i < H8_3D_PACKET_SIZE-1; i++)
|
for (uint8_t i=10; i < H8_3D_PACKET_SIZE-1; i++)
|
||||||
|
@ -12,9 +12,9 @@
|
|||||||
You should have received a copy of the GNU General Public License
|
You should have received a copy of the GNU General Public License
|
||||||
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
#ifdef CYRF6936_INSTALLED
|
||||||
#include "iface_cyrf6936.h"
|
#include "iface_cyrf6936.h"
|
||||||
|
|
||||||
|
|
||||||
void CYRF_WriteRegister(uint8_t address, uint8_t data)
|
void CYRF_WriteRegister(uint8_t address, uint8_t data)
|
||||||
{
|
{
|
||||||
CYRF_CSN_off;
|
CYRF_CSN_off;
|
||||||
@ -182,11 +182,6 @@ void CYRF_WritePreamble(uint32_t preamble)
|
|||||||
/*
|
/*
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
static void CYRF_StartReceive()
|
|
||||||
{
|
|
||||||
CYRF_WriteRegister(CYRF_05_RX_CTRL,0x87);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*static void CYRF_ReadDataPacket(uint8_t dpbuffer[])
|
/*static void CYRF_ReadDataPacket(uint8_t dpbuffer[])
|
||||||
{
|
{
|
||||||
CYRF_ReadRegisterMulti(CYRF_21_RX_BUFFER, dpbuffer, 0x10);
|
CYRF_ReadRegisterMulti(CYRF_21_RX_BUFFER, dpbuffer, 0x10);
|
||||||
@ -243,10 +238,14 @@ void CYRF_FindBestChannels(uint8_t *channels, uint8_t len, uint8_t minspace, uin
|
|||||||
for(i = 0; i < NUM_FREQ; i++)
|
for(i = 0; i < NUM_FREQ; i++)
|
||||||
{
|
{
|
||||||
CYRF_ConfigRFChannel(i);
|
CYRF_ConfigRFChannel(i);
|
||||||
CYRF_ReadRegister(CYRF_13_RSSI);
|
delayMicroseconds(270); //slow channel require 270usec for synthesizer to settle
|
||||||
CYRF_StartReceive();
|
if( !(CYRF_ReadRegister(CYRF_05_RX_CTRL) & 0x80)) {
|
||||||
delayMicroseconds(10);
|
CYRF_WriteRegister(CYRF_05_RX_CTRL, 0x80); //Prepare to receive
|
||||||
rssi[i] = CYRF_ReadRegister(CYRF_13_RSSI);
|
delayMicroseconds(15);
|
||||||
|
CYRF_ReadRegister(CYRF_13_RSSI); //dummy read
|
||||||
|
delayMicroseconds(15); //The conversion can occur as often as once every 12us
|
||||||
|
}
|
||||||
|
rssi[i] = CYRF_ReadRegister(CYRF_13_RSSI)&0x1F;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (i = 0; i < len; i++)
|
for (i = 0; i < len; i++)
|
||||||
@ -262,7 +261,9 @@ void CYRF_FindBestChannels(uint8_t *channels, uint8_t len, uint8_t minspace, uin
|
|||||||
rssi[j] = 0xff;
|
rssi[j] = 0xff;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
CYRF_WriteRegister(CYRF_29_RX_ABORT, 0x20); // Abort RX operation
|
||||||
CYRF_SetTxRxMode(TX_EN);
|
CYRF_SetTxRxMode(TX_EN);
|
||||||
|
CYRF_WriteRegister(CYRF_29_RX_ABORT, 0x20); // Clear abort RX
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(DEVO_CYRF6936_INO) || defined(J6PRO_CYRF6936_INO)
|
#if defined(DEVO_CYRF6936_INO) || defined(J6PRO_CYRF6936_INO)
|
||||||
@ -298,3 +299,4 @@ static void __attribute__((unused)) CYRF_PROGMEM_ConfigSOPCode(const uint8_t *da
|
|||||||
code[i]=pgm_read_byte_near(&data[i]);
|
code[i]=pgm_read_byte_near(&data[i]);
|
||||||
CYRF_ConfigSOPCode(code);
|
CYRF_ConfigSOPCode(code);
|
||||||
}
|
}
|
||||||
|
#endif
|
@ -15,17 +15,6 @@
|
|||||||
/************************/
|
/************************/
|
||||||
/** Convert routines **/
|
/** Convert routines **/
|
||||||
/************************/
|
/************************/
|
||||||
int16_t map( int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max)
|
|
||||||
{
|
|
||||||
// return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
|
||||||
long y ;
|
|
||||||
x -= in_min ;
|
|
||||||
y = out_max - out_min ;
|
|
||||||
y *= x ;
|
|
||||||
x = y / (in_max - in_min) ;
|
|
||||||
return x + out_min ;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Channel value is converted to 8bit values full scale
|
// Channel value is converted to 8bit values full scale
|
||||||
uint8_t convert_channel_8b(uint8_t num)
|
uint8_t convert_channel_8b(uint8_t num)
|
||||||
{
|
{
|
||||||
|
@ -135,8 +135,7 @@ static uint8_t __attribute__((unused)) get_pn_row(uint8_t channel)
|
|||||||
}
|
}
|
||||||
|
|
||||||
const uint8_t PROGMEM init_vals[][2] = {
|
const uint8_t PROGMEM init_vals[][2] = {
|
||||||
{CYRF_02_TX_CTRL, 0x03}, // TX interrupt complete and error enabled
|
{CYRF_02_TX_CTRL, 0x00}, // All TX interrupt disabled
|
||||||
//0x00 in deviation but needed to know when transmit is over
|
|
||||||
{CYRF_05_RX_CTRL, 0x00}, // All RX interrupt disabled
|
{CYRF_05_RX_CTRL, 0x00}, // All RX interrupt disabled
|
||||||
{CYRF_28_CLK_EN, 0x02}, // Force receive clock enable
|
{CYRF_28_CLK_EN, 0x02}, // Force receive clock enable
|
||||||
{CYRF_32_AUTO_CAL_TIME, 0x3c}, // Default init value
|
{CYRF_32_AUTO_CAL_TIME, 0x3c}, // Default init value
|
||||||
@ -369,13 +368,13 @@ static uint8_t __attribute__((unused)) DSM_Check_RX_packet()
|
|||||||
uint16_t ReadDsm()
|
uint16_t ReadDsm()
|
||||||
{
|
{
|
||||||
#define DSM_CH1_CH2_DELAY 4010 // Time between write of channel 1 and channel 2
|
#define DSM_CH1_CH2_DELAY 4010 // Time between write of channel 1 and channel 2
|
||||||
#define DSM_WRITE_DELAY 1550 // Time after write to verify write complete
|
#define DSM_WRITE_DELAY 1950 // Time after write to verify write complete
|
||||||
#define DSM_READ_DELAY 600 // Time before write to check read phase, and switch channels. Was 400 but 600 seems what the 328p needs to read a packet
|
#define DSM_READ_DELAY 600 // Time before write to check read phase, and switch channels. Was 400 but 600 seems what the 328p needs to read a packet
|
||||||
uint16_t start;
|
|
||||||
#if defined DSM_TELEMETRY
|
#if defined DSM_TELEMETRY
|
||||||
uint8_t rx_phase;
|
uint8_t rx_phase;
|
||||||
uint8_t len;
|
uint8_t len;
|
||||||
#endif
|
#endif
|
||||||
|
uint8_t start;
|
||||||
|
|
||||||
switch(phase)
|
switch(phase)
|
||||||
{
|
{
|
||||||
@ -451,19 +450,28 @@ uint16_t ReadDsm()
|
|||||||
return DSM_WRITE_DELAY;
|
return DSM_WRITE_DELAY;
|
||||||
case DSM_CH1_CHECK_A:
|
case DSM_CH1_CHECK_A:
|
||||||
case DSM_CH1_CHECK_B:
|
case DSM_CH1_CHECK_B:
|
||||||
start=micros();
|
|
||||||
while ((uint16_t)micros()-start < 500) // Wait max 500µs
|
|
||||||
if(CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02)
|
|
||||||
break;
|
|
||||||
set_sop_data_crc();
|
|
||||||
phase++; // change from CH1_CHECK to CH2_WRITE
|
|
||||||
return DSM_CH1_CH2_DELAY - DSM_WRITE_DELAY;
|
|
||||||
case DSM_CH2_CHECK_A:
|
case DSM_CH2_CHECK_A:
|
||||||
case DSM_CH2_CHECK_B:
|
case DSM_CH2_CHECK_B:
|
||||||
start=micros();
|
start=micros();
|
||||||
while ((uint16_t)micros()-start < 500) // Wait max 500µs
|
while ((uint8_t)micros()-start < 100) // Wait max 100µs, max I've seen is 50µs
|
||||||
if(CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02)
|
if(CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02)
|
||||||
break;
|
break;
|
||||||
|
if(phase==DSM_CH1_CHECK_A || phase==DSM_CH1_CHECK_B)
|
||||||
|
{
|
||||||
|
#if defined DSM_TELEMETRY
|
||||||
|
// reset cyrf6936 if freezed after switching from TX to RX
|
||||||
|
if (((CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x22) == 0x20) || (CYRF_ReadRegister(CYRF_02_TX_CTRL) & 0x80))
|
||||||
|
{
|
||||||
|
CYRF_Reset();
|
||||||
|
cyrf_config();
|
||||||
|
cyrf_configdata();
|
||||||
|
CYRF_SetTxRxMode(TX_EN);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
set_sop_data_crc();
|
||||||
|
phase++; // change from CH1_CHECK to CH2_WRITE
|
||||||
|
return DSM_CH1_CH2_DELAY - DSM_WRITE_DELAY;
|
||||||
|
}
|
||||||
if (phase == DSM_CH2_CHECK_A)
|
if (phase == DSM_CH2_CHECK_A)
|
||||||
CYRF_SetPower(0x28); //Keep transmit power in sync
|
CYRF_SetPower(0x28); //Keep transmit power in sync
|
||||||
#if defined DSM_TELEMETRY
|
#if defined DSM_TELEMETRY
|
||||||
|
@ -63,7 +63,7 @@ static void __attribute__((unused)) DEVO_add_pkt_suffix()
|
|||||||
BIND_SET_PULLUP; // set pullup
|
BIND_SET_PULLUP; // set pullup
|
||||||
if(IS_BIND_BUTTON_on)
|
if(IS_BIND_BUTTON_on)
|
||||||
{
|
{
|
||||||
eeprom_write_byte((uint8_t*)(30+mode_select),0x01); // Set fixed id mode for the current model
|
eeprom_write_byte((EE_ADDR)(30+mode_select),0x01); // Set fixed id mode for the current model
|
||||||
option=1;
|
option=1;
|
||||||
}
|
}
|
||||||
BIND_SET_OUTPUT;
|
BIND_SET_OUTPUT;
|
||||||
|
@ -1,42 +1,40 @@
|
|||||||
// adaptation de https://github.com/goebish/deviation/blob/c5dd9fcc1441fc05fe9effa4c378886aeb3938d4/src/protocol/flysky_afhds2a_a7105.c
|
// adaptation de https://github.com/goebish/deviation/blob/c5dd9fcc1441fc05fe9effa4c378886aeb3938d4/src/protocol/flysky_afhds2a_a7105.c
|
||||||
#ifdef AFHDS2A_A7105_INO
|
#ifdef AFHDS2A_A7105_INO
|
||||||
#define EEPROMadress 0 // rx ID 32bit
|
#define EEPROMadress 0 // rx ID 32bit
|
||||||
#define SERVO_HZ 0 //Frequency's servo 0=50 1=400 2=5
|
#define SERVO_HZ 50 //Frequency's servo 0=50 1=400 2=5
|
||||||
|
|
||||||
#define TXPACKET_SIZE 38
|
#define TXPACKET_SIZE 38
|
||||||
#define RXPACKET_SIZE 37
|
#define RXPACKET_SIZE 37
|
||||||
#define NUMFREQ 16
|
#define NUMFREQ 16
|
||||||
#define TXID_SIZE 4
|
|
||||||
#define RXID_SIZE 4
|
|
||||||
|
|
||||||
static uint8_t txid[RXID_SIZE];
|
static uint8_t txid[4];
|
||||||
static uint8_t rxid[RXID_SIZE];
|
static uint8_t rxid[4];
|
||||||
static uint8_t packet_type;
|
static uint8_t packet_type;
|
||||||
static uint8_t bind_reply;
|
static uint8_t bind_reply;
|
||||||
|
|
||||||
|
|
||||||
enum{
|
enum{
|
||||||
PACKET_STICKS,
|
PACKET_STICKS,
|
||||||
PACKET_SETTINGS,
|
PACKET_SETTINGS,
|
||||||
PACKET_FAILSAFE,
|
PACKET_FAILSAFE,
|
||||||
};
|
};
|
||||||
|
|
||||||
enum{
|
enum{
|
||||||
BIND1,
|
BIND1,
|
||||||
BIND2,
|
BIND2,
|
||||||
BIND3,
|
BIND3,
|
||||||
BIND4,
|
BIND4,
|
||||||
DATA1,
|
DATA1,
|
||||||
};
|
};
|
||||||
enum {
|
enum {
|
||||||
PWM_IBUS = 0,
|
PWM_IBUS = 0,
|
||||||
PPM_IBUS,
|
PPM_IBUS,
|
||||||
PWM_SBUS,
|
PWM_SBUS,
|
||||||
PPM_SBUS
|
PPM_SBUS
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
static void build_packet(uint8_t type) {
|
static void build_packet(uint8_t type) {
|
||||||
switch(type) {
|
switch(type) {
|
||||||
case PACKET_STICKS:
|
case PACKET_STICKS:
|
||||||
packet[0] = 0x58;
|
packet[0] = 0x58;
|
||||||
@ -49,6 +47,24 @@
|
|||||||
packet[37] = 0x00;
|
packet[37] = 0x00;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case PACKET_FAILSAFE:
|
||||||
|
packet[0] = 0x56;
|
||||||
|
memcpy( &packet[1], txid, 4);
|
||||||
|
memcpy( &packet[5], rxid, 4);
|
||||||
|
for(uint8_t ch=0; ch<14; ch++) {
|
||||||
|
if(ch==0) {
|
||||||
|
// if(ch < Model.num_channels && (Model.limits[ch].flags & CH_FAILSAFE_EN)) {
|
||||||
|
packet[9 + ch*2] = Servo_data[AUX11] & 0xff;
|
||||||
|
packet[10+ ch*2] = (Servo_data[AUX11] >> 8) & 0xff;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
packet[9 + ch*2] = 0xff;
|
||||||
|
packet[10+ ch*2] = 0xff;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
packet[37] = 0x00;
|
||||||
|
break;
|
||||||
|
|
||||||
case PACKET_SETTINGS:
|
case PACKET_SETTINGS:
|
||||||
packet[0] = 0xaa;
|
packet[0] = 0xaa;
|
||||||
memcpy( &packet[1], txid, 4);
|
memcpy( &packet[1], txid, 4);
|
||||||
@ -73,39 +89,19 @@
|
|||||||
packet[21] = 0xde;
|
packet[21] = 0xde;
|
||||||
packet[37] = 0x00;
|
packet[37] = 0x00;
|
||||||
break;
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
case PACKET_FAILSAFE:
|
// telemetry sensors ID
|
||||||
packet[0] = 0x56;
|
enum{
|
||||||
memcpy( &packet[1], txid, 4);
|
|
||||||
memcpy( &packet[5], rxid, 4);
|
|
||||||
for(uint8_t ch=0; ch<14; ch++) {
|
|
||||||
if(ch==0) {
|
|
||||||
// if(ch < Model.num_channels && (Model.limits[ch].flags & CH_FAILSAFE_EN)) {
|
|
||||||
uint32_t value = ((uint32_t)Servo_data[AUX11] + 100) * 5 + 1000;
|
|
||||||
packet[9 + ch*2] = value & 0xff;
|
|
||||||
packet[10+ ch*2] = (value >> 8) & 0xff;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
packet[9 + ch*2] = 0xff;
|
|
||||||
packet[10+ ch*2] = 0xff;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
packet[37] = 0x00;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(TELEMETRY)
|
|
||||||
// telemetry sensors ID
|
|
||||||
enum{
|
|
||||||
SENSOR_RX_VOLTAGE = 0x00,
|
SENSOR_RX_VOLTAGE = 0x00,
|
||||||
SENSOR_RX_ERR_RATE = 0xfe,
|
SENSOR_RX_ERR_RATE = 0xfe,
|
||||||
SENSOR_RX_RSSI = 0xfc,
|
SENSOR_RX_RSSI = 0xfc,
|
||||||
SENSOR_RX_NOISE = 0xfb,
|
SENSOR_RX_NOISE = 0xfb,
|
||||||
SENSOR_RX_SNR = 0xfa,
|
SENSOR_RX_SNR = 0xfa,
|
||||||
};
|
};
|
||||||
|
|
||||||
static void update_telemetry() {
|
static void update_telemetry() {
|
||||||
// AA | TXID | RXID | sensor id | sensor # | value 16 bit big endian | sensor id ......
|
// AA | TXID | RXID | sensor id | sensor # | value 16 bit big endian | sensor id ......
|
||||||
// max 7 sensors per packet
|
// max 7 sensors per packet
|
||||||
|
|
||||||
@ -129,10 +125,9 @@
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
static void afhds2a_build_bind_packet() {
|
static void afhds2a_build_bind_packet() {
|
||||||
uint8_t ch;
|
uint8_t ch;
|
||||||
memcpy( &packet[1], txid, 4);
|
memcpy( &packet[1], txid, 4);
|
||||||
memset( &packet[5], 0xff, 4);
|
memset( &packet[5], 0xff, 4);
|
||||||
@ -162,11 +157,11 @@
|
|||||||
packet[28]= 0x80;
|
packet[28]= 0x80;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void calc_afhds_channels(uint32_t seed) {
|
static void calc_afhds_channels() {
|
||||||
int idx = 0;
|
int idx = 0;
|
||||||
uint32_t rnd = seed;
|
uint32_t rnd = MProtocol_id;
|
||||||
while (idx < NUMFREQ) {
|
while (idx < NUMFREQ) {
|
||||||
int i;
|
int i;
|
||||||
int count_1_42 = 0, count_43_85 = 0, count_86_128 = 0, count_129_168=0;
|
int count_1_42 = 0, count_43_85 = 0, count_86_128 = 0, count_129_168=0;
|
||||||
@ -174,7 +169,7 @@
|
|||||||
|
|
||||||
uint8_t next_ch = ((rnd >> (idx%32)) % 0xa8) + 1;
|
uint8_t next_ch = ((rnd >> (idx%32)) % 0xa8) + 1;
|
||||||
// Keep the distance 2 between the channels - either odd or even
|
// Keep the distance 2 between the channels - either odd or even
|
||||||
if (((next_ch ^ seed) & 0x01 )== 0)
|
if (((next_ch ^ MProtocol_id) & 0x01 )== 0)
|
||||||
continue;
|
continue;
|
||||||
// Check that it's not duplicate and spread uniformly
|
// Check that it's not duplicate and spread uniformly
|
||||||
for (i = 0; i < idx; i++) {
|
for (i = 0; i < idx; i++) {
|
||||||
@ -199,10 +194,10 @@
|
|||||||
hopping_frequency[idx++] = next_ch;
|
hopping_frequency[idx++] = next_ch;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#define WAIT_WRITE 0x80
|
#define WAIT_WRITE 0x80
|
||||||
static uint16_t afhds2a_cb() {
|
static uint16_t afhds2a_cb() {
|
||||||
switch(phase) {
|
switch(phase) {
|
||||||
case BIND1:
|
case BIND1:
|
||||||
case BIND2:
|
case BIND2:
|
||||||
@ -217,7 +212,7 @@
|
|||||||
for(uint8_t i=0; i<4; i++) {
|
for(uint8_t i=0; i<4; i++) {
|
||||||
rxid[i] = packet[5+i];
|
rxid[i] = packet[5+i];
|
||||||
}
|
}
|
||||||
eeprom_write_block((const void*)rxid,(void*)EEPROMadress,4);
|
eeprom_write_block((const void*)rxid,(EE_ADDR)EEPROMadress,4);
|
||||||
if(packet[9] == 0x01)
|
if(packet[9] == 0x01)
|
||||||
phase = BIND4;
|
phase = BIND4;
|
||||||
}
|
}
|
||||||
@ -279,9 +274,7 @@
|
|||||||
packet_type=PACKET_SETTINGS;
|
packet_type=PACKET_SETTINGS;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
#if defined(TELEMETRY)
|
|
||||||
update_telemetry();
|
update_telemetry();
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -294,16 +287,16 @@
|
|||||||
return 2150;
|
return 2150;
|
||||||
}
|
}
|
||||||
return 3850; // never reached, please the compiler
|
return 3850; // never reached, please the compiler
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t AFHDS2A_setup()
|
static uint16_t AFHDS2A_setup()
|
||||||
{
|
{
|
||||||
A7105_Init(INIT_FLYSKY_AFHDS2A); //flysky_init();
|
A7105_Init(INIT_FLYSKY_AFHDS2A); //flysky_init();
|
||||||
for (u8 i = 0; i < TXID_SIZE; ++i) {
|
for (uint8_t i = 0; i < 4; ++i) {
|
||||||
txid[i] = rx_tx_addr[0];
|
txid[i] = rx_tx_addr[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
calc_afhds_channels(MProtocol_id);
|
calc_afhds_channels();
|
||||||
packet_type = PACKET_STICKS;
|
packet_type = PACKET_STICKS;
|
||||||
packet_count = 0;
|
packet_count = 0;
|
||||||
bind_reply = 0;
|
bind_reply = 0;
|
||||||
@ -313,9 +306,9 @@
|
|||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
phase = DATA1;
|
phase = DATA1;
|
||||||
eeprom_read_block((void*)rxid,(const void*)EEPROMadress,4);
|
eeprom_read_block((void*)rxid,(EE_ADDR)EEPROMadress,4);
|
||||||
}
|
}
|
||||||
channel = 0;
|
channel = 0;
|
||||||
return 50000;
|
return 50000;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -274,8 +274,9 @@ uint16_t ReadFrSkyX()
|
|||||||
return 3000;
|
return 3000;
|
||||||
case FRSKY_DATA4:
|
case FRSKY_DATA4:
|
||||||
len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||||
if (len && (len<MAX_PKT))
|
if (len && (len<=(0x0E + 3))) //Telemetry frame is 17
|
||||||
{
|
{
|
||||||
|
counter=0;
|
||||||
CC2500_ReadData(pkt, len);
|
CC2500_ReadData(pkt, len);
|
||||||
#if defined TELEMETRY
|
#if defined TELEMETRY
|
||||||
frsky_check_telemetry(pkt,len); //check if valid telemetry packets
|
frsky_check_telemetry(pkt,len); //check if valid telemetry packets
|
||||||
@ -292,7 +293,9 @@ uint16_t ReadFrSkyX()
|
|||||||
seq_last_sent = 0;
|
seq_last_sent = 0;
|
||||||
seq_last_rcvd = 8;
|
seq_last_rcvd = 8;
|
||||||
counter=0;
|
counter=0;
|
||||||
|
telemetry_lost=1;
|
||||||
}
|
}
|
||||||
|
CC2500_Strobe(CC2500_SFRX); //flush the RXFIFO
|
||||||
}
|
}
|
||||||
state = FRSKY_DATA1;
|
state = FRSKY_DATA1;
|
||||||
return 300;
|
return 300;
|
||||||
|
@ -58,7 +58,7 @@ const uint8_t h7_mys_byte[] = {
|
|||||||
0x05, 0x15, 0x06, 0x16, 0x07, 0x17, 0x00, 0x10
|
0x05, 0x15, 0x06, 0x16, 0x07, 0x17, 0x00, 0x10
|
||||||
};
|
};
|
||||||
|
|
||||||
static const u8 ls_mys_byte[] = {
|
static const uint8_t ls_mys_byte[] = {
|
||||||
0x05, 0x15, 0x25, 0x06, 0x16, 0x26,
|
0x05, 0x15, 0x25, 0x06, 0x16, 0x26,
|
||||||
0x07, 0x17, 0x27, 0x00, 0x10, 0x20,
|
0x07, 0x17, 0x27, 0x00, 0x10, 0x20,
|
||||||
0x01, 0x11, 0x21, 0x02, 0x12, 0x22,
|
0x01, 0x11, 0x21, 0x02, 0x12, 0x22,
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
#define ARDUINO_AVR_PRO 1
|
#define ARDUINO_AVR_PRO 1
|
||||||
//#define __AVR_ATmega328P__ 1
|
//#define __AVR_ATmega328P__ 1
|
||||||
|
|
||||||
#define XMEGA 1
|
#define ORANGE_TX 1
|
||||||
|
|
||||||
// For BLUE module use:
|
// For BLUE module use:
|
||||||
//#define DSM_BLUE
|
//#define DSM_BLUE
|
@ -21,24 +21,34 @@
|
|||||||
You should have received a copy of the GNU General Public License
|
You should have received a copy of the GNU General Public License
|
||||||
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
#include <avr/eeprom.h>
|
|
||||||
#include <avr/pgmspace.h>
|
#include <avr/pgmspace.h>
|
||||||
//#define DEBUG_TX
|
//#define DEBUG_TX
|
||||||
#include "Pins.h"
|
|
||||||
#include "Multiprotocol.h"
|
#include "Multiprotocol.h"
|
||||||
|
|
||||||
//Multiprotocol module configuration file
|
//Multiprotocol module configuration file
|
||||||
#include "_Config.h"
|
#include "_Config.h"
|
||||||
|
#include "Pins.h"
|
||||||
#include "TX_Def.h"
|
#include "TX_Def.h"
|
||||||
|
#include "Validate.h"
|
||||||
|
|
||||||
#ifdef XMEGA
|
#ifndef STM32_BOARD
|
||||||
#undef ENABLE_PPM // Disable PPM for OrangeTX module
|
#include <avr/eeprom.h>
|
||||||
#undef A7105_INSTALLED // Disable A7105 for OrangeTX module
|
#else
|
||||||
#undef CC2500_INSTALLED // Disable CC2500 for OrangeTX module
|
/*
|
||||||
#undef NRF24L01_INSTALLED // Disable NRF for OrangeTX module
|
#include <arduino.h>
|
||||||
#define TELEMETRY // Enable telemetry
|
#include <libmaple/usart.h>
|
||||||
#define INVERT_TELEMETRY // Enable invert telemetry
|
#include <libmaple/timer.h>
|
||||||
#define DSM_TELEMETRY // Enable DSM telemetry
|
#include <SPI.h>
|
||||||
|
#include <EEPROM.h>
|
||||||
|
HardwareTimer timer(2);
|
||||||
|
void PPM_decode();
|
||||||
|
void ISR_COMPB();
|
||||||
|
extern "C"
|
||||||
|
{
|
||||||
|
void __irq_usart2(void);
|
||||||
|
void __irq_usart3(void);
|
||||||
|
}
|
||||||
|
*/
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//Global constants/variables
|
//Global constants/variables
|
||||||
@ -98,7 +108,7 @@ uint8_t protocol_flags=0,protocol_flags2=0;
|
|||||||
// PPM variable
|
// PPM variable
|
||||||
volatile uint16_t PPM_data[NUM_CHN];
|
volatile uint16_t PPM_data[NUM_CHN];
|
||||||
|
|
||||||
#ifndef XMEGA
|
#ifndef ORANGE_TX
|
||||||
//Random variable
|
//Random variable
|
||||||
volatile uint32_t gWDT_entropy=0;
|
volatile uint32_t gWDT_entropy=0;
|
||||||
#endif
|
#endif
|
||||||
@ -118,35 +128,13 @@ volatile uint8_t rx_buff[RXBUFFER_SIZE];
|
|||||||
volatile uint8_t rx_ok_buff[RXBUFFER_SIZE];
|
volatile uint8_t rx_ok_buff[RXBUFFER_SIZE];
|
||||||
volatile uint8_t discard_frame = 0;
|
volatile uint8_t discard_frame = 0;
|
||||||
|
|
||||||
//Make sure telemetry is selected correctly
|
|
||||||
#ifndef TELEMETRY
|
|
||||||
#undef INVERT_TELEMETRY
|
|
||||||
#undef DSM_TELEMETRY
|
|
||||||
#undef SPORT_TELEMETRY
|
|
||||||
#undef HUB_TELEMETRY
|
|
||||||
#else
|
|
||||||
#if not defined(CYRF6936_INSTALLED) || not defined(DSM_CYRF6936_INO)
|
|
||||||
#undef DSM_TELEMETRY
|
|
||||||
#endif
|
|
||||||
#if (not defined(CC2500_INSTALLED) || not defined(FRSKYD_CC2500_INO)) && (not defined(A7105_INSTALLED) || not defined(HUBSAN_A7105_INO))
|
|
||||||
#undef HUB_TELEMETRY
|
|
||||||
#endif
|
|
||||||
#if not defined(CC2500_INSTALLED) || not defined(FRSKYX_CC2500_INO)
|
|
||||||
#undef SPORT_TELEMETRY
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
#if not defined(DSM_TELEMETRY) && not defined(HUB_TELEMETRY) && not defined(SPORT_TELEMETRY)
|
|
||||||
#undef TELEMETRY
|
|
||||||
#undef INVERT_TELEMETRY
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// Telemetry
|
// Telemetry
|
||||||
#define MAX_PKT 27
|
#define MAX_PKT 27
|
||||||
uint8_t pkt[MAX_PKT];//telemetry receiving packets
|
uint8_t pkt[MAX_PKT];//telemetry receiving packets
|
||||||
#if defined(TELEMETRY)
|
#if defined(TELEMETRY)
|
||||||
#ifdef INVERT_TELEMETRY
|
#ifdef INVERT_TELEMETRY
|
||||||
|
#if not defined(ORANGE_TX) && not defined(STM32_BOARD)
|
||||||
// enable bit bash for serial
|
// enable bit bash for serial
|
||||||
#ifndef XMEGA
|
|
||||||
#define BASH_SERIAL 1
|
#define BASH_SERIAL 1
|
||||||
#endif
|
#endif
|
||||||
#define INVERT_SERIAL 1
|
#define INVERT_SERIAL 1
|
||||||
@ -161,9 +149,9 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets
|
|||||||
#endif // BASH_SERIAL
|
#endif // BASH_SERIAL
|
||||||
uint8_t v_lipo;
|
uint8_t v_lipo;
|
||||||
int16_t RSSI_dBm;
|
int16_t RSSI_dBm;
|
||||||
//const uint8_t RSSI_offset=72;//69 71.72 values db
|
|
||||||
uint8_t telemetry_link=0;
|
uint8_t telemetry_link=0;
|
||||||
uint8_t telemetry_counter=0;
|
uint8_t telemetry_counter=0;
|
||||||
|
uint8_t telemetry_lost;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Callback
|
// Callback
|
||||||
@ -173,8 +161,9 @@ void_function_t remote_callback = 0;
|
|||||||
// Init
|
// Init
|
||||||
void setup()
|
void setup()
|
||||||
{
|
{
|
||||||
#ifdef XMEGA
|
|
||||||
// General pinout
|
// General pinout
|
||||||
|
#ifdef ORANGE_TX
|
||||||
|
//XMEGA
|
||||||
PORTD.OUTSET = 0x17 ;
|
PORTD.OUTSET = 0x17 ;
|
||||||
PORTD.DIRSET = 0xB2 ;
|
PORTD.DIRSET = 0xB2 ;
|
||||||
PORTD.DIRCLR = 0x4D ;
|
PORTD.DIRCLR = 0x4D ;
|
||||||
@ -190,8 +179,40 @@ void setup()
|
|||||||
TCC1.PER = 0xFFFF ;
|
TCC1.PER = 0xFFFF ;
|
||||||
TCNT1 = 0 ;
|
TCNT1 = 0 ;
|
||||||
TCC1.CTRLA = 0x0B ; // Event3 (prescale of 16)
|
TCC1.CTRLA = 0x0B ; // Event3 (prescale of 16)
|
||||||
|
#elif defined STM32_BOARD
|
||||||
|
//STM32
|
||||||
|
pinMode(A7105_CSN_pin,OUTPUT);
|
||||||
|
pinMode(CC25_CSN_pin,OUTPUT);
|
||||||
|
pinMode(NRF_CSN_pin,OUTPUT);
|
||||||
|
pinMode(CYRF_CSN_pin,OUTPUT);
|
||||||
|
pinMode(CYRF_RST_pin,OUTPUT);
|
||||||
|
pinMode(PE1_pin,OUTPUT);
|
||||||
|
pinMode(PE2_pin,OUTPUT);
|
||||||
|
#if defined TELEMETRY
|
||||||
|
pinMode(TX_INV_pin,OUTPUT);
|
||||||
|
pinMode(RX_INV_pin,OUTPUT);
|
||||||
|
#if defined INVERT_SERIAL
|
||||||
|
TX_INV_on;//activated inverter for both serial TX and RX signals
|
||||||
|
RX_INV_on;
|
||||||
#else
|
#else
|
||||||
// General pinout
|
TX_INV_off;
|
||||||
|
RX_INV_off;
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
pinMode(BIND_pin,INPUT_PULLUP);
|
||||||
|
pinMode(PPM_pin,INPUT);
|
||||||
|
pinMode(S1_pin,INPUT_PULLUP);//dial switch
|
||||||
|
pinMode(S2_pin,INPUT_PULLUP);
|
||||||
|
pinMode(S3_pin,INPUT_PULLUP);
|
||||||
|
pinMode(S4_pin,INPUT_PULLUP);
|
||||||
|
//Random pins
|
||||||
|
pinMode(PB0, INPUT_ANALOG); // set up pin for analog input
|
||||||
|
pinMode(PB1, INPUT_ANALOG); // set up pin for analog input
|
||||||
|
|
||||||
|
//select the counter clock.
|
||||||
|
start_timer2();//0.5us
|
||||||
|
#else
|
||||||
|
//ATMEGA328p
|
||||||
// all inputs
|
// all inputs
|
||||||
DDRB=0x00;DDRC=0x00;DDRD=0x00;
|
DDRB=0x00;DDRC=0x00;DDRD=0x00;
|
||||||
// outputs
|
// outputs
|
||||||
@ -243,8 +264,12 @@ void setup()
|
|||||||
NRF_CSN_on;
|
NRF_CSN_on;
|
||||||
#endif
|
#endif
|
||||||
// Set SPI lines
|
// Set SPI lines
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
initSPI2();
|
||||||
|
#else
|
||||||
SDI_on;
|
SDI_on;
|
||||||
SCLK_off;
|
SCLK_off;
|
||||||
|
#endif
|
||||||
|
|
||||||
// Set servos positions
|
// Set servos positions
|
||||||
for(uint8_t i=0;i<NUM_CHN;i++)
|
for(uint8_t i=0;i<NUM_CHN;i++)
|
||||||
@ -265,6 +290,8 @@ void setup()
|
|||||||
// after this mode_select will be one of {0000, 0001, ..., 1111}
|
// after this mode_select will be one of {0000, 0001, ..., 1111}
|
||||||
#ifndef ENABLE_PPM
|
#ifndef ENABLE_PPM
|
||||||
mode_select = MODE_SERIAL ; // force serial mode
|
mode_select = MODE_SERIAL ; // force serial mode
|
||||||
|
#elif defined STM32_BOARD
|
||||||
|
mode_select= 0x0F -(uint8_t)(((GPIOA->regs->IDR)>>4)&0x0F);
|
||||||
#else
|
#else
|
||||||
// mode_select=0x0F - ( ( (PINB>>2)&0x07 ) | ( (PINC<<3)&0x08) );//encoder dip switches 1,2,4,8=>B2,B3,B4,C0
|
// mode_select=0x0F - ( ( (PINB>>2)&0x07 ) | ( (PINC<<3)&0x08) );//encoder dip switches 1,2,4,8=>B2,B3,B4,C0
|
||||||
mode_select =
|
mode_select =
|
||||||
@ -281,10 +308,14 @@ void setup()
|
|||||||
//Init RF modules
|
//Init RF modules
|
||||||
modules_reset();
|
modules_reset();
|
||||||
|
|
||||||
#ifndef XMEGA
|
#ifndef ORANGE_TX
|
||||||
//Init the seed with a random value created from watchdog timer for all protocols requiring random values
|
//Init the seed with a random value created from watchdog timer for all protocols requiring random values
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
randomSeed((uint32_t)analogRead(PB0) << 10 | analogRead(PB1));
|
||||||
|
#else
|
||||||
randomSeed(random_value());
|
randomSeed(random_value());
|
||||||
#endif
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
// Read or create protocol id
|
// Read or create protocol id
|
||||||
MProtocol_id_master=random_id(10,false);
|
MProtocol_id_master=random_id(10,false);
|
||||||
@ -308,6 +339,7 @@ void setup()
|
|||||||
|
|
||||||
protocol_init();
|
protocol_init();
|
||||||
|
|
||||||
|
#ifndef STM32_BOARD
|
||||||
//Configure PPM interrupt
|
//Configure PPM interrupt
|
||||||
#if PPM_pin == 2
|
#if PPM_pin == 2
|
||||||
EICRA |= _BV(ISC01); // The rising edge of INT0 pin D2 generates an interrupt request
|
EICRA |= _BV(ISC01); // The rising edge of INT0 pin D2 generates an interrupt request
|
||||||
@ -318,6 +350,9 @@ void setup()
|
|||||||
#else
|
#else
|
||||||
#error PPM pin can only be 2 or 3
|
#error PPM pin can only be 2 or 3
|
||||||
#endif
|
#endif
|
||||||
|
#else
|
||||||
|
attachInterrupt(PPM_pin,PPM_decode,FALLING);
|
||||||
|
#endif
|
||||||
|
|
||||||
#if defined(TELEMETRY)
|
#if defined(TELEMETRY)
|
||||||
PPM_Telemetry_serial_init(); // Configure serial for telemetry
|
PPM_Telemetry_serial_init(); // Configure serial for telemetry
|
||||||
@ -353,6 +388,7 @@ void loop()
|
|||||||
}
|
}
|
||||||
while(remote_callback==0);
|
while(remote_callback==0);
|
||||||
}
|
}
|
||||||
|
#ifndef STM32_BOARD
|
||||||
if( (TIFR1 & OCF1A_bm) != 0)
|
if( (TIFR1 & OCF1A_bm) != 0)
|
||||||
{
|
{
|
||||||
cli(); // Disable global int due to RW of 16 bits registers
|
cli(); // Disable global int due to RW of 16 bits registers
|
||||||
@ -361,6 +397,16 @@ void loop()
|
|||||||
}
|
}
|
||||||
else
|
else
|
||||||
while((TIFR1 & OCF1A_bm) == 0); // Wait before callback
|
while((TIFR1 & OCF1A_bm) == 0); // Wait before callback
|
||||||
|
#else
|
||||||
|
if((TIMER2_BASE->SR & TIMER_SR_CC1IF)!=0)
|
||||||
|
{
|
||||||
|
cli();
|
||||||
|
OCR1A = TCNT1;
|
||||||
|
sei();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
while((TIMER2_BASE->SR & TIMER_SR_CC1IF )==0); // Wait before callback
|
||||||
|
#endif
|
||||||
do
|
do
|
||||||
{
|
{
|
||||||
TX_MAIN_PAUSE_on;
|
TX_MAIN_PAUSE_on;
|
||||||
@ -373,18 +419,30 @@ void loop()
|
|||||||
next_callback-=2000; // We will wait below for 2ms
|
next_callback-=2000; // We will wait below for 2ms
|
||||||
cli(); // Disable global int due to RW of 16 bits registers
|
cli(); // Disable global int due to RW of 16 bits registers
|
||||||
OCR1A += 2000*2 ; // set compare A for callback
|
OCR1A += 2000*2 ; // set compare A for callback
|
||||||
|
#ifndef STM32_BOARD
|
||||||
TIFR1=OCF1A_bm; // clear compare A=callback flag
|
TIFR1=OCF1A_bm; // clear compare A=callback flag
|
||||||
|
#else
|
||||||
|
TIMER2_BASE->SR &= ~TIMER_SR_CC1IF; //clear compare Flag
|
||||||
|
#endif
|
||||||
sei(); // enable global int
|
sei(); // enable global int
|
||||||
Update_All();
|
Update_All();
|
||||||
if(IS_CHANGE_PROTOCOL_FLAG_on)
|
if(IS_CHANGE_PROTOCOL_FLAG_on)
|
||||||
break; // Protocol has been changed
|
break; // Protocol has been changed
|
||||||
|
#ifndef STM32_BOARD
|
||||||
while((TIFR1 & OCF1A_bm) == 0); // wait 2ms...
|
while((TIFR1 & OCF1A_bm) == 0); // wait 2ms...
|
||||||
|
#else
|
||||||
|
while((TIMER2_BASE->SR & TIMER_SR_CC1IF)==0);//2ms wait
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
// at this point we have a maximum of 4ms in next_callback
|
// at this point we have a maximum of 4ms in next_callback
|
||||||
next_callback *= 2 ;
|
next_callback *= 2 ;
|
||||||
cli(); // Disable global int due to RW of 16 bits registers
|
cli(); // Disable global int due to RW of 16 bits registers
|
||||||
OCR1A+= next_callback ; // set compare A for callback
|
OCR1A+= next_callback ; // set compare A for callback
|
||||||
|
#ifndef STM32_BOARD
|
||||||
TIFR1=OCF1A_bm; // clear compare A=callback flag
|
TIFR1=OCF1A_bm; // clear compare A=callback flag
|
||||||
|
#else
|
||||||
|
TIMER2_BASE->SR &= ~TIMER_SR_CC1IF; //clear compare Flag write zero
|
||||||
|
#endif
|
||||||
diff=OCR1A-TCNT1; // compare timer and comparator
|
diff=OCR1A-TCNT1; // compare timer and comparator
|
||||||
sei(); // enable global int
|
sei(); // enable global int
|
||||||
}
|
}
|
||||||
@ -400,13 +458,6 @@ void Update_All()
|
|||||||
{
|
{
|
||||||
update_serial_data(); // Update protocol and data
|
update_serial_data(); // Update protocol and data
|
||||||
update_aux_flags();
|
update_aux_flags();
|
||||||
if(Servo_data[AUX10]>PPM_SWITCH && !( (cur_protocol[0]&0x1F)==MODE_FRSKYD || (cur_protocol[0]&0x1F)==MODE_DSM || (cur_protocol[0]&0x1F)==MODE_AFHDS2A ) ) { CHANGE_PROTOCOL_FLAG_on; } // Rebind voie
|
|
||||||
if(IS_CHANGE_PROTOCOL_FLAG_on)
|
|
||||||
{ // Protocol needs to be changed
|
|
||||||
LED_off; //led off during protocol init
|
|
||||||
modules_reset(); //reset all modules
|
|
||||||
protocol_init(); //init new protocol
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
#endif //ENABLE_SERIAL
|
#endif //ENABLE_SERIAL
|
||||||
#ifdef ENABLE_PPM
|
#ifdef ENABLE_PPM
|
||||||
@ -440,6 +491,16 @@ static void update_aux_flags(void)
|
|||||||
for(uint8_t i=0;i<8;i++)
|
for(uint8_t i=0;i<8;i++)
|
||||||
if(Servo_data[AUX1+i]>PPM_SWITCH)
|
if(Servo_data[AUX1+i]>PPM_SWITCH)
|
||||||
Servo_AUX|=1<<i;
|
Servo_AUX|=1<<i;
|
||||||
|
if(Servo_data[AUX10]>PPM_SWITCH && !( (cur_protocol[0]&0x1F)==MODE_FRSKYD || (cur_protocol[0]&0x1F)==MODE_DSM || (cur_protocol[0]&0x1F)==MODE_AFHDS2A ) ) { CHANGE_PROTOCOL_FLAG_on; } // Rebind voie
|
||||||
|
if(IS_CHANGE_PROTOCOL_FLAG_on)
|
||||||
|
{ // Protocol needs to be changed
|
||||||
|
LED_off; //led off during protocol init
|
||||||
|
modules_reset(); //reset all modules
|
||||||
|
#ifdef ENABLE_PPM
|
||||||
|
AUTOBIND_FLAG_on;
|
||||||
|
#endif //ENABLE_PPM
|
||||||
|
protocol_init(); //init new protocol
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update led status based on binding and serial
|
// Update led status based on binding and serial
|
||||||
@ -468,8 +529,9 @@ static void update_led_status(void)
|
|||||||
|
|
||||||
inline void tx_pause()
|
inline void tx_pause()
|
||||||
{
|
{
|
||||||
|
#ifndef STM32_BOARD
|
||||||
#ifdef TELEMETRY
|
#ifdef TELEMETRY
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
USARTC0.CTRLA &= ~0x03 ; // Pause telemetry by disabling transmitter interrupt
|
USARTC0.CTRLA &= ~0x03 ; // Pause telemetry by disabling transmitter interrupt
|
||||||
#else
|
#else
|
||||||
#ifndef BASH_SERIAL
|
#ifndef BASH_SERIAL
|
||||||
@ -477,14 +539,16 @@ inline void tx_pause()
|
|||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
inline void tx_resume()
|
inline void tx_resume()
|
||||||
{
|
{
|
||||||
|
#ifndef STM32_BOARD
|
||||||
#ifdef TELEMETRY
|
#ifdef TELEMETRY
|
||||||
if(!IS_TX_PAUSE_on)
|
if(!IS_TX_PAUSE_on)
|
||||||
{
|
{
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
cli() ;
|
cli() ;
|
||||||
USARTC0.CTRLA = (USARTC0.CTRLA & 0xFC) | 0x01 ; // Resume telemetry by enabling transmitter interrupt
|
USARTC0.CTRLA = (USARTC0.CTRLA & 0xFC) | 0x01 ; // Resume telemetry by enabling transmitter interrupt
|
||||||
sei() ;
|
sei() ;
|
||||||
@ -497,8 +561,24 @@ inline void tx_resume()
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
void start_timer2()
|
||||||
|
{
|
||||||
|
// Pause the timer while we're configuring it
|
||||||
|
timer.pause();
|
||||||
|
TIMER2_BASE->PSC = 35; //36-1;for 72 MHZ /0.5sec/(35+1)
|
||||||
|
TIMER2_BASE->ARR = 0xFFFF; //count till max
|
||||||
|
timer.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
|
||||||
|
timer.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
|
||||||
|
// Refresh the timer's count, prescale, and overflow
|
||||||
|
timer.refresh();
|
||||||
|
timer.resume();
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
// Protocol start
|
// Protocol start
|
||||||
static void protocol_init()
|
static void protocol_init()
|
||||||
{
|
{
|
||||||
@ -510,6 +590,7 @@ static void protocol_init()
|
|||||||
tx_pause();
|
tx_pause();
|
||||||
pass=0;
|
pass=0;
|
||||||
telemetry_link=0;
|
telemetry_link=0;
|
||||||
|
telemetry_lost=1;
|
||||||
#ifndef BASH_SERIAL
|
#ifndef BASH_SERIAL
|
||||||
tx_tail=0;
|
tx_tail=0;
|
||||||
tx_head=0;
|
tx_head=0;
|
||||||
@ -624,12 +705,12 @@ static void protocol_init()
|
|||||||
{
|
{
|
||||||
if(IS_BIND_BUTTON_FLAG_on)
|
if(IS_BIND_BUTTON_FLAG_on)
|
||||||
{
|
{
|
||||||
eeprom_write_byte((uint8_t*)(30+mode_select),0x00); // reset to autobind mode for the current model
|
eeprom_write_byte((EE_ADDR)(30+mode_select),0x00); // reset to autobind mode for the current model
|
||||||
option=0;
|
option=0;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
option=eeprom_read_byte((uint8_t*)(30+mode_select)); // load previous mode: autobind or fixed id
|
option=eeprom_read_byte((EE_ADDR)(30+mode_select)); // load previous mode: autobind or fixed id
|
||||||
if(option!=1) option=0; // if not fixed id mode then it should be autobind
|
if(option!=1) option=0; // if not fixed id mode then it should be autobind
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -817,7 +898,11 @@ static void protocol_init()
|
|||||||
cli(); // disable global int
|
cli(); // disable global int
|
||||||
OCR1A=TCNT1+next_callback*2; // set compare A for callback
|
OCR1A=TCNT1+next_callback*2; // set compare A for callback
|
||||||
sei(); // enable global int
|
sei(); // enable global int
|
||||||
|
#ifndef STM32_BOARD
|
||||||
TIFR1 = OCF1A_bm ; // clear compare A flag
|
TIFR1 = OCF1A_bm ; // clear compare A flag
|
||||||
|
#else
|
||||||
|
TIMER2_BASE->SR &= ~TIMER_SR_CC1IF; //clear compare Flag write zero
|
||||||
|
#endif
|
||||||
BIND_BUTTON_FLAG_off; // do not bind/reset id anymore even if protocol change
|
BIND_BUTTON_FLAG_off; // do not bind/reset id anymore even if protocol change
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -873,7 +958,7 @@ void update_serial_data()
|
|||||||
Servo_data[i]=((((*((uint32_t *)p))>>dec)&0x7FF)*5)/8+860; //value range 860<->2140 -125%<->+125%
|
Servo_data[i]=((((*((uint32_t *)p))>>dec)&0x7FF)*5)/8+860; //value range 860<->2140 -125%<->+125%
|
||||||
}
|
}
|
||||||
RX_DONOTUPDTAE_off;
|
RX_DONOTUPDTAE_off;
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
cli();
|
cli();
|
||||||
#else
|
#else
|
||||||
UCSR0B &= ~_BV(RXCIE0); // RX interrupt disable
|
UCSR0B &= ~_BV(RXCIE0); // RX interrupt disable
|
||||||
@ -883,7 +968,7 @@ void update_serial_data()
|
|||||||
RX_FLAG_on; // data to be processed next time...
|
RX_FLAG_on; // data to be processed next time...
|
||||||
RX_MISSED_BUFF_off;
|
RX_MISSED_BUFF_off;
|
||||||
}
|
}
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
sei();
|
sei();
|
||||||
#else
|
#else
|
||||||
UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
|
UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
|
||||||
@ -912,7 +997,7 @@ void modules_reset()
|
|||||||
|
|
||||||
void Mprotocol_serial_init()
|
void Mprotocol_serial_init()
|
||||||
{
|
{
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
PORTC.OUTSET = 0x08 ;
|
PORTC.OUTSET = 0x08 ;
|
||||||
PORTC.DIRSET = 0x08 ;
|
PORTC.DIRSET = 0x08 ;
|
||||||
|
|
||||||
@ -923,10 +1008,17 @@ void Mprotocol_serial_init()
|
|||||||
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
|
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
|
||||||
USARTC0.CTRLC = 0x2B ;
|
USARTC0.CTRLC = 0x2B ;
|
||||||
UDR0 ;
|
UDR0 ;
|
||||||
#ifdef INVERT_TELEMETRY
|
#ifdef INVERT_SERIAL
|
||||||
PORTC.PIN3CTRL |= 0x40 ;
|
PORTC.PIN3CTRL |= 0x40 ;
|
||||||
#endif
|
#endif
|
||||||
|
#elif defined STM32_BOARD
|
||||||
|
Serial1.begin(100000,SERIAL_8E2);//USART2
|
||||||
|
Serial2.begin(100000,SERIAL_8E2);//USART3
|
||||||
|
USART2_BASE->CR1 |= USART_CR1_PCE_BIT;
|
||||||
|
USART3_BASE->CR1 &= ~ USART_CR1_RE;//disable
|
||||||
|
USART2_BASE->CR1 &= ~ USART_CR1_TE;//disable transmit
|
||||||
#else
|
#else
|
||||||
|
//ATMEGA328p
|
||||||
#include <util/setbaud.h>
|
#include <util/setbaud.h>
|
||||||
UBRR0H = UBRRH_VALUE;
|
UBRR0H = UBRRH_VALUE;
|
||||||
UBRR0L = UBRRL_VALUE;
|
UBRR0L = UBRRL_VALUE;
|
||||||
@ -942,7 +1034,7 @@ void Mprotocol_serial_init()
|
|||||||
initTXSerial( SPEED_100K ) ;
|
initTXSerial( SPEED_100K ) ;
|
||||||
#endif //TELEMETRY
|
#endif //TELEMETRY
|
||||||
#endif //DEBUG_TX
|
#endif //DEBUG_TX
|
||||||
#endif //XMEGA
|
#endif //ORANGE_TX
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(TELEMETRY)
|
#if defined(TELEMETRY)
|
||||||
@ -967,7 +1059,7 @@ static void set_rx_tx_addr(uint32_t id)
|
|||||||
rx_tx_addr[4] = (rx_tx_addr[2]&0xF0)|(rx_tx_addr[3]&0x0F);
|
rx_tx_addr[4] = (rx_tx_addr[2]&0xF0)|(rx_tx_addr[3]&0x0F);
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifndef XMEGA
|
#if not defined (ORANGE_TX) && not defined (STM32_BOARD)
|
||||||
static void random_init(void)
|
static void random_init(void)
|
||||||
{
|
{
|
||||||
cli(); // Temporarily turn off interrupts, until WDT configured
|
cli(); // Temporarily turn off interrupts, until WDT configured
|
||||||
@ -986,25 +1078,35 @@ static uint32_t random_value(void)
|
|||||||
|
|
||||||
static uint32_t random_id(uint16_t adress, uint8_t create_new)
|
static uint32_t random_id(uint16_t adress, uint8_t create_new)
|
||||||
{
|
{
|
||||||
#define nb_txid 5
|
uint32_t id=0;
|
||||||
uint32_t id;
|
|
||||||
uint8_t txid[nb_txid];
|
|
||||||
|
|
||||||
if (eeprom_read_byte((uint8_t*)(adress+10))==0xf0 && !create_new)
|
if(eeprom_read_byte((EE_ADDR)(adress+10))==0xf0 && !create_new)
|
||||||
{ // TXID exists in EEPROM
|
{ // TXID exists in EEPROM
|
||||||
eeprom_read_block((void*)txid,(const void*)adress,nb_txid);
|
// eeprom_read_block((void*)txid,(const void*)adress,nb_txid);
|
||||||
id=(txid[0] | ((uint32_t)txid[1]<<8) | ((uint32_t)txid[2]<<16) | ((uint32_t)txid[3]<<24));
|
// id=(txid[0] | ((uint32_t)txid[1]<<8) | ((uint32_t)txid[2]<<16) | ((uint32_t)txid[3]<<24));
|
||||||
|
for(uint8_t i=4;i>0;i--)
|
||||||
|
{
|
||||||
|
id<<=8;
|
||||||
|
id|=eeprom_read_byte((EE_ADDR)adress+i-1);
|
||||||
|
}
|
||||||
if(id!=0x2AD141A7) //ID with seed=0
|
if(id!=0x2AD141A7) //ID with seed=0
|
||||||
return id;
|
return id;
|
||||||
}
|
}
|
||||||
// Generate a random ID
|
// Generate a random ID
|
||||||
id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16);
|
id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16);
|
||||||
txid[0]= (id &0xFF);
|
/* txid[0]= (id &0xFF);
|
||||||
txid[1] = ((id >> 8) & 0xFF);
|
txid[1] = ((id >> 8) & 0xFF);
|
||||||
txid[2] = ((id >> 16) & 0xFF);
|
txid[2] = ((id >> 16) & 0xFF);
|
||||||
txid[3] = ((id >> 24) & 0xFF);
|
txid[3] = ((id >> 24) & 0xFF);
|
||||||
eeprom_write_block((const void*)txid,(void*)adress,nb_txid);
|
eeprom_write_block((const void*)txid,(void*)adress,nb_txid);
|
||||||
eeprom_write_byte((uint8_t*)(adress+10),0xf0);//write bind flag in eeprom.
|
eeprom_write_byte((uint8_t*)(adress+10),0xf0);//write bind flag in eeprom.
|
||||||
|
*/
|
||||||
|
for(uint8_t i=0;i<4;i++)
|
||||||
|
{
|
||||||
|
eeprom_write_byte((EE_ADDR)adress+i,id);
|
||||||
|
id>>=8;
|
||||||
|
}
|
||||||
|
eeprom_write_byte((EE_ADDR)(adress+10),0xf0);//write bind flag in eeprom.
|
||||||
return id;
|
return id;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1016,12 +1118,14 @@ static uint32_t random_id(uint16_t adress, uint8_t create_new)
|
|||||||
|
|
||||||
//PPM
|
//PPM
|
||||||
#ifdef ENABLE_PPM
|
#ifdef ENABLE_PPM
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
#if PPM_pin == 2
|
#if PPM_pin == 2
|
||||||
ISR(PORTD_INT0_vect)
|
ISR(PORTD_INT0_vect)
|
||||||
#else
|
#else
|
||||||
ISR(PORTD_INT1_vect)
|
ISR(PORTD_INT1_vect)
|
||||||
#endif
|
#endif
|
||||||
|
#elif defined STM32_BOARD
|
||||||
|
void PPM_decode()
|
||||||
#else
|
#else
|
||||||
#if PPM_pin == 2
|
#if PPM_pin == 2
|
||||||
ISR(INT0_vect, ISR_NOBLOCK)
|
ISR(INT0_vect, ISR_NOBLOCK)
|
||||||
@ -1056,15 +1160,19 @@ static uint32_t random_id(uint16_t adress, uint8_t create_new)
|
|||||||
|
|
||||||
//Serial RX
|
//Serial RX
|
||||||
#ifdef ENABLE_SERIAL
|
#ifdef ENABLE_SERIAL
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
ISR(USARTC0_RXC_vect)
|
ISR(USARTC0_RXC_vect)
|
||||||
|
#elif defined STM32_BOARD
|
||||||
|
void __irq_usart2()
|
||||||
#else
|
#else
|
||||||
ISR(USART_RX_vect)
|
ISR(USART_RX_vect)
|
||||||
#endif
|
#endif
|
||||||
{ // RX interrupt
|
{ // RX interrupt
|
||||||
static uint8_t idx=0;
|
static uint8_t idx=0;
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
if((USARTC0.STATUS & 0x1C)==0) // Check frame error, data overrun and parity error
|
if((USARTC0.STATUS & 0x1C)==0) // Check frame error, data overrun and parity error
|
||||||
|
#elif defined STM32_BOARD
|
||||||
|
if((USART2_BASE->SR & USART_SR_RXNE) && (USART2_BASE->SR &0x0F)==0)
|
||||||
#else
|
#else
|
||||||
UCSR0B &= ~_BV(RXCIE0) ; // RX interrupt disable
|
UCSR0B &= ~_BV(RXCIE0) ; // RX interrupt disable
|
||||||
sei() ;
|
sei() ;
|
||||||
@ -1078,11 +1186,18 @@ static uint32_t random_id(uint16_t adress, uint8_t create_new)
|
|||||||
rx_buff[0]=UDR0;
|
rx_buff[0]=UDR0;
|
||||||
if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok
|
if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok
|
||||||
{
|
{
|
||||||
|
#if defined STM32_BOARD
|
||||||
|
uint16_t OCR1B;
|
||||||
|
OCR1B =TCNT1+(6500L);
|
||||||
|
timer.setCompare(TIMER_CH2,OCR1B);
|
||||||
|
timer.attachCompare2Interrupt(ISR_COMPB);
|
||||||
|
#else
|
||||||
TX_RX_PAUSE_on;
|
TX_RX_PAUSE_on;
|
||||||
tx_pause();
|
tx_pause();
|
||||||
OCR1B = TCNT1+(6500L) ; // Full message should be received within timer of 3250us
|
OCR1B = TCNT1+(6500L) ; // Full message should be received within timer of 3250us
|
||||||
TIFR1 = OCF1B_bm ; // clear OCR1B match flag
|
TIFR1 = OCF1B_bm ; // clear OCR1B match flag
|
||||||
SET_TIMSK1_OCIE1B ; // enable interrupt on compare B match
|
SET_TIMSK1_OCIE1B ; // enable interrupt on compare B match
|
||||||
|
#endif
|
||||||
idx++;
|
idx++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -1109,34 +1224,44 @@ static uint32_t random_id(uint16_t adress, uint8_t create_new)
|
|||||||
}
|
}
|
||||||
if(discard_frame==1)
|
if(discard_frame==1)
|
||||||
{
|
{
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
detachInterrupt(2); // Disable interrupt on ch2
|
||||||
|
#else
|
||||||
CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
|
CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
|
||||||
TX_RX_PAUSE_off;
|
TX_RX_PAUSE_off;
|
||||||
tx_resume();
|
tx_resume();
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
#ifndef XMEGA
|
#if not defined (ORANGE_TX) && not defined (STM32_BOARD)
|
||||||
cli() ;
|
cli() ;
|
||||||
UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
|
UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
//Serial timer
|
//Serial timer
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
ISR(TCC1_CCB_vect)
|
ISR(TCC1_CCB_vect)
|
||||||
|
#elif defined STM32_BOARD
|
||||||
|
void ISR_COMPB()
|
||||||
#else
|
#else
|
||||||
ISR(TIMER1_COMPB_vect, ISR_NOBLOCK )
|
ISR(TIMER1_COMPB_vect, ISR_NOBLOCK )
|
||||||
#endif
|
#endif
|
||||||
{ // Timer1 compare B interrupt
|
{ // Timer1 compare B interrupt
|
||||||
discard_frame=1;
|
discard_frame=1;
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
detachInterrupt(2); // Disable interrupt on ch2
|
||||||
|
#else
|
||||||
CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
|
CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
|
||||||
tx_resume();
|
tx_resume();
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
#endif //ENABLE_SERIAL
|
#endif //ENABLE_SERIAL
|
||||||
|
|
||||||
#ifndef XMEGA
|
#if not defined (ORANGE_TX) && not defined (STM32_BOARD)
|
||||||
// Random interrupt service routine called every time the WDT interrupt is triggered.
|
// Random interrupt service routine called every time the WDT interrupt is triggered.
|
||||||
// It is only enabled at startup to generate a seed.
|
// It is only enabled at startup to generate a seed.
|
||||||
ISR(WDT_vect)
|
ISR(WDT_vect)
|
||||||
{
|
{
|
||||||
static uint8_t gWDT_buffer_position=0;
|
static uint8_t gWDT_buffer_position=0;
|
||||||
#define gWDT_buffer_SIZE 32
|
#define gWDT_buffer_SIZE 32
|
||||||
static uint8_t gWDT_buffer[gWDT_buffer_SIZE];
|
static uint8_t gWDT_buffer[gWDT_buffer_SIZE];
|
||||||
@ -1156,5 +1281,5 @@ ISR(WDT_vect)
|
|||||||
gWDT_entropy += (gWDT_entropy << 15);
|
gWDT_entropy += (gWDT_entropy << 15);
|
||||||
WDTCSR = 0; // Disable Watchdog interrupt
|
WDTCSR = 0; // Disable Watchdog interrupt
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
@ -14,9 +14,7 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
//---------------------------
|
#ifdef NRF24L01_INSTALLED
|
||||||
// AVR nrf chip bitbang SPI functions
|
|
||||||
//---------------------------
|
|
||||||
#include "iface_nrf24l01.h"
|
#include "iface_nrf24l01.h"
|
||||||
|
|
||||||
|
|
||||||
@ -189,7 +187,8 @@ void NRF24L01_SetTxRxMode(enum TXRX_State mode)
|
|||||||
NRF_CE_on;
|
NRF_CE_on;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
if (mode == RX_EN) {
|
if (mode == RX_EN)
|
||||||
|
{
|
||||||
NRF_CE_off;
|
NRF_CE_off;
|
||||||
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // reset the flag(s)
|
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // reset the flag(s)
|
||||||
NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0F); // switch to RX mode
|
NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0F); // switch to RX mode
|
||||||
@ -335,7 +334,7 @@ void XN297_Configure(uint8_t flags)
|
|||||||
NRF24L01_WriteReg(NRF24L01_00_CONFIG, flags & 0xFF);
|
NRF24L01_WriteReg(NRF24L01_00_CONFIG, flags & 0xFF);
|
||||||
}
|
}
|
||||||
|
|
||||||
void XN297_SetScrambledMode(const u8 mode)
|
void XN297_SetScrambledMode(const uint8_t mode)
|
||||||
{
|
{
|
||||||
xn297_scramble_enabled = mode;
|
xn297_scramble_enabled = mode;
|
||||||
}
|
}
|
||||||
@ -591,3 +590,4 @@ void LT8900_WritePayload(uint8_t* msg, uint8_t len)
|
|||||||
NRF24L01_WritePayload(LT8900_buffer+LT8900_buffer_start,pos_final+pos-LT8900_buffer_start);
|
NRF24L01_WritePayload(LT8900_buffer+LT8900_buffer_start,pos_final+pos-LT8900_buffer_start);
|
||||||
}
|
}
|
||||||
// End of LT8900 emulation
|
// End of LT8900 emulation
|
||||||
|
#endif
|
||||||
|
@ -4,7 +4,8 @@
|
|||||||
You should have received a copy of the GNU General Public License along with Deviation. If not, see <http://www.gnu.org/licenses/>.
|
You should have received a copy of the GNU General Public License along with Deviation. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* This protocol is for the HM Hobby HM830 RC Paper Airplane
|
/*
|
||||||
|
This protocol is for the HM Hobby HM830 RC Paper Airplane
|
||||||
Protocol spec:
|
Protocol spec:
|
||||||
Channel data:
|
Channel data:
|
||||||
AA BB CC DD EE FF GG
|
AA BB CC DD EE FF GG
|
||||||
|
@ -15,113 +15,113 @@
|
|||||||
//*******************
|
//*******************
|
||||||
//*** Pinouts ***
|
//*** Pinouts ***
|
||||||
//*******************
|
//*******************
|
||||||
|
#ifndef STM32_BOARD
|
||||||
// TX
|
// TX
|
||||||
#define SERIAL_TX_pin 1 //PD1
|
#define SERIAL_TX_pin 1 //PD1
|
||||||
#define SERIAL_TX_port PORTD
|
#define SERIAL_TX_port PORTD
|
||||||
#define SERIAL_TX_ddr DDRD
|
#define SERIAL_TX_ddr DDRD
|
||||||
#define SERIAL_TX_output SERIAL_TX_ddr |= _BV(SERIAL_TX_pin)
|
#define SERIAL_TX_output SERIAL_TX_ddr |= _BV(SERIAL_TX_pin)
|
||||||
#define SERIAL_TX_on SERIAL_TX_port |= _BV(SERIAL_TX_pin)
|
#define SERIAL_TX_on SERIAL_TX_port |= _BV(SERIAL_TX_pin)
|
||||||
#define SERIAL_TX_off SERIAL_TX_port &= ~_BV(SERIAL_TX_pin)
|
#define SERIAL_TX_off SERIAL_TX_port &= ~_BV(SERIAL_TX_pin)
|
||||||
#ifdef DEBUG_TX
|
#ifdef DEBUG_TX
|
||||||
#define DEBUG_TX_on SERIAL_TX_ON
|
#define DEBUG_TX_on SERIAL_TX_on
|
||||||
#define DEBUG_TX_off SERIAL_TX_OFF
|
#define DEBUG_TX_off SERIAL_TX_off
|
||||||
#define DEBUG_TX_toggle SERIAL_TX_port ^= _BV(SERIAL_TX_pin)
|
#define DEBUG_TX_toggle SERIAL_TX_port ^= _BV(SERIAL_TX_pin)
|
||||||
#else
|
#else
|
||||||
#define DEBUG_TX_on
|
#define DEBUG_TX_on
|
||||||
#define DEBUG_TX_off
|
#define DEBUG_TX_off
|
||||||
#define DEBUG_TX_toggle
|
#define DEBUG_TX_toggle
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Dial
|
// Dial
|
||||||
#define MODE_DIAL1_pin 2
|
#define MODE_DIAL1_pin 2
|
||||||
#define MODE_DIAL1_port PORTB
|
#define MODE_DIAL1_port PORTB
|
||||||
#define MODE_DIAL1_ipr PINB
|
#define MODE_DIAL1_ipr PINB
|
||||||
#define MODE_DIAL2_pin 3
|
#define MODE_DIAL2_pin 3
|
||||||
#define MODE_DIAL2_port PORTB
|
#define MODE_DIAL2_port PORTB
|
||||||
#define MODE_DIAL2_ipr PINB
|
#define MODE_DIAL2_ipr PINB
|
||||||
#define MODE_DIAL3_pin 4
|
#define MODE_DIAL3_pin 4
|
||||||
#define MODE_DIAL3_port PORTB
|
#define MODE_DIAL3_port PORTB
|
||||||
#define MODE_DIAL3_ipr PINB
|
#define MODE_DIAL3_ipr PINB
|
||||||
#define MODE_DIAL4_pin 0
|
#define MODE_DIAL4_pin 0
|
||||||
#define MODE_DIAL4_port PORTC
|
#define MODE_DIAL4_port PORTC
|
||||||
#define MODE_DIAL4_ipr PINC
|
#define MODE_DIAL4_ipr PINC
|
||||||
|
|
||||||
// PPM
|
// PPM
|
||||||
#define PPM_pin 3 //D3 = PD3
|
#define PPM_pin 3 //D3 = PD3
|
||||||
#define PPM_port PORTD
|
#define PPM_port PORTD
|
||||||
|
|
||||||
// SDIO
|
// SDIO
|
||||||
#define SDI_pin 5 //D5 = PD5
|
#define SDI_pin 5 //D5 = PD5
|
||||||
#define SDI_port PORTD
|
#define SDI_port PORTD
|
||||||
#define SDI_ipr PIND
|
#define SDI_ipr PIND
|
||||||
#define SDI_ddr DDRD
|
#define SDI_ddr DDRD
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
#define SDI_on SDI_port.OUTSET = _BV(SDI_pin)
|
#define SDI_on SDI_port.OUTSET = _BV(SDI_pin)
|
||||||
#define SDI_off SDI_port.OUTCLR = _BV(SDI_pin)
|
#define SDI_off SDI_port.OUTCLR = _BV(SDI_pin)
|
||||||
#else
|
#else
|
||||||
#define SDI_on SDI_port |= _BV(SDI_pin)
|
#define SDI_on SDI_port |= _BV(SDI_pin)
|
||||||
#define SDI_off SDI_port &= ~_BV(SDI_pin)
|
#define SDI_off SDI_port &= ~_BV(SDI_pin)
|
||||||
#define SDI_1 (SDI_ipr & _BV(SDI_pin))
|
#define SDI_1 (SDI_ipr & _BV(SDI_pin))
|
||||||
#define SDI_0 (SDI_ipr & _BV(SDI_pin)) == 0x00
|
#define SDI_0 (SDI_ipr & _BV(SDI_pin)) == 0x00
|
||||||
#endif
|
#endif
|
||||||
#define SDI_input SDI_ddr &= ~_BV(SDI_pin)
|
#define SDI_input SDI_ddr &= ~_BV(SDI_pin)
|
||||||
#define SDI_output SDI_ddr |= _BV(SDI_pin)
|
#define SDI_output SDI_ddr |= _BV(SDI_pin)
|
||||||
|
|
||||||
//SDO
|
//SDO
|
||||||
#define SDO_pin 6 //D6 = PD6
|
#define SDO_pin 6 //D6 = PD6
|
||||||
#define SDO_port PORTD
|
#define SDO_port PORTD
|
||||||
#define SDO_ipr PIND
|
#define SDO_ipr PIND
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
#define SDO_1 (SDO_port.IN & _BV(SDO_pin))
|
#define SDO_1 (SDO_port.IN & _BV(SDO_pin))
|
||||||
#define SDO_0 (SDO_port.IN & _BV(SDO_pin)) == 0x00
|
#define SDO_0 (SDO_port.IN & _BV(SDO_pin)) == 0x00
|
||||||
#else
|
#else
|
||||||
#define SDO_1 (SDO_ipr & _BV(SDO_pin))
|
#define SDO_1 (SDO_ipr & _BV(SDO_pin))
|
||||||
#define SDO_0 (SDO_ipr & _BV(SDO_pin)) == 0x00
|
#define SDO_0 (SDO_ipr & _BV(SDO_pin)) == 0x00
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// SCLK
|
// SCLK
|
||||||
#define SCLK_port PORTD
|
#define SCLK_port PORTD
|
||||||
#define SCLK_ddr DDRD
|
#define SCLK_ddr DDRD
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
#define SCLK_pin 7 //PD7
|
#define SCLK_pin 7 //PD7
|
||||||
#define SCLK_on SCLK_port.OUTSET = _BV(SCLK_pin)
|
#define SCLK_on SCLK_port.OUTSET = _BV(SCLK_pin)
|
||||||
#define SCLK_off SCLK_port.OUTCLR = _BV(SCLK_pin)
|
#define SCLK_off SCLK_port.OUTCLR = _BV(SCLK_pin)
|
||||||
#else
|
#else
|
||||||
#define SCLK_pin 4 //D4 = PD4
|
#define SCLK_pin 4 //D4 = PD4
|
||||||
#define SCLK_output SCLK_ddr |= _BV(SCLK_pin)
|
#define SCLK_output SCLK_ddr |= _BV(SCLK_pin)
|
||||||
#define SCLK_on SCLK_port |= _BV(SCLK_pin)
|
#define SCLK_on SCLK_port |= _BV(SCLK_pin)
|
||||||
#define SCLK_off SCLK_port &= ~_BV(SCLK_pin)
|
#define SCLK_off SCLK_port &= ~_BV(SCLK_pin)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// A7105
|
// A7105
|
||||||
#define A7105_CSN_pin 2 //D2 = PD2
|
#define A7105_CSN_pin 2 //D2 = PD2
|
||||||
#define A7105_CSN_port PORTD
|
#define A7105_CSN_port PORTD
|
||||||
#define A7105_CSN_ddr DDRD
|
#define A7105_CSN_ddr DDRD
|
||||||
#define A7105_CSN_output A7105_CSN_ddr |= _BV(A7105_CSN_pin)
|
#define A7105_CSN_output A7105_CSN_ddr |= _BV(A7105_CSN_pin)
|
||||||
#define A7105_CSN_on A7105_CSN_port |= _BV(A7105_CSN_pin)
|
#define A7105_CSN_on A7105_CSN_port |= _BV(A7105_CSN_pin)
|
||||||
#define A7105_CSN_off A7105_CSN_port &= ~_BV(A7105_CSN_pin)
|
#define A7105_CSN_off A7105_CSN_port &= ~_BV(A7105_CSN_pin)
|
||||||
|
|
||||||
// CC2500
|
// CC2500
|
||||||
#define CC25_CSN_pin 7 //D7 = PD7
|
#define CC25_CSN_pin 7 //D7 = PD7
|
||||||
#define CC25_CSN_port PORTD
|
#define CC25_CSN_port PORTD
|
||||||
#define CC25_CSN_ddr DDRD
|
#define CC25_CSN_ddr DDRD
|
||||||
#define CC25_CSN_output CC25_CSN_ddr |= _BV(CC25_CSN_pin)
|
#define CC25_CSN_output CC25_CSN_ddr |= _BV(CC25_CSN_pin)
|
||||||
#define CC25_CSN_on CC25_CSN_port |= _BV(CC25_CSN_pin)
|
#define CC25_CSN_on CC25_CSN_port |= _BV(CC25_CSN_pin)
|
||||||
#define CC25_CSN_off CC25_CSN_port &= ~_BV(CC25_CSN_pin)
|
#define CC25_CSN_off CC25_CSN_port &= ~_BV(CC25_CSN_pin)
|
||||||
|
|
||||||
// NRF24L01
|
// NRF24L01
|
||||||
#define NRF_CSN_pin 0 //D8 = PB0
|
#define NRF_CSN_pin 0 //D8 = PB0
|
||||||
#define NRF_CSN_port PORTB
|
#define NRF_CSN_port PORTB
|
||||||
#define NRF_CSN_ddr DDRB
|
#define NRF_CSN_ddr DDRB
|
||||||
#define NRF_CSN_output NRF_CSN_ddr |= _BV(NRF_CSN_pin)
|
#define NRF_CSN_output NRF_CSN_ddr |= _BV(NRF_CSN_pin)
|
||||||
#define NRF_CSN_on NRF_CSN_port |= _BV(NRF_CSN_pin)
|
#define NRF_CSN_on NRF_CSN_port |= _BV(NRF_CSN_pin)
|
||||||
#define NRF_CSN_off NRF_CSN_port &= ~_BV(NRF_CSN_pin)
|
#define NRF_CSN_off NRF_CSN_port &= ~_BV(NRF_CSN_pin)
|
||||||
#define NRF_CE_on
|
#define NRF_CE_on
|
||||||
#define NRF_CE_off
|
#define NRF_CE_off
|
||||||
|
|
||||||
// CYRF6936
|
// CYRF6936
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
#define CYRF_CSN_pin 4 //PD4
|
#define CYRF_CSN_pin 4 //PD4
|
||||||
#define CYRF_CSN_port PORTD
|
#define CYRF_CSN_port PORTD
|
||||||
#define CYRF_CSN_ddr DDRD
|
#define CYRF_CSN_ddr DDRD
|
||||||
@ -133,7 +133,7 @@
|
|||||||
#define CYRF_RST_ddr DDRE
|
#define CYRF_RST_ddr DDRE
|
||||||
#define CYRF_RST_HI CYRF_RST_port.OUTSET = _BV(CYRF_RST_pin)
|
#define CYRF_RST_HI CYRF_RST_port.OUTSET = _BV(CYRF_RST_pin)
|
||||||
#define CYRF_RST_LO CYRF_RST_port.OUTCLR = _BV(CYRF_RST_pin)
|
#define CYRF_RST_LO CYRF_RST_port.OUTCLR = _BV(CYRF_RST_pin)
|
||||||
#else
|
#else
|
||||||
#define CYRF_CSN_pin 1 //D9 = PB1
|
#define CYRF_CSN_pin 1 //D9 = PB1
|
||||||
#define CYRF_CSN_port PORTB
|
#define CYRF_CSN_port PORTB
|
||||||
#define CYRF_CSN_ddr DDRB
|
#define CYRF_CSN_ddr DDRB
|
||||||
@ -147,15 +147,15 @@
|
|||||||
#define CYRF_RST_output CYRF_RST_ddr |= _BV(CYRF_RST_pin)
|
#define CYRF_RST_output CYRF_RST_ddr |= _BV(CYRF_RST_pin)
|
||||||
#define CYRF_RST_HI CYRF_RST_port |= _BV(CYRF_RST_pin)
|
#define CYRF_RST_HI CYRF_RST_port |= _BV(CYRF_RST_pin)
|
||||||
#define CYRF_RST_LO CYRF_RST_port &= ~_BV(CYRF_RST_pin)
|
#define CYRF_RST_LO CYRF_RST_port &= ~_BV(CYRF_RST_pin)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//RF Switch
|
//RF Switch
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
#define PE1_on
|
#define PE1_on
|
||||||
#define PE1_off
|
#define PE1_off
|
||||||
#define PE2_on
|
#define PE2_on
|
||||||
#define PE2_off
|
#define PE2_off
|
||||||
#else
|
#else
|
||||||
#define PE1_pin 1 //A1 = PC1
|
#define PE1_pin 1 //A1 = PC1
|
||||||
#define PE1_port PORTC
|
#define PE1_port PORTC
|
||||||
#define PE1_ddr DDRC
|
#define PE1_ddr DDRC
|
||||||
@ -169,10 +169,10 @@
|
|||||||
#define PE2_output PE2_ddr |= _BV(PE2_pin)
|
#define PE2_output PE2_ddr |= _BV(PE2_pin)
|
||||||
#define PE2_on PE2_port |= _BV(PE2_pin)
|
#define PE2_on PE2_port |= _BV(PE2_pin)
|
||||||
#define PE2_off PE2_port &= ~_BV(PE2_pin)
|
#define PE2_off PE2_port &= ~_BV(PE2_pin)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// LED
|
// LED
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
#define LED_pin 1 //PD1
|
#define LED_pin 1 //PD1
|
||||||
#define LED_port PORTD
|
#define LED_port PORTD
|
||||||
#define LED_ddr DDRD
|
#define LED_ddr DDRD
|
||||||
@ -181,7 +181,7 @@
|
|||||||
#define LED_toggle LED_port.OUTTGL = _BV(LED_pin)
|
#define LED_toggle LED_port.OUTTGL = _BV(LED_pin)
|
||||||
#define LED_output LED_port.DIRSET = _BV(LED_pin)
|
#define LED_output LED_port.DIRSET = _BV(LED_pin)
|
||||||
#define IS_LED_on (LED_port.OUT & _BV(LED_pin))
|
#define IS_LED_on (LED_port.OUT & _BV(LED_pin))
|
||||||
#else
|
#else
|
||||||
#define LED_pin 5 //D13 = PB5
|
#define LED_pin 5 //D13 = PB5
|
||||||
#define LED_port PORTB
|
#define LED_port PORTB
|
||||||
#define LED_ddr DDRB
|
#define LED_ddr DDRB
|
||||||
@ -190,14 +190,14 @@
|
|||||||
#define LED_toggle LED_port ^= _BV(LED_pin)
|
#define LED_toggle LED_port ^= _BV(LED_pin)
|
||||||
#define LED_output LED_ddr |= _BV(LED_pin)
|
#define LED_output LED_ddr |= _BV(LED_pin)
|
||||||
#define IS_LED_on (LED_port & _BV(LED_pin))
|
#define IS_LED_on (LED_port & _BV(LED_pin))
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//BIND
|
//BIND
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
#define BIND_pin 2 //PD2
|
#define BIND_pin 2 //PD2
|
||||||
#define BIND_port PORTD
|
#define BIND_port PORTD
|
||||||
#define IS_BIND_BUTTON_on ( (BIND_port.IN & _BV(BIND_pin)) == 0x00 )
|
#define IS_BIND_BUTTON_on ( (BIND_port.IN & _BV(BIND_pin)) == 0x00 )
|
||||||
#else
|
#else
|
||||||
#define BIND_pin 5 //D13 = PB5
|
#define BIND_pin 5 //D13 = PB5
|
||||||
#define BIND_port PORTB
|
#define BIND_port PORTB
|
||||||
#define BIND_ipr PINB
|
#define BIND_ipr PINB
|
||||||
@ -206,4 +206,147 @@
|
|||||||
#define BIND_SET_OUTPUT BIND_ddr |= _BV(BIND_pin)
|
#define BIND_SET_OUTPUT BIND_ddr |= _BV(BIND_pin)
|
||||||
#define BIND_SET_PULLUP BIND_port |= _BV(BIND_pin)
|
#define BIND_SET_PULLUP BIND_port |= _BV(BIND_pin)
|
||||||
#define IS_BIND_BUTTON_on ( (BIND_ipr & _BV(BIND_pin)) == 0x00 )
|
#define IS_BIND_BUTTON_on ( (BIND_ipr & _BV(BIND_pin)) == 0x00 )
|
||||||
|
#endif
|
||||||
|
#else //STM32_BOARD
|
||||||
|
#define BIND_pin PA0
|
||||||
|
#define LED_pin PA1
|
||||||
|
//
|
||||||
|
#define PPM_pin PA8 //PPM 5V tolerant
|
||||||
|
//
|
||||||
|
#define S1_pin PA4 //Dial switch pins
|
||||||
|
#define S2_pin PA5
|
||||||
|
#define S3_pin PA6
|
||||||
|
#define S4_pin PA7
|
||||||
|
//
|
||||||
|
#define PE1_pin PB4 //PE1
|
||||||
|
#define PE2_pin PB5 //PE2
|
||||||
|
//CS pins
|
||||||
|
#define CC25_CSN_pin PB6 //CC2500
|
||||||
|
#define NRF_CSN_pin PB7 //NRF24L01
|
||||||
|
#define CYRF_RST_pin PB8 //CYRF RESET
|
||||||
|
#define A7105_CSN_pin PB9 //A7105
|
||||||
|
#define CYRF_CSN_pin PB12 //CYRF CSN
|
||||||
|
//SPI pins
|
||||||
|
#define SCK_pin PB13 //SCK
|
||||||
|
#define SDO_pin PB14 //MISO
|
||||||
|
#define SDI_pin PB15 //MOSI
|
||||||
|
//
|
||||||
|
#define TX_INV_pin PB3
|
||||||
|
#define RX_INV_pin PB1
|
||||||
|
//
|
||||||
|
#define PE1_on digitalWrite(PE1_pin,HIGH)
|
||||||
|
#define PE1_off digitalWrite(PE1_pin,LOW)
|
||||||
|
//
|
||||||
|
#define PE2_on digitalWrite(PE2_pin,HIGH)
|
||||||
|
#define PE2_off digitalWrite(PE2_pin,LOW)
|
||||||
|
|
||||||
|
#define A7105_CSN_on digitalWrite(A7105_CSN_pin,HIGH)
|
||||||
|
#define A7105_CSN_off digitalWrite(A7105_CSN_pin,LOW)
|
||||||
|
|
||||||
|
#define NRF_CE_on
|
||||||
|
#define NRF_CE_off
|
||||||
|
|
||||||
|
#define SCK_on digitalWrite(SCK_pin,HIGH)
|
||||||
|
#define SCK_off digitalWrite(SCK_pin,LOW)
|
||||||
|
|
||||||
|
#define SDI_on digitalWrite(SDI_pin,HIGH)
|
||||||
|
#define SDI_off digitalWrite(SDI_pin,LOW)
|
||||||
|
|
||||||
|
#define SDI_1 (digitalRead(SDI_pin)==HIGH)
|
||||||
|
#define SDI_0 (digitalRead(SDI_pin)==LOW)
|
||||||
|
|
||||||
|
#define CC25_CSN_on digitalWrite(CC25_CSN_pin,HIGH)
|
||||||
|
#define CC25_CSN_off digitalWrite(CC25_CSN_pin,LOW)
|
||||||
|
|
||||||
|
#define NRF_CSN_on digitalWrite(NRF_CSN_pin,HIGH)
|
||||||
|
#define NRF_CSN_off digitalWrite(NRF_CSN_pin,LOW)
|
||||||
|
|
||||||
|
#define CYRF_CSN_on digitalWrite(CYRF_CSN_pin,HIGH)
|
||||||
|
#define CYRF_CSN_off digitalWrite(CYRF_CSN_pin,LOW)
|
||||||
|
|
||||||
|
#define CYRF_RST_HI digitalWrite(CYRF_RST_pin,HIGH) //reset cyrf
|
||||||
|
#define CYRF_RST_LO digitalWrite(CYRF_RST_pin,LOW) //
|
||||||
|
|
||||||
|
#define SDO_1 (digitalRead(SDO_pin)==HIGH)
|
||||||
|
#define SDO_0 (digitalRead(SDO_pin)==LOW)
|
||||||
|
|
||||||
|
#define TX_INV_on digitalWrite(TX_INV_pin,HIGH)
|
||||||
|
#define TX_INV_off digitalWrite(TX_INV_pin,LOW)
|
||||||
|
|
||||||
|
#define RX_INV_on digitalWrite(RX_INV_pin,HIGH)
|
||||||
|
#define RX_INV_off digitalWrite(RX_INV_pin,LOW)
|
||||||
|
|
||||||
|
#define LED_on digitalWrite(LED_pin,HIGH)
|
||||||
|
#define LED_off digitalWrite(LED_pin,LOW)
|
||||||
|
#define LED_toggle digitalWrite(LED_pin ,!digitalRead(LED_pin))
|
||||||
|
#define LED_output pinMode(LED_pin,OUTPUT)
|
||||||
|
#define IS_LED_on ( digitalRead(LED_pin)==HIGH)
|
||||||
|
|
||||||
|
#define BIND_SET_INPUT pinMode(BIND_pin,INPUT)
|
||||||
|
#define BIND_SET_PULLUP digitalWrite(BIND_pin,HIGH)
|
||||||
|
#define BIND_SET_OUTPUT pinMode(BIND_pin,OUTPUT)
|
||||||
|
#define IS_BIND_BUTTON_on (digitalRead(BIND_pin)==LOW)
|
||||||
|
|
||||||
|
#define cli() noInterrupts()
|
||||||
|
#define sei() interrupts()
|
||||||
|
#define delayMilliseconds(x) delay(x)
|
||||||
|
//TX Pause
|
||||||
|
#undef TX_MAIN_PAUSE_off
|
||||||
|
#undef TX_MAIN_PAUSE_on
|
||||||
|
#undef IS_TX_MAIN_PAUSE_on
|
||||||
|
#undef TX_RX_PAUSE_off
|
||||||
|
#undef TX_RX_PAUSE_on
|
||||||
|
#undef IS_TX_RX_PAUSE_on
|
||||||
|
#undef IS_TX_PAUSE_on
|
||||||
|
#define TX_MAIN_PAUSE_off
|
||||||
|
#define TX_MAIN_PAUSE_on
|
||||||
|
#define IS_TX_MAIN_PAUSE_on
|
||||||
|
#define TX_RX_PAUSE_off
|
||||||
|
#define TX_RX_PAUSE_on
|
||||||
|
#define IS_TX_RX_PAUSE_on
|
||||||
|
#define IS_TX_PAUSE_on
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//*******************
|
||||||
|
//*** Timer ***
|
||||||
|
//*******************
|
||||||
|
#ifdef ORANGE_TX
|
||||||
|
#define TIFR1 TCC1.INTFLAGS
|
||||||
|
#define OCF1A_bm TC1_CCAIF_bm
|
||||||
|
#define OCR1A TCC1.CCA
|
||||||
|
#define TCNT1 TCC1.CNT
|
||||||
|
#define UDR0 USARTC0.DATA
|
||||||
|
#define OCF1B_bm TC1_CCBIF_bm
|
||||||
|
#define OCR1B TCC1.CCB
|
||||||
|
#define TIMSK1 TCC1.INTCTRLB
|
||||||
|
#define SET_TIMSK1_OCIE1B TIMSK1 = (TIMSK1 & 0xF3) | 0x04
|
||||||
|
#define CLR_TIMSK1_OCIE1B TIMSK1 &= 0xF3
|
||||||
|
#else
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
#define OCR1A TIMER2_BASE->CCR1
|
||||||
|
#define TCNT1 TIMER2_BASE->CNT
|
||||||
|
#define UDR0 USART2_BASE->DR
|
||||||
|
#define TIFR1 TIMER2_BASE->SR
|
||||||
|
#define OCF1A_bm TIMER_SR_CC1IF
|
||||||
|
#define UCSR0B USART2_BASE->CR1
|
||||||
|
#define RXCIE0 USART_CR1_RXNEIE_BIT
|
||||||
|
#define TXCIE0 USART_CR1_TXEIE_BIT
|
||||||
|
//#define TIFR1 TIMER2_BASE->SR
|
||||||
|
#else
|
||||||
|
#define OCF1A_bm _BV(OCF1A)
|
||||||
|
#define OCF1B_bm _BV(OCF1B)
|
||||||
|
#define SET_TIMSK1_OCIE1B TIMSK1 |= _BV(OCIE1B)
|
||||||
|
#define CLR_TIMSK1_OCIE1B TIMSK1 &=~_BV(OCIE1B)
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//*******************
|
||||||
|
//*** EEPROM ***
|
||||||
|
//*******************
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
#define EE_ADDR uint16
|
||||||
|
#define eeprom_write_byte EEPROM.write
|
||||||
|
#define eeprom_read_byte EEPROM.read
|
||||||
|
#else
|
||||||
|
#define EE_ADDR uint8_t*
|
||||||
#endif
|
#endif
|
||||||
|
@ -126,6 +126,22 @@ CH1|CH2|CH3|CH4
|
|||||||
---|---|---|---
|
---|---|---|---
|
||||||
A|E|T|R
|
A|E|T|R
|
||||||
|
|
||||||
|
###CG023
|
||||||
|
###Sub_protocol H8_3D
|
||||||
|
Models: EAchine H8 mini 3D, JJRC H20/H22, JJRC H11D
|
||||||
|
|
||||||
|
CH5|CH6|CH7|CH8|CH9|CH10|CH11
|
||||||
|
---|---|---|---|---|---|---
|
||||||
|
FLIP|LIGTH|OPT1|OPT2|CAL|SNAPSHOT|VIDEO
|
||||||
|
|
||||||
|
JJRC H20: OPT1=Headless, OPT2=RTH
|
||||||
|
|
||||||
|
JJRC H22: OPT1=RTH, OPT2=180/360° flip mode
|
||||||
|
|
||||||
|
H8 3D: OPT1=RTH then press a direction to enter headless mode (like stock TX), OPT2=switch 180/360° flip mode
|
||||||
|
|
||||||
|
CAL: calibrate accelerometers
|
||||||
|
|
||||||
###ESKY150
|
###ESKY150
|
||||||
|
|
||||||
Autobind
|
Autobind
|
||||||
@ -159,6 +175,9 @@ CH1|CH2|CH3|CH4|CH5|CH6|CH7|CH8
|
|||||||
---|---|---|---|---|---|---|---
|
---|---|---|---|---|---|---|---
|
||||||
A|E|T|R|CH5|CH6|CH7|CH8
|
A|E|T|R|CH5|CH6|CH7|CH8
|
||||||
|
|
||||||
|
###HISKY
|
||||||
|
####Sub_protocol HK310
|
||||||
|
|
||||||
###HM830
|
###HM830
|
||||||
Modele: HM Hobby HM830 RC Paper Airplane
|
Modele: HM Hobby HM830 RC Paper Airplane
|
||||||
|
|
||||||
@ -226,5 +245,20 @@ A|E|T|R|FLIP 360|FLIP|VIDEO|LED|MODE 2
|
|||||||
####Sub_protocol U839_2014
|
####Sub_protocol U839_2014
|
||||||
Same channels assignement as above.
|
Same channels assignement as above.
|
||||||
|
|
||||||
|
###V2X2
|
||||||
|
####Sub_protocol V2X2
|
||||||
|
Models: WLToys V202/252/272, JXD 385/388, JJRC H6C, Yizhan Tarantula X6 ...
|
||||||
|
|
||||||
|
CH1|CH2|CH3|CH4|CH5|CH6|CH7|CH8|CH9|CH10|CH11
|
||||||
|
---|---|---|---|---|---|---|---|---|----|----
|
||||||
|
A|E|T|R|FLIP|LIGHT|PICTURE|VIDEO|HEADLESS|MAG_CAL_X|MAG_CAL_Y
|
||||||
|
|
||||||
|
PICTURE: also automatic Missile Launcher and Hoist in one direction
|
||||||
|
|
||||||
|
VIDEO: also Sprayer, Bubbler, Missile Launcher(1), and Hoist in the other dir
|
||||||
|
####Sub_protocol JXD-506
|
||||||
|
CH10|CH11|CH12
|
||||||
|
----|----|----
|
||||||
|
ARM|EMERGENCY|PAN CAMERA
|
||||||
|
|
||||||
###D'autres à venir
|
###D'autres à venir
|
@ -15,14 +15,86 @@
|
|||||||
/********************/
|
/********************/
|
||||||
/** SPI routines **/
|
/** SPI routines **/
|
||||||
/********************/
|
/********************/
|
||||||
#ifdef XMEGA
|
#ifdef STM32_BOARD
|
||||||
#define XNOP() NOP()
|
|
||||||
#else
|
|
||||||
#define XNOP()
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void SPI_Write(uint8_t command)
|
SPIClass SPI_2(2); //Create an instance of the SPI Class called SPI_2 that uses the 2nd SPI Port
|
||||||
{
|
|
||||||
|
void initSPI2()
|
||||||
|
{
|
||||||
|
//SPI_DISABLE();
|
||||||
|
SPI_2.end();
|
||||||
|
SPI2_BASE->CR1 &= ~SPI_CR1_DFF_8_BIT; //8 bits format This bit should be written only when SPI is disabled (SPE = ?0?) for correct operation.
|
||||||
|
SPI_2.begin(); //Initialize the SPI_2 port.
|
||||||
|
|
||||||
|
SPI_2.setBitOrder(MSBFIRST); // Set the SPI_2 bit order
|
||||||
|
SPI_2.setDataMode(SPI_MODE0); // Set the SPI_2 data mode 0
|
||||||
|
SPI_2.setClockDivider(SPI_CLOCK_DIV8); // Set the speed (36 / 8 = 4.5 MHz SPI_2 speed)
|
||||||
|
}
|
||||||
|
|
||||||
|
void SPI_Write(uint8_t command)
|
||||||
|
{//working OK
|
||||||
|
SPI2_BASE->DR = command; //Write the first data item to be transmitted into the SPI_DR register (this clears the TXE flag).
|
||||||
|
while (!(SPI2_BASE->SR & SPI_SR_RXNE));
|
||||||
|
command = SPI2_BASE->DR; // ... and read the last received data.
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t SPI_Read(void)
|
||||||
|
{
|
||||||
|
SPI_Write(0x00);
|
||||||
|
return SPI2_BASE->DR;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t SPI_SDI_Read()
|
||||||
|
{
|
||||||
|
uint8_t rx=0;
|
||||||
|
cli(); //Fix Hubsan droputs??
|
||||||
|
while(!(SPI2_BASE->SR & SPI_SR_TXE));
|
||||||
|
while((SPI2_BASE->SR & SPI_SR_BSY));
|
||||||
|
//
|
||||||
|
SPI_DISABLE();
|
||||||
|
SPI_SET_BIDIRECTIONAL();
|
||||||
|
volatile uint8_t x = SPI2_BASE->DR;
|
||||||
|
(void)x;
|
||||||
|
SPI_ENABLE();
|
||||||
|
//
|
||||||
|
SPI_DISABLE();
|
||||||
|
while(!(SPI2_BASE->SR& SPI_SR_RXNE));
|
||||||
|
rx=SPI2_BASE->DR;
|
||||||
|
SPI_SET_UNIDIRECTIONAL();
|
||||||
|
SPI_ENABLE();
|
||||||
|
sei();//fix Hubsan dropouts??
|
||||||
|
return rx;
|
||||||
|
}
|
||||||
|
|
||||||
|
void SPI_ENABLE()
|
||||||
|
{
|
||||||
|
SPI2_BASE->CR1 |= SPI_CR1_SPE;
|
||||||
|
}
|
||||||
|
|
||||||
|
void SPI_DISABLE()
|
||||||
|
{
|
||||||
|
SPI2_BASE->CR1 &= ~SPI_CR1_SPE;
|
||||||
|
}
|
||||||
|
|
||||||
|
void SPI_SET_BIDIRECTIONAL()
|
||||||
|
{
|
||||||
|
SPI2_BASE->CR1 |= SPI_CR1_BIDIMODE;
|
||||||
|
SPI2_BASE->CR1 &= ~ SPI_CR1_BIDIOE;//receive only
|
||||||
|
}
|
||||||
|
|
||||||
|
void SPI_SET_UNIDIRECTIONAL()
|
||||||
|
{
|
||||||
|
SPI2_BASE->CR1 &= ~SPI_CR1_BIDIMODE;
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
#ifdef ORANGE_TX
|
||||||
|
#define XNOP() NOP()
|
||||||
|
#else
|
||||||
|
#define XNOP()
|
||||||
|
#endif
|
||||||
|
|
||||||
|
void SPI_Write(uint8_t command)
|
||||||
|
{
|
||||||
uint8_t n=8;
|
uint8_t n=8;
|
||||||
|
|
||||||
SCLK_off;//SCK start low
|
SCLK_off;//SCK start low
|
||||||
@ -45,10 +117,10 @@ void SPI_Write(uint8_t command)
|
|||||||
}
|
}
|
||||||
while(--n) ;
|
while(--n) ;
|
||||||
SDI_on;
|
SDI_on;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t SPI_Read(void)
|
uint8_t SPI_Read(void)
|
||||||
{
|
{
|
||||||
uint8_t result=0,i;
|
uint8_t result=0,i;
|
||||||
for(i=0;i<8;i++)
|
for(i=0;i<8;i++)
|
||||||
{
|
{
|
||||||
@ -64,11 +136,11 @@ uint8_t SPI_Read(void)
|
|||||||
XNOP();
|
XNOP();
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef A7105_INSTALLED
|
#ifdef A7105_INSTALLED
|
||||||
uint8_t SPI_SDIO_Read(void)
|
uint8_t SPI_SDI_Read(void)
|
||||||
{
|
{
|
||||||
uint8_t result=0;
|
uint8_t result=0;
|
||||||
SDI_input;
|
SDI_input;
|
||||||
for(uint8_t i=0;i<8;i++)
|
for(uint8_t i=0;i<8;i++)
|
||||||
@ -82,5 +154,6 @@ uint8_t SPI_SDIO_Read(void)
|
|||||||
}
|
}
|
||||||
SDI_output;
|
SDI_output;
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
#endif//STM32_BOARD
|
@ -60,199 +60,151 @@
|
|||||||
|
|
||||||
//Channel definitions
|
//Channel definitions
|
||||||
#ifdef AETR
|
#ifdef AETR
|
||||||
enum {
|
#define AILERON 0
|
||||||
AILERON =0,
|
#define ELEVATOR 1
|
||||||
ELEVATOR,
|
#define THROTTLE 2
|
||||||
THROTTLE,
|
#define RUDDER 3
|
||||||
RUDDER,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef AERT
|
#ifdef AERT
|
||||||
enum {
|
#define AILERON 0
|
||||||
AILERON =0,
|
#define ELEVATOR 1
|
||||||
ELEVATOR,
|
#define THROTTLE 3
|
||||||
RUDDER,
|
#define RUDDER 2
|
||||||
THROTTLE,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef ARET
|
#ifdef ARET
|
||||||
enum {
|
#define AILERON 0
|
||||||
AILERON =0,
|
#define ELEVATOR 2
|
||||||
RUDDER,
|
#define THROTTLE 3
|
||||||
ELEVATOR,
|
#define RUDDER 1
|
||||||
THROTTLE,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef ARTE
|
#ifdef ARTE
|
||||||
enum {
|
#define AILERON 0
|
||||||
AILERON =0,
|
#define ELEVATOR 3
|
||||||
RUDDER,
|
#define THROTTLE 2
|
||||||
THROTTLE,
|
#define RUDDER 1
|
||||||
ELEVATOR,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef ATRE
|
#ifdef ATRE
|
||||||
enum {
|
#define AILERON 0
|
||||||
AILERON =0,
|
#define ELEVATOR 3
|
||||||
THROTTLE,
|
#define THROTTLE 1
|
||||||
RUDDER,
|
#define RUDDER 2
|
||||||
ELEVATOR,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef ATER
|
#ifdef ATER
|
||||||
enum {
|
#define AILERON 0
|
||||||
AILERON =0,
|
#define ELEVATOR 2
|
||||||
THROTTLE,
|
#define THROTTLE 1
|
||||||
ELEVATOR,
|
#define RUDDER 3
|
||||||
RUDDER,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef EATR
|
#ifdef EATR
|
||||||
enum {
|
#define AILERON 1
|
||||||
ELEVATOR =0,
|
#define ELEVATOR 0
|
||||||
AILERON,
|
#define THROTTLE 2
|
||||||
THROTTLE,
|
#define RUDDER 3
|
||||||
RUDDER,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef EART
|
#ifdef EART
|
||||||
enum {
|
#define AILERON 1
|
||||||
ELEVATOR =0,
|
#define ELEVATOR 0
|
||||||
AILERON,
|
#define THROTTLE 3
|
||||||
RUDDER,
|
#define RUDDER 2
|
||||||
THROTTLE,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef ERAT
|
#ifdef ERAT
|
||||||
enum {
|
#define AILERON 2
|
||||||
ELEVATOR =0,
|
#define ELEVATOR 0
|
||||||
RUDDER,
|
#define THROTTLE 3
|
||||||
AILERON,
|
#define RUDDER 1
|
||||||
THROTTLE,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef ERTA
|
#ifdef ERTA
|
||||||
enum {
|
#define AILERON 3
|
||||||
ELEVATOR =0,
|
#define ELEVATOR 0
|
||||||
RUDDER,
|
#define THROTTLE 2
|
||||||
THROTTLE,
|
#define RUDDER 1
|
||||||
AILERON,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef ETRA
|
#ifdef ETRA
|
||||||
enum {
|
#define AILERON 3
|
||||||
ELEVATOR =0,
|
#define ELEVATOR 0
|
||||||
THROTTLE,
|
#define THROTTLE 1
|
||||||
RUDDER,
|
#define RUDDER 2
|
||||||
AILERON,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef ETAR
|
#ifdef ETAR
|
||||||
enum {
|
#define AILERON 2
|
||||||
ELEVATOR =0,
|
#define ELEVATOR 0
|
||||||
THROTTLE,
|
#define THROTTLE 1
|
||||||
AILERON,
|
#define RUDDER 3
|
||||||
RUDDER,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef TEAR
|
#ifdef TEAR
|
||||||
enum {
|
#define AILERON 2
|
||||||
THROTTLE =0,
|
#define ELEVATOR 1
|
||||||
ELEVATOR,
|
#define THROTTLE 0
|
||||||
AILERON,
|
#define RUDDER 3
|
||||||
RUDDER,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef TERA
|
#ifdef TERA
|
||||||
enum {
|
#define AILERON 3
|
||||||
THROTTLE =0,
|
#define ELEVATOR 1
|
||||||
ELEVATOR,
|
#define THROTTLE 0
|
||||||
RUDDER,
|
#define RUDDER 2
|
||||||
AILERON,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef TREA
|
#ifdef TREA
|
||||||
enum {
|
#define AILERON 3
|
||||||
THROTTLE =0,
|
#define ELEVATOR 2
|
||||||
RUDDER,
|
#define THROTTLE 0
|
||||||
ELEVATOR,
|
#define RUDDER 1
|
||||||
AILERON,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef TRAE
|
#ifdef TRAE
|
||||||
enum {
|
#define AILERON 2
|
||||||
THROTTLE =0,
|
#define ELEVATOR 3
|
||||||
RUDDER,
|
#define THROTTLE 0
|
||||||
AILERON,
|
#define RUDDER 1
|
||||||
ELEVATOR,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef TARE
|
#ifdef TARE
|
||||||
enum {
|
#define AILERON 1
|
||||||
THROTTLE =0,
|
#define ELEVATOR 3
|
||||||
AILERON,
|
#define THROTTLE 0
|
||||||
RUDDER,
|
#define RUDDER 2
|
||||||
ELEVATOR,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef TAER
|
#ifdef TAER
|
||||||
enum {
|
#define AILERON 1
|
||||||
THROTTLE =0,
|
#define ELEVATOR 2
|
||||||
AILERON,
|
#define THROTTLE 0
|
||||||
ELEVATOR,
|
#define RUDDER 3
|
||||||
RUDDER,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef RETA
|
#ifdef RETA
|
||||||
enum {
|
#define AILERON 3
|
||||||
RUDDER =0,
|
#define ELEVATOR 1
|
||||||
ELEVATOR,
|
#define THROTTLE 2
|
||||||
THROTTLE,
|
#define RUDDER 0
|
||||||
AILERON,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef REAT
|
#ifdef REAT
|
||||||
enum {
|
#define AILERON 2
|
||||||
RUDDER =0,
|
#define ELEVATOR 1
|
||||||
ELEVATOR,
|
#define THROTTLE 3
|
||||||
AILERON,
|
#define RUDDER 0
|
||||||
THROTTLE,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef RAET
|
#ifdef RAET
|
||||||
enum {
|
#define AILERON 1
|
||||||
RUDDER =0,
|
#define ELEVATOR 2
|
||||||
AILERON,
|
#define THROTTLE 3
|
||||||
ELEVATOR,
|
#define RUDDER 0
|
||||||
THROTTLE,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef RATE
|
#ifdef RATE
|
||||||
enum {
|
#define AILERON 1
|
||||||
RUDDER =0,
|
#define ELEVATOR 3
|
||||||
AILERON,
|
#define THROTTLE 2
|
||||||
THROTTLE,
|
#define RUDDER 0
|
||||||
ELEVATOR,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef RTAE
|
#ifdef RTAE
|
||||||
enum {
|
#define AILERON 2
|
||||||
RUDDER =0,
|
#define ELEVATOR 3
|
||||||
THROTTLE,
|
#define THROTTLE 1
|
||||||
AILERON,
|
#define RUDDER 0
|
||||||
ELEVATOR,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
#ifdef RTEA
|
#ifdef RTEA
|
||||||
enum {
|
#define AILERON 3
|
||||||
RUDDER =0,
|
#define ELEVATOR 2
|
||||||
THROTTLE,
|
#define THROTTLE 1
|
||||||
ELEVATOR,
|
#define RUDDER 0
|
||||||
AILERON,
|
|
||||||
};
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define AUX1 4
|
#define AUX1 4
|
||||||
|
@ -38,7 +38,7 @@
|
|||||||
#define MAX_PKTX 10
|
#define MAX_PKTX 10
|
||||||
uint8_t pktx[MAX_PKTX];
|
uint8_t pktx[MAX_PKTX];
|
||||||
uint8_t pktx1[MAX_PKTX];
|
uint8_t pktx1[MAX_PKTX];
|
||||||
uint8_t index;
|
uint8_t indx;
|
||||||
uint8_t frame[18];
|
uint8_t frame[18];
|
||||||
|
|
||||||
#ifdef BASH_SERIAL
|
#ifdef BASH_SERIAL
|
||||||
@ -94,6 +94,7 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
|
|||||||
for (uint8_t i=3;i<len;i++)
|
for (uint8_t i=3;i<len;i++)
|
||||||
pktt[i]=pkt[i];
|
pktt[i]=pkt[i];
|
||||||
telemetry_link=1;
|
telemetry_link=1;
|
||||||
|
telemetry_lost=0;
|
||||||
if(pktt[6])
|
if(pktt[6])
|
||||||
telemetry_counter=(telemetry_counter+1)%32;
|
telemetry_counter=(telemetry_counter+1)%32;
|
||||||
//
|
//
|
||||||
@ -172,28 +173,28 @@ void frsky_user_frame()
|
|||||||
pass=1;
|
pass=1;
|
||||||
|
|
||||||
case 1:
|
case 1:
|
||||||
index=indexx;
|
indx=indexx;
|
||||||
prev_index = indexx;
|
prev_index = indexx;
|
||||||
if(index<USER_MAX_BYTES)
|
if(indx<USER_MAX_BYTES)
|
||||||
{
|
{
|
||||||
for(i=0;i<index;i++)
|
for(i=0;i<indx;i++)
|
||||||
frame[i+3]=pktx[i];
|
frame[i+3]=pktx[i];
|
||||||
pktt[6]=0;
|
pktt[6]=0;
|
||||||
pass=0;
|
pass=0;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
index = USER_MAX_BYTES;
|
indx = USER_MAX_BYTES;
|
||||||
for(i=0;i<index;i++)
|
for(i=0;i<indx;i++)
|
||||||
frame[i+3]=pktx[i];
|
frame[i+3]=pktx[i];
|
||||||
pass=2;
|
pass=2;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
index = prev_index - index;
|
indx = prev_index - indx;
|
||||||
prev_index=0;
|
prev_index=0;
|
||||||
if(index<=(MAX_PKTX-USER_MAX_BYTES)) //10-6=4
|
if(indx<=(MAX_PKTX-USER_MAX_BYTES)) //10-6=4
|
||||||
for(i=0;i<index;i++)
|
for(i=0;i<indx;i++)
|
||||||
frame[i+3]=pktx[USER_MAX_BYTES+i];
|
frame[i+3]=pktx[USER_MAX_BYTES+i];
|
||||||
pass=0;
|
pass=0;
|
||||||
pktt[6]=0;
|
pktt[6]=0;
|
||||||
@ -201,9 +202,9 @@ void frsky_user_frame()
|
|||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if(!index)
|
if(!indx)
|
||||||
return;
|
return;
|
||||||
frame[1] = index;
|
frame[1] = indx;
|
||||||
frskySendStuffed();
|
frskySendStuffed();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
@ -311,7 +312,11 @@ void sportSendFrame()
|
|||||||
{
|
{
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
sport_counter = (sport_counter + 1) %36;
|
sport_counter = (sport_counter + 1) %36;
|
||||||
|
if(telemetry_lost)
|
||||||
|
{
|
||||||
|
sportIdle();
|
||||||
|
return;
|
||||||
|
}
|
||||||
if(sport_counter<6)
|
if(sport_counter<6)
|
||||||
{
|
{
|
||||||
frame[0] = 0x98;
|
frame[0] = 0x98;
|
||||||
@ -362,30 +367,30 @@ void proces_sport_data(uint8_t data)
|
|||||||
case 0:
|
case 0:
|
||||||
if (data == START_STOP)
|
if (data == START_STOP)
|
||||||
{//waiting for 0x7e
|
{//waiting for 0x7e
|
||||||
index = 0;
|
indx = 0;
|
||||||
pass = 1;
|
pass = 1;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case 1:
|
case 1:
|
||||||
if (data == START_STOP) // Happens if missed packet
|
if (data == START_STOP) // Happens if missed packet
|
||||||
{//waiting for 0x7e
|
{//waiting for 0x7e
|
||||||
index = 0;
|
indx = 0;
|
||||||
pass = 1;
|
pass = 1;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if(data == BYTESTUFF)//if they are stuffed
|
if(data == BYTESTUFF)//if they are stuffed
|
||||||
pass=2;
|
pass=2;
|
||||||
else
|
else
|
||||||
if (index < MAX_PKTX)
|
if (indx < MAX_PKTX)
|
||||||
pktx[index++] = data;
|
pktx[indx++] = data;
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
if (index < MAX_PKTX)
|
if (indx < MAX_PKTX)
|
||||||
pktx[index++] = data ^ STUFF_MASK; //unstuff bytes
|
pktx[indx++] = data ^ STUFF_MASK; //unstuff bytes
|
||||||
pass=1;
|
pass=1;
|
||||||
break;
|
break;
|
||||||
} // end switch
|
} // end switch
|
||||||
if (index >= FRSKY_SPORT_PACKET_SIZE)
|
if (indx >= FRSKY_SPORT_PACKET_SIZE)
|
||||||
{//8 bytes no crc
|
{//8 bytes no crc
|
||||||
if ( sport )
|
if ( sport )
|
||||||
{
|
{
|
||||||
@ -408,22 +413,6 @@ void proces_sport_data(uint8_t data)
|
|||||||
|
|
||||||
void TelemetryUpdate()
|
void TelemetryUpdate()
|
||||||
{
|
{
|
||||||
#if defined SPORT_TELEMETRY
|
|
||||||
if (protocol==MODE_FRSKYX)
|
|
||||||
{ // FrSkyX
|
|
||||||
if(telemetry_link)
|
|
||||||
{
|
|
||||||
if(pktt[4] & 0x80)
|
|
||||||
rssi=pktt[4] & 0x7F ;
|
|
||||||
else
|
|
||||||
RxBt = (pktt[4]<<1) + 1 ;
|
|
||||||
for (uint8_t i=0; i < pktt[6]; i++)
|
|
||||||
proces_sport_data(pktt[7+i]);
|
|
||||||
telemetry_link=0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// check for space in tx buffer
|
// check for space in tx buffer
|
||||||
|
|
||||||
#ifdef BASH_SERIAL
|
#ifdef BASH_SERIAL
|
||||||
@ -463,6 +452,33 @@ void TelemetryUpdate()
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if defined SPORT_TELEMETRY
|
||||||
|
if (protocol==MODE_FRSKYX)
|
||||||
|
{ // FrSkyX
|
||||||
|
if(telemetry_link)
|
||||||
|
{
|
||||||
|
if(pktt[4] & 0x80)
|
||||||
|
rssi=pktt[4] & 0x7F ;
|
||||||
|
else
|
||||||
|
RxBt = (pktt[4]<<1) + 1 ;
|
||||||
|
if(pktt[6]<=6)
|
||||||
|
for (uint8_t i=0; i < pktt[6]; i++)
|
||||||
|
proces_sport_data(pktt[7+i]);
|
||||||
|
telemetry_link=0;
|
||||||
|
}
|
||||||
|
uint32_t now = micros();
|
||||||
|
if ((now - last) > SPORT_TIME)
|
||||||
|
{
|
||||||
|
sportSendFrame();
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
last=now;
|
||||||
|
#else
|
||||||
|
last += SPORT_TIME ;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
#if defined DSM_TELEMETRY
|
#if defined DSM_TELEMETRY
|
||||||
if(telemetry_link && protocol == MODE_DSM )
|
if(telemetry_link && protocol == MODE_DSM )
|
||||||
{ // DSM
|
{ // DSM
|
||||||
@ -484,17 +500,6 @@ void TelemetryUpdate()
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
#if defined SPORT_TELEMETRY
|
|
||||||
if (protocol==MODE_FRSKYX)
|
|
||||||
{ // FrSkyX
|
|
||||||
uint32_t now = micros();
|
|
||||||
if ((now - last) > SPORT_TIME)
|
|
||||||
{
|
|
||||||
sportSendFrame();
|
|
||||||
last += SPORT_TIME ;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -505,9 +510,9 @@ void TelemetryUpdate()
|
|||||||
/**************************/
|
/**************************/
|
||||||
|
|
||||||
#ifndef BASH_SERIAL
|
#ifndef BASH_SERIAL
|
||||||
// Routines for normal serial output
|
// Routines for normal serial output
|
||||||
void Serial_write(uint8_t data)
|
void Serial_write(uint8_t data)
|
||||||
{
|
{
|
||||||
uint8_t nextHead ;
|
uint8_t nextHead ;
|
||||||
nextHead = tx_head + 1 ;
|
nextHead = tx_head + 1 ;
|
||||||
if ( nextHead >= TXBUFFER_SIZE )
|
if ( nextHead >= TXBUFFER_SIZE )
|
||||||
@ -515,90 +520,130 @@ void Serial_write(uint8_t data)
|
|||||||
tx_buff[nextHead]=data;
|
tx_buff[nextHead]=data;
|
||||||
tx_head = nextHead ;
|
tx_head = nextHead ;
|
||||||
tx_resume();
|
tx_resume();
|
||||||
}
|
}
|
||||||
|
|
||||||
void initTXSerial( uint8_t speed)
|
void initTXSerial( uint8_t speed)
|
||||||
{
|
{
|
||||||
#ifdef ENABLE_PPM
|
#ifdef ENABLE_PPM
|
||||||
if(speed==SPEED_9600)
|
if(speed==SPEED_9600)
|
||||||
{ // 9600
|
{ // 9600
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
USARTC0.BAUDCTRLA = 207 ;
|
USARTC0.BAUDCTRLA = 207 ;
|
||||||
USARTC0.BAUDCTRLB = 0 ;
|
USARTC0.BAUDCTRLB = 0 ;
|
||||||
USARTC0.CTRLB = 0x18 ;
|
USARTC0.CTRLB = 0x18 ;
|
||||||
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
|
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
|
||||||
USARTC0.CTRLC = 0x03 ;
|
USARTC0.CTRLC = 0x03 ;
|
||||||
#else
|
#else
|
||||||
//9600 bauds
|
#ifdef STM32_BOARD
|
||||||
|
Serial2.begin(9600); //USART3
|
||||||
|
USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
|
||||||
|
#else
|
||||||
UBRR0H = 0x00;
|
UBRR0H = 0x00;
|
||||||
UBRR0L = 0x67;
|
UBRR0L = 0x67;
|
||||||
UCSR0A = 0 ; // Clear X2 bit
|
UCSR0A = 0 ; // Clear X2 bit
|
||||||
//Set frame format to 8 data bits, none, 1 stop bit
|
//Set frame format to 8 data bits, none, 1 stop bit
|
||||||
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
|
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
|
||||||
#endif
|
#endif
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
else if(speed==SPEED_57600)
|
else if(speed==SPEED_57600)
|
||||||
{ // 57600
|
{ // 57600
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
/*USARTC0.BAUDCTRLA = 207 ;
|
/*USARTC0.BAUDCTRLA = 207 ;
|
||||||
USARTC0.BAUDCTRLB = 0 ;
|
USARTC0.BAUDCTRLB = 0 ;
|
||||||
USARTC0.CTRLB = 0x18 ;
|
USARTC0.CTRLB = 0x18 ;
|
||||||
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
|
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
|
||||||
USARTC0.CTRLC = 0x03 ;*/
|
USARTC0.CTRLC = 0x03 ;*/
|
||||||
#else
|
#else
|
||||||
//57600 bauds
|
#ifdef STM32_BOARD
|
||||||
|
Serial2.begin(57600); //USART3
|
||||||
|
USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
|
||||||
|
#else
|
||||||
UBRR0H = 0x00;
|
UBRR0H = 0x00;
|
||||||
UBRR0L = 0x22;
|
UBRR0L = 0x22;
|
||||||
UCSR0A = 0x02 ; // Set X2 bit
|
UCSR0A = 0x02 ; // Set X2 bit
|
||||||
//Set frame format to 8 data bits, none, 1 stop bit
|
//Set frame format to 8 data bits, none, 1 stop bit
|
||||||
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
|
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
|
||||||
#endif
|
#endif
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
else if(speed==SPEED_125K)
|
else if(speed==SPEED_125K)
|
||||||
{ // 125000
|
{ // 125000
|
||||||
#ifdef XMEGA
|
#ifdef ORANGE_TX
|
||||||
/*USARTC0.BAUDCTRLA = 207 ;
|
/*USARTC0.BAUDCTRLA = 207 ;
|
||||||
USARTC0.BAUDCTRLB = 0 ;
|
USARTC0.BAUDCTRLB = 0 ;
|
||||||
USARTC0.CTRLB = 0x18 ;
|
USARTC0.CTRLB = 0x18 ;
|
||||||
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
|
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
|
||||||
USARTC0.CTRLC = 0x03 ;*/
|
USARTC0.CTRLC = 0x03 ;*/
|
||||||
#else
|
#else
|
||||||
//125000 bauds
|
#ifdef STM32_BOARD
|
||||||
|
Serial2.begin(125000); //USART3
|
||||||
|
USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
|
||||||
|
#else
|
||||||
UBRR0H = 0x00;
|
UBRR0H = 0x00;
|
||||||
UBRR0L = 0x07;
|
UBRR0L = 0x07;
|
||||||
UCSR0A = 0x00 ; // Clear X2 bit
|
UCSR0A = 0x00 ; // Clear X2 bit
|
||||||
//Set frame format to 8 data bits, none, 1 stop bit
|
//Set frame format to 8 data bits, none, 1 stop bit
|
||||||
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
|
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
|
||||||
#endif
|
#endif
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
#ifndef XMEGA
|
#ifndef ORANGE_TX
|
||||||
|
#ifndef STM32_BOARD
|
||||||
UCSR0B |= (1<<TXEN0);//tx enable
|
UCSR0B |= (1<<TXEN0);//tx enable
|
||||||
#endif
|
#endif
|
||||||
}
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
#ifdef XMEGA
|
//Serial TX
|
||||||
ISR(USARTC0_DRE_vect)
|
#ifdef ORANGE_TX
|
||||||
#else
|
ISR(USARTC0_DRE_vect)
|
||||||
ISR(USART_UDRE_vect)
|
#else
|
||||||
#endif
|
#ifdef STM32_BOARD
|
||||||
{ // Transmit interrupt
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
void __irq_usart3()
|
||||||
|
#else
|
||||||
|
ISR(USART_UDRE_vect)
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
{ // Transmit interrupt
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
if(USART3_BASE->SR & USART_SR_TXE)
|
||||||
|
{
|
||||||
|
#endif
|
||||||
if(tx_head!=tx_tail)
|
if(tx_head!=tx_tail)
|
||||||
{
|
{
|
||||||
if(++tx_tail>=TXBUFFER_SIZE)//head
|
if(++tx_tail>=TXBUFFER_SIZE)//head
|
||||||
tx_tail=0;
|
tx_tail=0;
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
USART3_BASE->DR=tx_buff[tx_tail];//clears TXE bit
|
||||||
|
#else
|
||||||
UDR0=tx_buff[tx_tail];
|
UDR0=tx_buff[tx_tail];
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
if (tx_tail == tx_head)
|
if (tx_tail == tx_head)
|
||||||
|
#ifdef STM32_BOARD
|
||||||
|
USART3_BASE->CR1 &= ~USART_CR1_TXEIE;//disable interrupt
|
||||||
|
}
|
||||||
|
#else
|
||||||
tx_pause(); // Check if all data is transmitted . if yes disable transmitter UDRE interrupt
|
tx_pause(); // Check if all data is transmitted . if yes disable transmitter UDRE interrupt
|
||||||
}
|
#endif
|
||||||
|
}
|
||||||
|
#if defined STM32_BOARD
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
#endif //STM32_BOARD
|
||||||
|
|
||||||
#else //BASH_SERIAL
|
#else //BASH_SERIAL
|
||||||
// Routines for bit-bashed serial output
|
// Routines for bit-bashed serial output
|
||||||
|
|
||||||
// Speed is 0 for 100K and 1 for 9600
|
// Speed is 0 for 100K and 1 for 9600
|
||||||
void initTXSerial( uint8_t speed)
|
void initTXSerial( uint8_t speed)
|
||||||
{
|
{
|
||||||
TIMSK0 = 0 ; // Stop all timer 0 interrupts
|
TIMSK0 = 0 ; // Stop all timer 0 interrupts
|
||||||
#ifdef INVERT_SERIAL
|
#ifdef INVERT_SERIAL
|
||||||
SERIAL_TX_off;
|
SERIAL_TX_off;
|
||||||
@ -619,27 +664,27 @@ void initTXSerial( uint8_t speed)
|
|||||||
TCCR0A = 0 ;
|
TCCR0A = 0 ;
|
||||||
TCCR0B = 2 ; // Clock/8 (0.5uS)
|
TCCR0B = 2 ; // Clock/8 (0.5uS)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void Serial_write( uint8_t byte )
|
void Serial_write( uint8_t byte )
|
||||||
{
|
{
|
||||||
uint8_t temp ;
|
uint8_t temp ;
|
||||||
uint8_t temp1 ;
|
uint8_t temp1 ;
|
||||||
uint8_t byteLo ;
|
uint8_t byteLo ;
|
||||||
|
|
||||||
#ifdef INVERT_SERIAL
|
#ifdef INVERT_SERIAL
|
||||||
byte = ~byte ;
|
byte = ~byte ;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
byteLo = byte ;
|
byteLo = byte ;
|
||||||
byteLo >>= 7 ; // Top bit
|
byteLo >>= 7 ; // Top bit
|
||||||
if ( SerialControl.speed == SPEED_100K )
|
if ( SerialControl.speed == SPEED_100K )
|
||||||
{
|
{
|
||||||
#ifdef INVERT_SERIAL
|
#ifdef INVERT_SERIAL
|
||||||
byteLo |= 0x02 ; // Parity bit
|
byteLo |= 0x02 ; // Parity bit
|
||||||
#else
|
#else
|
||||||
byteLo |= 0xFC ; // Stop bits
|
byteLo |= 0xFC ; // Stop bits
|
||||||
#endif
|
#endif
|
||||||
// calc parity
|
// calc parity
|
||||||
temp = byte ;
|
temp = byte ;
|
||||||
temp >>= 4 ;
|
temp >>= 4 ;
|
||||||
@ -651,20 +696,20 @@ void Serial_write( uint8_t byte )
|
|||||||
temp1 <<= 1 ;
|
temp1 <<= 1 ;
|
||||||
temp ^= temp1 ;
|
temp ^= temp1 ;
|
||||||
temp &= 0x02 ;
|
temp &= 0x02 ;
|
||||||
#ifdef INVERT_SERIAL
|
#ifdef INVERT_SERIAL
|
||||||
byteLo ^= temp ;
|
byteLo ^= temp ;
|
||||||
#else
|
#else
|
||||||
byteLo |= temp ;
|
byteLo |= temp ;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
byteLo |= 0xFE ; // Stop bit
|
byteLo |= 0xFE ; // Stop bit
|
||||||
}
|
}
|
||||||
byte <<= 1 ;
|
byte <<= 1 ;
|
||||||
#ifdef INVERT_SERIAL
|
#ifdef INVERT_SERIAL
|
||||||
byte |= 1 ; // Start bit
|
byte |= 1 ; // Start bit
|
||||||
#endif
|
#endif
|
||||||
uint8_t next = (SerialControl.head + 2) & 0x3f ;
|
uint8_t next = (SerialControl.head + 2) & 0x3f ;
|
||||||
if ( next != SerialControl.tail )
|
if ( next != SerialControl.tail )
|
||||||
{
|
{
|
||||||
@ -674,20 +719,20 @@ void Serial_write( uint8_t byte )
|
|||||||
}
|
}
|
||||||
if(!IS_TX_PAUSE_on)
|
if(!IS_TX_PAUSE_on)
|
||||||
tx_resume();
|
tx_resume();
|
||||||
}
|
}
|
||||||
|
|
||||||
void resumeBashSerial()
|
void resumeBashSerial()
|
||||||
{
|
{
|
||||||
cli() ;
|
cli() ;
|
||||||
if ( SerialControl.busy == 0 )
|
if ( SerialControl.busy == 0 )
|
||||||
{
|
{
|
||||||
sei() ;
|
sei() ;
|
||||||
// Start the transmission here
|
// Start the transmission here
|
||||||
#ifdef INVERT_SERIAL
|
#ifdef INVERT_SERIAL
|
||||||
GPIOR2 = 0 ;
|
GPIOR2 = 0 ;
|
||||||
#else
|
#else
|
||||||
GPIOR2 = 0x01 ;
|
GPIOR2 = 0x01 ;
|
||||||
#endif
|
#endif
|
||||||
if ( SerialControl.speed == SPEED_100K )
|
if ( SerialControl.speed == SPEED_100K )
|
||||||
{
|
{
|
||||||
GPIOR1 = 1 ;
|
GPIOR1 = 1 ;
|
||||||
@ -709,12 +754,12 @@ void resumeBashSerial()
|
|||||||
{
|
{
|
||||||
sei() ;
|
sei() ;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Assume timer0 at 0.5uS clock
|
// Assume timer0 at 0.5uS clock
|
||||||
|
|
||||||
ISR(TIMER0_COMPA_vect)
|
ISR(TIMER0_COMPA_vect)
|
||||||
{
|
{
|
||||||
uint8_t byte ;
|
uint8_t byte ;
|
||||||
byte = GPIOR0 ;
|
byte = GPIOR0 ;
|
||||||
if ( byte & 0x01 )
|
if ( byte & 0x01 )
|
||||||
@ -732,10 +777,10 @@ ISR(TIMER0_COMPA_vect)
|
|||||||
{
|
{
|
||||||
OCR0A += 20 ;
|
OCR0A += 20 ;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
ISR(TIMER0_COMPB_vect)
|
ISR(TIMER0_COMPB_vect)
|
||||||
{
|
{
|
||||||
uint8_t byte ;
|
uint8_t byte ;
|
||||||
byte = GPIOR2 ;
|
byte = GPIOR2 ;
|
||||||
if ( byte & 0x01 )
|
if ( byte & 0x01 )
|
||||||
@ -776,10 +821,10 @@ ISR(TIMER0_COMPB_vect)
|
|||||||
{
|
{
|
||||||
OCR0B += 20 ;
|
OCR0B += 20 ;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
ISR(TIMER0_OVF_vect)
|
ISR(TIMER0_OVF_vect)
|
||||||
{
|
{
|
||||||
uint8_t byte ;
|
uint8_t byte ;
|
||||||
if ( GPIOR1 > 2 )
|
if ( GPIOR1 > 2 )
|
||||||
{
|
{
|
||||||
@ -819,9 +864,7 @@ ISR(TIMER0_OVF_vect)
|
|||||||
TIMSK0 &= ~(1<<TOIE0) ;
|
TIMSK0 &= ~(1<<TOIE0) ;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
#endif // BASH_SERIAL
|
#endif // BASH_SERIAL
|
||||||
|
|
||||||
#endif // TELEMETRY
|
#endif // TELEMETRY
|
||||||
|
@ -15,6 +15,8 @@
|
|||||||
// compatible with WLToys V2x2, JXD JD38x, JD39x, JJRC H6C, Yizhan Tarantula X6 ...
|
// compatible with WLToys V2x2, JXD JD38x, JD39x, JJRC H6C, Yizhan Tarantula X6 ...
|
||||||
// Last sync with hexfet new_protocols/v202_nrf24l01.c dated 2015-03-15
|
// Last sync with hexfet new_protocols/v202_nrf24l01.c dated 2015-03-15
|
||||||
|
|
||||||
|
// ajout jdx506 : https://github.com/DeviationTX/deviation/compare/master...goebish:protocol_jxd_506
|
||||||
|
|
||||||
#if defined(V2X2_NRF24L01_INO)
|
#if defined(V2X2_NRF24L01_INO)
|
||||||
|
|
||||||
|
|
||||||
@ -40,7 +42,12 @@ enum {
|
|||||||
// flags going to byte 10
|
// flags going to byte 10
|
||||||
V2X2_FLAG_HEADLESS = 0x02,
|
V2X2_FLAG_HEADLESS = 0x02,
|
||||||
V2X2_FLAG_MAG_CAL_X = 0x08,
|
V2X2_FLAG_MAG_CAL_X = 0x08,
|
||||||
V2X2_FLAG_MAG_CAL_Y = 0x20
|
V2X2_FLAG_MAG_CAL_Y = 0x20,
|
||||||
|
//
|
||||||
|
JXD_FLAG_START_STOP= 0x40, // arm / disarm JXD-506
|
||||||
|
JXD_FLAG_EMERGENCY = 0x80, // JXD-506
|
||||||
|
JXD_FLAG_CAMERA_UP = 0x01, // JXD-506
|
||||||
|
JXD_FLAG_CAMERA_DN = 0x02, // JXD-506
|
||||||
};
|
};
|
||||||
|
|
||||||
//
|
//
|
||||||
@ -182,6 +189,22 @@ static void __attribute__((unused)) V2X2_send_packet(uint8_t bind)
|
|||||||
// Channel 9
|
// Channel 9
|
||||||
if (Servo_AUX5)
|
if (Servo_AUX5)
|
||||||
flags2 = V2X2_FLAG_HEADLESS;
|
flags2 = V2X2_FLAG_HEADLESS;
|
||||||
|
|
||||||
|
if(sub_protocol == FORMAT_JXD506) {
|
||||||
|
// Channel 10
|
||||||
|
if (Servo_AUX6) flags2 |= JXD_FLAG_START_STOP;
|
||||||
|
// Channel 11
|
||||||
|
if (Servo_AUX7) flags2 |= JXD_FLAG_EMERGENCY;
|
||||||
|
|
||||||
|
/* // Channel 12 down
|
||||||
|
if (num_channels < 11 || Channels[CHANNEL11] >= CHAN_MIN_VALUE/2) *flags2 &= ~FLAG_CAMERA_DN;
|
||||||
|
else *flags2 |= JXD_FLAG_CAMERA_DN;
|
||||||
|
|
||||||
|
// Channel 12 up
|
||||||
|
if (num_channels < 11 || Channels[CHANNEL11] <= CHAN_MAX_VALUE/2) *flags2 &= ~FLAG_CAMERA_UP;
|
||||||
|
else *flags2 |= JXD_FLAG_CAMERA_UP;
|
||||||
|
*/
|
||||||
|
} else {
|
||||||
// Channel 10
|
// Channel 10
|
||||||
if (Servo_AUX6)
|
if (Servo_AUX6)
|
||||||
flags2 |= V2X2_FLAG_MAG_CAL_X;
|
flags2 |= V2X2_FLAG_MAG_CAL_X;
|
||||||
@ -189,6 +212,7 @@ static void __attribute__((unused)) V2X2_send_packet(uint8_t bind)
|
|||||||
if (Servo_AUX7)
|
if (Servo_AUX7)
|
||||||
flags2 |= V2X2_FLAG_MAG_CAL_Y;
|
flags2 |= V2X2_FLAG_MAG_CAL_Y;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
// TX id
|
// TX id
|
||||||
packet[7] = rx_tx_addr[1];
|
packet[7] = rx_tx_addr[1];
|
||||||
packet[8] = rx_tx_addr[2];
|
packet[8] = rx_tx_addr[2];
|
||||||
|
96
Multiprotocol/Validate.h
Normal file
96
Multiprotocol/Validate.h
Normal file
@ -0,0 +1,96 @@
|
|||||||
|
// Check selected board type
|
||||||
|
#if defined (STM32_BOARD) && defined (ORANGE_TX)
|
||||||
|
#error You must comment the board type STM32_BOARD in _Config.h to compile ORANGE_TX
|
||||||
|
#endif
|
||||||
|
#if not defined (ORANGE_TX) && not defined (STM32_BOARD)
|
||||||
|
//Atmega328p
|
||||||
|
#if not defined(ARDUINO_AVR_PRO) && not defined(ARDUINO_AVR_MINI) && not defined(ARDUINO_AVR_NANO)
|
||||||
|
#error You must select one of these boards: "Multi 4-in-1", "Arduino Pro or Pro Mini" or "Arduino Mini"
|
||||||
|
#endif
|
||||||
|
#if F_CPU != 16000000L || not defined(__AVR_ATmega328P__)
|
||||||
|
#error You must select the processor type "ATmega328(5V, 16MHz)"
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
#if defined (STM32_BOARD) && not defined (ORANGE_TX)
|
||||||
|
//STM32
|
||||||
|
#ifndef ARDUINO_GENERIC_STM32F103C
|
||||||
|
#error You must select the board type "Generic STM32F103C series"
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//Change/Force configuration if OrangeTX
|
||||||
|
#ifdef ORANGE_TX
|
||||||
|
#undef ENABLE_PPM // Disable PPM for OrangeTX module
|
||||||
|
#undef A7105_INSTALLED // Disable A7105 for OrangeTX module
|
||||||
|
#undef CC2500_INSTALLED // Disable CC2500 for OrangeTX module
|
||||||
|
#undef NRF24L01_INSTALLED // Disable NRF for OrangeTX module
|
||||||
|
#define TELEMETRY // Enable telemetry
|
||||||
|
#define INVERT_TELEMETRY // Enable invert telemetry
|
||||||
|
#define DSM_TELEMETRY // Enable DSM telemetry
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//Make sure protocols are selected correctly
|
||||||
|
#ifndef A7105_INSTALLED
|
||||||
|
#undef FLYSKY_A7105_INO
|
||||||
|
#undef HUBSAN_A7105_INO
|
||||||
|
#endif
|
||||||
|
#ifndef CYRF6936_INSTALLED
|
||||||
|
#undef DEVO_CYRF6936_INO
|
||||||
|
#undef DSM_CYRF6936_INO
|
||||||
|
#undef J6PRO_CYRF6936_INO
|
||||||
|
#endif
|
||||||
|
#ifndef CC2500_INSTALLED
|
||||||
|
#undef FRSKYD_CC2500_INO
|
||||||
|
#undef FRSKYV_CC2500_INO
|
||||||
|
#undef FRSKYX_CC2500_INO
|
||||||
|
#undef SFHSS_CC2500_INO
|
||||||
|
#endif
|
||||||
|
#ifndef NRF24L01_INSTALLED
|
||||||
|
#undef BAYANG_NRF24L01_INO
|
||||||
|
#undef CG023_NRF24L01_INO
|
||||||
|
#undef CX10_NRF24L01_INO
|
||||||
|
#undef ESKY_NRF24L01_INO
|
||||||
|
#undef HISKY_NRF24L01_INO
|
||||||
|
#undef KN_NRF24L01_INO
|
||||||
|
#undef SLT_NRF24L01_INO
|
||||||
|
#undef SYMAX_NRF24L01_INO
|
||||||
|
#undef V2X2_NRF24L01_INO
|
||||||
|
#undef YD717_NRF24L01_INO
|
||||||
|
#undef MT99XX_NRF24L01_INO
|
||||||
|
#undef MJXQ_NRF24L01_INO
|
||||||
|
#undef SHENQI_NRF24L01_INO
|
||||||
|
#undef FY326_NRF24L01_INO
|
||||||
|
#undef FQ777_NRF24L01_INO
|
||||||
|
#undef ASSAN_NRF24L01_INO
|
||||||
|
#undef HONTAI_NRF24L01_INO
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//Make sure telemetry is selected correctly
|
||||||
|
#ifndef TELEMETRY
|
||||||
|
#undef INVERT_TELEMETRY
|
||||||
|
#undef DSM_TELEMETRY
|
||||||
|
#undef SPORT_TELEMETRY
|
||||||
|
#undef HUB_TELEMETRY
|
||||||
|
#else
|
||||||
|
#if not defined(CYRF6936_INSTALLED) || not defined(DSM_CYRF6936_INO)
|
||||||
|
#undef DSM_TELEMETRY
|
||||||
|
#endif
|
||||||
|
#if (not defined(CC2500_INSTALLED) || not defined(FRSKYD_CC2500_INO)) && (not defined(A7105_INSTALLED) || not defined(HUBSAN_A7105_INO))
|
||||||
|
#undef HUB_TELEMETRY
|
||||||
|
#endif
|
||||||
|
#if not defined(CC2500_INSTALLED) || not defined(FRSKYX_CC2500_INO)
|
||||||
|
#undef SPORT_TELEMETRY
|
||||||
|
#endif
|
||||||
|
#if not defined(DSM_TELEMETRY) && not defined(HUB_TELEMETRY) && not defined(SPORT_TELEMETRY)
|
||||||
|
#undef TELEMETRY
|
||||||
|
#undef INVERT_TELEMETRY
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//Make sure TX is defined correctly
|
||||||
|
#ifndef AILERON
|
||||||
|
#error You must select a correct channel order.
|
||||||
|
#endif
|
||||||
|
#if not defined(PPM_MAX_100) || not defined(PPM_MIN_100) || not defined(PPM_MAX_125) || not defined(PPM_MIN_125)
|
||||||
|
#error You must set correct TX end points.
|
||||||
|
#endif
|
@ -17,6 +17,14 @@
|
|||||||
/** Multiprotocol module configuration file ***/
|
/** Multiprotocol module configuration file ***/
|
||||||
/**********************************************/
|
/**********************************************/
|
||||||
|
|
||||||
|
/********************/
|
||||||
|
/*** BOARD TYPE ***/
|
||||||
|
/********************/
|
||||||
|
//Uncomment one of the line below if you have a different module not based on the original Multi Atmega328p design which includes the 4-in-1.
|
||||||
|
//If you don't know then leave them commented.
|
||||||
|
//#define STM32_BOARD
|
||||||
|
|
||||||
|
|
||||||
/*******************/
|
/*******************/
|
||||||
/*** TX SETTINGS ***/
|
/*** TX SETTINGS ***/
|
||||||
/*******************/
|
/*******************/
|
||||||
@ -43,7 +51,7 @@
|
|||||||
/*** PROTOCOLS TO INCLUDE ***/
|
/*** PROTOCOLS TO INCLUDE ***/
|
||||||
/****************************/
|
/****************************/
|
||||||
//In this section select the protocols you want to be accessible when using the module.
|
//In this section select the protocols you want to be accessible when using the module.
|
||||||
//All the protocols will not fit in the module so you need to pick and choose.
|
//All the protocols will not fit in the Atmega328p module so you need to pick and choose.
|
||||||
//Comment the protocols you are not using with "//" to save Flash space.
|
//Comment the protocols you are not using with "//" to save Flash space.
|
||||||
|
|
||||||
//The protocols below need an A7105 to be installed
|
//The protocols below need an A7105 to be installed
|
||||||
@ -146,9 +154,9 @@
|
|||||||
//#define TX_CUSTOM //Custom
|
//#define TX_CUSTOM //Custom
|
||||||
|
|
||||||
// The lines below are used to set the end points in microseconds (µs) if you have selected TX_CUSTOM.
|
// The lines below are used to set the end points in microseconds (µs) if you have selected TX_CUSTOM.
|
||||||
// A few things to considered:
|
// A few things to consider:
|
||||||
// - If you put too big values compared to your TX you won't be able to reach the extremes which is bad for throttle as an example
|
// - If you put too big values compared to your TX you won't be able to reach the extremes which is bad for throttle as an example
|
||||||
// - If you put too low values you won't be able to use your full stick range, it will be maxed out before reaching the end
|
// - If you put too low values you won't be able to use your full stick range, it will be maxed out before reaching the ends
|
||||||
// - Centered stick value is usually 1500. It should match the middle between MIN and MAX, ie Center=(MAX-MIN)/2+MIN. If your TX is not centered you can adjust the value MIN or MAX.
|
// - Centered stick value is usually 1500. It should match the middle between MIN and MAX, ie Center=(MAX-MIN)/2+MIN. If your TX is not centered you can adjust the value MIN or MAX.
|
||||||
// - 100% is the value when the model is by default, 125% is the value when you extend the servo travel which is only used by some protocols
|
// - 100% is the value when the model is by default, 125% is the value when you extend the servo travel which is only used by some protocols
|
||||||
#if defined(TX_CUSTOM)
|
#if defined(TX_CUSTOM)
|
||||||
@ -276,7 +284,7 @@ const PPM_Parameters PPM_prot[15]= {
|
|||||||
|
|
||||||
// Auto Bind AUTOBIND or NO_AUTOBIND
|
// Auto Bind AUTOBIND or NO_AUTOBIND
|
||||||
// For protocols which does not require binding at each power up (like Flysky, FrSky...), you might still want a bind to be initiated each time you power up the TX.
|
// For protocols which does not require binding at each power up (like Flysky, FrSky...), you might still want a bind to be initiated each time you power up the TX.
|
||||||
// As an exxample, it's usefull for the WLTOYS F929/F939/F949/F959 (all using the Flysky protocol) which requires a bind at each power up.
|
// As an example, it's usefull for the WLTOYS F929/F939/F949/F959 (all using the Flysky protocol) which requires a bind at each power up.
|
||||||
|
|
||||||
// Option: the value is between -127 and +127.
|
// Option: the value is between -127 and +127.
|
||||||
// The option value is only valid for some protocols, read this page for more information: https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/blob/master/Protocols_Details.md
|
// The option value is only valid for some protocols, read this page for more information: https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/blob/master/Protocols_Details.md
|
890
Multiprotocol/boards.txt
Normal file
890
Multiprotocol/boards.txt
Normal file
@ -0,0 +1,890 @@
|
|||||||
|
# See: http://code.google.com/p/arduino/wiki/Platforms
|
||||||
|
|
||||||
|
menu.cpu=Processor
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
yun.name=Arduino Yún
|
||||||
|
yun.upload.via_ssh=true
|
||||||
|
|
||||||
|
yun.vid.0=0x2341
|
||||||
|
yun.pid.0=0x0041
|
||||||
|
yun.vid.1=0x2341
|
||||||
|
yun.pid.1=0x8041
|
||||||
|
yun.vid.2=0x2A03
|
||||||
|
yun.pid.2=0x0041
|
||||||
|
yun.vid.3=0x2A03
|
||||||
|
yun.pid.3=0x8041
|
||||||
|
|
||||||
|
yun.upload.tool=avrdude
|
||||||
|
yun.upload.protocol=avr109
|
||||||
|
yun.upload.maximum_size=28672
|
||||||
|
yun.upload.maximum_data_size=2560
|
||||||
|
yun.upload.speed=57600
|
||||||
|
yun.upload.disable_flushing=true
|
||||||
|
yun.upload.use_1200bps_touch=true
|
||||||
|
yun.upload.wait_for_upload_port=true
|
||||||
|
|
||||||
|
yun.bootloader.tool=avrdude
|
||||||
|
yun.bootloader.low_fuses=0xff
|
||||||
|
yun.bootloader.high_fuses=0xd8
|
||||||
|
yun.bootloader.extended_fuses=0xfb
|
||||||
|
yun.bootloader.file=caterina/Caterina-Yun.hex
|
||||||
|
yun.bootloader.unlock_bits=0x3F
|
||||||
|
yun.bootloader.lock_bits=0x2F
|
||||||
|
|
||||||
|
yun.build.mcu=atmega32u4
|
||||||
|
yun.build.f_cpu=16000000L
|
||||||
|
yun.build.vid=0x2341
|
||||||
|
yun.build.pid=0x8041
|
||||||
|
yun.build.usb_product="Arduino Yun"
|
||||||
|
yun.build.board=AVR_YUN
|
||||||
|
yun.build.core=arduino
|
||||||
|
yun.build.variant=yun
|
||||||
|
yun.build.extra_flags={build.usb_flags}
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
uno.name=Arduino/Genuino Uno
|
||||||
|
|
||||||
|
uno.vid.0=0x2341
|
||||||
|
uno.pid.0=0x0043
|
||||||
|
uno.vid.1=0x2341
|
||||||
|
uno.pid.1=0x0001
|
||||||
|
uno.vid.2=0x2A03
|
||||||
|
uno.pid.2=0x0043
|
||||||
|
uno.vid.3=0x2341
|
||||||
|
uno.pid.3=0x0243
|
||||||
|
|
||||||
|
uno.upload.tool=avrdude
|
||||||
|
uno.upload.protocol=arduino
|
||||||
|
uno.upload.maximum_size=32256
|
||||||
|
uno.upload.maximum_data_size=2048
|
||||||
|
uno.upload.speed=115200
|
||||||
|
|
||||||
|
uno.bootloader.tool=avrdude
|
||||||
|
uno.bootloader.low_fuses=0xFF
|
||||||
|
uno.bootloader.high_fuses=0xDE
|
||||||
|
uno.bootloader.extended_fuses=0x05
|
||||||
|
uno.bootloader.unlock_bits=0x3F
|
||||||
|
uno.bootloader.lock_bits=0x0F
|
||||||
|
uno.bootloader.file=optiboot/optiboot_atmega328.hex
|
||||||
|
|
||||||
|
uno.build.mcu=atmega328p
|
||||||
|
uno.build.f_cpu=16000000L
|
||||||
|
uno.build.board=AVR_UNO
|
||||||
|
uno.build.core=arduino
|
||||||
|
uno.build.variant=standard
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
diecimila.name=Arduino Duemilanove or Diecimila
|
||||||
|
|
||||||
|
diecimila.upload.tool=avrdude
|
||||||
|
diecimila.upload.protocol=arduino
|
||||||
|
|
||||||
|
diecimila.bootloader.tool=avrdude
|
||||||
|
diecimila.bootloader.low_fuses=0xFF
|
||||||
|
diecimila.bootloader.unlock_bits=0x3F
|
||||||
|
diecimila.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
diecimila.build.f_cpu=16000000L
|
||||||
|
diecimila.build.board=AVR_DUEMILANOVE
|
||||||
|
diecimila.build.core=arduino
|
||||||
|
diecimila.build.variant=standard
|
||||||
|
|
||||||
|
## Arduino Duemilanove or Diecimila w/ ATmega328
|
||||||
|
## ---------------------------------------------
|
||||||
|
diecimila.menu.cpu.atmega328=ATmega328
|
||||||
|
|
||||||
|
diecimila.menu.cpu.atmega328.upload.maximum_size=30720
|
||||||
|
diecimila.menu.cpu.atmega328.upload.maximum_data_size=2048
|
||||||
|
diecimila.menu.cpu.atmega328.upload.speed=57600
|
||||||
|
|
||||||
|
diecimila.menu.cpu.atmega328.bootloader.high_fuses=0xDA
|
||||||
|
diecimila.menu.cpu.atmega328.bootloader.extended_fuses=0x05
|
||||||
|
diecimila.menu.cpu.atmega328.bootloader.file=atmega/ATmegaBOOT_168_atmega328.hex
|
||||||
|
|
||||||
|
diecimila.menu.cpu.atmega328.build.mcu=atmega328p
|
||||||
|
|
||||||
|
## Arduino Duemilanove or Diecimila w/ ATmega168
|
||||||
|
## ---------------------------------------------
|
||||||
|
diecimila.menu.cpu.atmega168=ATmega168
|
||||||
|
|
||||||
|
diecimila.menu.cpu.atmega168.upload.maximum_size=14336
|
||||||
|
diecimila.menu.cpu.atmega168.upload.maximum_data_size=1024
|
||||||
|
diecimila.menu.cpu.atmega168.upload.speed=19200
|
||||||
|
|
||||||
|
diecimila.menu.cpu.atmega168.bootloader.high_fuses=0xdd
|
||||||
|
diecimila.menu.cpu.atmega168.bootloader.extended_fuses=0x00
|
||||||
|
diecimila.menu.cpu.atmega168.bootloader.file=atmega/ATmegaBOOT_168_diecimila.hex
|
||||||
|
|
||||||
|
diecimila.menu.cpu.atmega168.build.mcu=atmega168
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
nano.name=Arduino Nano
|
||||||
|
|
||||||
|
nano.upload.tool=avrdude
|
||||||
|
nano.upload.protocol=arduino
|
||||||
|
|
||||||
|
nano.bootloader.tool=avrdude
|
||||||
|
nano.bootloader.unlock_bits=0x3F
|
||||||
|
nano.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
nano.build.f_cpu=16000000L
|
||||||
|
nano.build.board=AVR_NANO
|
||||||
|
nano.build.core=arduino
|
||||||
|
nano.build.variant=eightanaloginputs
|
||||||
|
|
||||||
|
## Arduino Nano w/ ATmega328
|
||||||
|
## -------------------------
|
||||||
|
nano.menu.cpu.atmega328=ATmega328
|
||||||
|
|
||||||
|
nano.menu.cpu.atmega328.upload.maximum_size=30720
|
||||||
|
nano.menu.cpu.atmega328.upload.maximum_data_size=2048
|
||||||
|
nano.menu.cpu.atmega328.upload.speed=57600
|
||||||
|
|
||||||
|
nano.menu.cpu.atmega328.bootloader.low_fuses=0xFF
|
||||||
|
nano.menu.cpu.atmega328.bootloader.high_fuses=0xDA
|
||||||
|
nano.menu.cpu.atmega328.bootloader.extended_fuses=0x05
|
||||||
|
nano.menu.cpu.atmega328.bootloader.file=atmega/ATmegaBOOT_168_atmega328.hex
|
||||||
|
|
||||||
|
nano.menu.cpu.atmega328.build.mcu=atmega328p
|
||||||
|
|
||||||
|
## Arduino Nano w/ ATmega168
|
||||||
|
## -------------------------
|
||||||
|
nano.menu.cpu.atmega168=ATmega168
|
||||||
|
|
||||||
|
nano.menu.cpu.atmega168.upload.maximum_size=14336
|
||||||
|
nano.menu.cpu.atmega168.upload.maximum_data_size=1024
|
||||||
|
nano.menu.cpu.atmega168.upload.speed=19200
|
||||||
|
|
||||||
|
nano.menu.cpu.atmega168.bootloader.low_fuses=0xff
|
||||||
|
nano.menu.cpu.atmega168.bootloader.high_fuses=0xdd
|
||||||
|
nano.menu.cpu.atmega168.bootloader.extended_fuses=0x00
|
||||||
|
nano.menu.cpu.atmega168.bootloader.file=atmega/ATmegaBOOT_168_diecimila.hex
|
||||||
|
|
||||||
|
nano.menu.cpu.atmega168.build.mcu=atmega168
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
mega.name=Arduino/Genuino Mega or Mega 2560
|
||||||
|
|
||||||
|
mega.vid.0=0x2341
|
||||||
|
mega.pid.0=0x0010
|
||||||
|
mega.vid.1=0x2341
|
||||||
|
mega.pid.1=0x0042
|
||||||
|
mega.vid.2=0x2A03
|
||||||
|
mega.pid.2=0x0010
|
||||||
|
mega.vid.3=0x2A03
|
||||||
|
mega.pid.3=0x0042
|
||||||
|
mega.vid.4=0x2341
|
||||||
|
mega.pid.4=0x0210
|
||||||
|
mega.vid.5=0x2341
|
||||||
|
mega.pid.5=0x0242
|
||||||
|
|
||||||
|
mega.upload.tool=avrdude
|
||||||
|
mega.upload.maximum_data_size=8192
|
||||||
|
|
||||||
|
mega.bootloader.tool=avrdude
|
||||||
|
mega.bootloader.low_fuses=0xFF
|
||||||
|
mega.bootloader.unlock_bits=0x3F
|
||||||
|
mega.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
mega.build.f_cpu=16000000L
|
||||||
|
mega.build.core=arduino
|
||||||
|
mega.build.variant=mega
|
||||||
|
# default board may be overridden by the cpu menu
|
||||||
|
mega.build.board=AVR_MEGA2560
|
||||||
|
|
||||||
|
## Arduino/Genuino Mega w/ ATmega2560
|
||||||
|
## -------------------------
|
||||||
|
mega.menu.cpu.atmega2560=ATmega2560 (Mega 2560)
|
||||||
|
|
||||||
|
mega.menu.cpu.atmega2560.upload.protocol=wiring
|
||||||
|
mega.menu.cpu.atmega2560.upload.maximum_size=253952
|
||||||
|
mega.menu.cpu.atmega2560.upload.speed=115200
|
||||||
|
|
||||||
|
mega.menu.cpu.atmega2560.bootloader.high_fuses=0xD8
|
||||||
|
mega.menu.cpu.atmega2560.bootloader.extended_fuses=0xFD
|
||||||
|
mega.menu.cpu.atmega2560.bootloader.file=stk500v2/stk500boot_v2_mega2560.hex
|
||||||
|
|
||||||
|
mega.menu.cpu.atmega2560.build.mcu=atmega2560
|
||||||
|
mega.menu.cpu.atmega2560.build.board=AVR_MEGA2560
|
||||||
|
|
||||||
|
## Arduino Mega w/ ATmega1280
|
||||||
|
## -------------------------
|
||||||
|
mega.menu.cpu.atmega1280=ATmega1280
|
||||||
|
|
||||||
|
mega.menu.cpu.atmega1280.upload.protocol=arduino
|
||||||
|
mega.menu.cpu.atmega1280.upload.maximum_size=126976
|
||||||
|
mega.menu.cpu.atmega1280.upload.speed=57600
|
||||||
|
|
||||||
|
mega.menu.cpu.atmega1280.bootloader.high_fuses=0xDA
|
||||||
|
mega.menu.cpu.atmega1280.bootloader.extended_fuses=0xF5
|
||||||
|
mega.menu.cpu.atmega1280.bootloader.file=atmega/ATmegaBOOT_168_atmega1280.hex
|
||||||
|
|
||||||
|
mega.menu.cpu.atmega1280.build.mcu=atmega1280
|
||||||
|
mega.menu.cpu.atmega1280.build.board=AVR_MEGA
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
megaADK.name=Arduino Mega ADK
|
||||||
|
|
||||||
|
megaADK.vid.0=0x2341
|
||||||
|
megaADK.pid.0=0x003f
|
||||||
|
megaADK.vid.1=0x2341
|
||||||
|
megaADK.pid.1=0x0044
|
||||||
|
megaADK.vid.2=0x2A03
|
||||||
|
megaADK.pid.2=0x003f
|
||||||
|
megaADK.vid.3=0x2A03
|
||||||
|
megaADK.pid.3=0x0044
|
||||||
|
|
||||||
|
megaADK.upload.tool=avrdude
|
||||||
|
megaADK.upload.protocol=wiring
|
||||||
|
megaADK.upload.maximum_size=253952
|
||||||
|
megaADK.upload.maximum_data_size=8192
|
||||||
|
megaADK.upload.speed=115200
|
||||||
|
|
||||||
|
megaADK.bootloader.tool=avrdude
|
||||||
|
megaADK.bootloader.low_fuses=0xFF
|
||||||
|
megaADK.bootloader.high_fuses=0xD8
|
||||||
|
megaADK.bootloader.extended_fuses=0xFD
|
||||||
|
megaADK.bootloader.file=stk500v2/stk500boot_v2_mega2560.hex
|
||||||
|
megaADK.bootloader.unlock_bits=0x3F
|
||||||
|
megaADK.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
megaADK.build.mcu=atmega2560
|
||||||
|
megaADK.build.f_cpu=16000000L
|
||||||
|
megaADK.build.board=AVR_ADK
|
||||||
|
megaADK.build.core=arduino
|
||||||
|
megaADK.build.variant=mega
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
leonardo.name=Arduino Leonardo
|
||||||
|
leonardo.vid.0=0x2341
|
||||||
|
leonardo.pid.0=0x0036
|
||||||
|
leonardo.vid.1=0x2341
|
||||||
|
leonardo.pid.1=0x8036
|
||||||
|
leonardo.vid.2=0x2A03
|
||||||
|
leonardo.pid.2=0x0036
|
||||||
|
leonardo.vid.3=0x2A03
|
||||||
|
leonardo.pid.3=0x8036
|
||||||
|
|
||||||
|
leonardo.upload.tool=avrdude
|
||||||
|
leonardo.upload.protocol=avr109
|
||||||
|
leonardo.upload.maximum_size=28672
|
||||||
|
leonardo.upload.maximum_data_size=2560
|
||||||
|
leonardo.upload.speed=57600
|
||||||
|
leonardo.upload.disable_flushing=true
|
||||||
|
leonardo.upload.use_1200bps_touch=true
|
||||||
|
leonardo.upload.wait_for_upload_port=true
|
||||||
|
|
||||||
|
leonardo.bootloader.tool=avrdude
|
||||||
|
leonardo.bootloader.low_fuses=0xff
|
||||||
|
leonardo.bootloader.high_fuses=0xd8
|
||||||
|
leonardo.bootloader.extended_fuses=0xcb
|
||||||
|
leonardo.bootloader.file=caterina/Caterina-Leonardo.hex
|
||||||
|
leonardo.bootloader.unlock_bits=0x3F
|
||||||
|
leonardo.bootloader.lock_bits=0x2F
|
||||||
|
|
||||||
|
leonardo.build.mcu=atmega32u4
|
||||||
|
leonardo.build.f_cpu=16000000L
|
||||||
|
leonardo.build.vid=0x2341
|
||||||
|
leonardo.build.pid=0x8036
|
||||||
|
leonardo.build.usb_product="Arduino Leonardo"
|
||||||
|
leonardo.build.board=AVR_LEONARDO
|
||||||
|
leonardo.build.core=arduino
|
||||||
|
leonardo.build.variant=leonardo
|
||||||
|
leonardo.build.extra_flags={build.usb_flags}
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
micro.name=Arduino/Genuino Micro
|
||||||
|
|
||||||
|
micro.vid.0=0x2341
|
||||||
|
micro.pid.0=0x0037
|
||||||
|
micro.vid.1=0x2341
|
||||||
|
micro.pid.1=0x8037
|
||||||
|
micro.vid.2=0x2A03
|
||||||
|
micro.pid.2=0x0037
|
||||||
|
micro.vid.3=0x2A03
|
||||||
|
micro.pid.3=0x8037
|
||||||
|
|
||||||
|
micro.vid.4=0x2341
|
||||||
|
micro.pid.4=0x0237
|
||||||
|
# If the board is a 2341:0237 use 2341:8237 for build and set
|
||||||
|
# other parameters as well
|
||||||
|
micro.vid.4.build.vid=0x2341
|
||||||
|
micro.vid.4.build.pid=0x8237
|
||||||
|
micro.vid.4.build.usb_product="Genuino Micro"
|
||||||
|
micro.vid.4.bootloader.file=caterina/Caterina-Genuino-Micro.hex
|
||||||
|
|
||||||
|
micro.vid.5=0x2341
|
||||||
|
micro.pid.5=0x8237
|
||||||
|
# If the board is a 2341:8237 use 2341:8237 for build and set
|
||||||
|
# other paramters as well
|
||||||
|
micro.vid.5.build.vid=0x2341
|
||||||
|
micro.vid.5.build.pid=0x8237
|
||||||
|
micro.vid.5.build.usb_product="Genuino Micro"
|
||||||
|
micro.vid.5.bootloader.file=caterina/Caterina-Genuino-Micro.hex
|
||||||
|
|
||||||
|
micro.upload.tool=avrdude
|
||||||
|
micro.upload.protocol=avr109
|
||||||
|
micro.upload.maximum_size=28672
|
||||||
|
micro.upload.maximum_data_size=2560
|
||||||
|
micro.upload.speed=57600
|
||||||
|
micro.upload.disable_flushing=true
|
||||||
|
micro.upload.use_1200bps_touch=true
|
||||||
|
micro.upload.wait_for_upload_port=true
|
||||||
|
|
||||||
|
micro.bootloader.tool=avrdude
|
||||||
|
micro.bootloader.low_fuses=0xff
|
||||||
|
micro.bootloader.high_fuses=0xd8
|
||||||
|
micro.bootloader.extended_fuses=0xcb
|
||||||
|
micro.bootloader.file=caterina/Caterina-Micro.hex
|
||||||
|
micro.bootloader.unlock_bits=0x3F
|
||||||
|
micro.bootloader.lock_bits=0x2F
|
||||||
|
|
||||||
|
micro.build.mcu=atmega32u4
|
||||||
|
micro.build.f_cpu=16000000L
|
||||||
|
micro.build.vid=0x2341
|
||||||
|
micro.build.pid=0x8037
|
||||||
|
micro.build.usb_product="Arduino Micro"
|
||||||
|
micro.build.board=AVR_MICRO
|
||||||
|
micro.build.core=arduino
|
||||||
|
micro.build.variant=micro
|
||||||
|
micro.build.extra_flags={build.usb_flags}
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
esplora.name=Arduino Esplora
|
||||||
|
esplora.vid.0=0x2341
|
||||||
|
esplora.pid.0=0x003C
|
||||||
|
esplora.vid.1=0x2341
|
||||||
|
esplora.pid.1=0x803C
|
||||||
|
esplora.vid.2=0x2A03
|
||||||
|
esplora.pid.2=0x003C
|
||||||
|
esplora.vid.3=0x2A03
|
||||||
|
esplora.pid.3=0x803C
|
||||||
|
|
||||||
|
esplora.upload.tool=avrdude
|
||||||
|
esplora.upload.protocol=avr109
|
||||||
|
esplora.upload.maximum_size=28672
|
||||||
|
esplora.upload.maximum_data_size=2560
|
||||||
|
esplora.upload.speed=57600
|
||||||
|
esplora.upload.disable_flushing=true
|
||||||
|
esplora.upload.use_1200bps_touch=true
|
||||||
|
esplora.upload.wait_for_upload_port=true
|
||||||
|
|
||||||
|
esplora.bootloader.tool=avrdude
|
||||||
|
esplora.bootloader.low_fuses=0xff
|
||||||
|
esplora.bootloader.high_fuses=0xd8
|
||||||
|
esplora.bootloader.extended_fuses=0xcb
|
||||||
|
esplora.bootloader.file=caterina/Caterina-Esplora.hex
|
||||||
|
esplora.bootloader.unlock_bits=0x3F
|
||||||
|
esplora.bootloader.lock_bits=0x2F
|
||||||
|
|
||||||
|
esplora.build.mcu=atmega32u4
|
||||||
|
esplora.build.f_cpu=16000000L
|
||||||
|
esplora.build.vid=0x2341
|
||||||
|
esplora.build.pid=0x803c
|
||||||
|
esplora.build.usb_product="Arduino Esplora"
|
||||||
|
esplora.build.board=AVR_ESPLORA
|
||||||
|
esplora.build.core=arduino
|
||||||
|
esplora.build.variant=leonardo
|
||||||
|
esplora.build.extra_flags={build.usb_flags}
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
mini.name=Arduino Mini
|
||||||
|
|
||||||
|
mini.upload.tool=avrdude
|
||||||
|
mini.upload.protocol=arduino
|
||||||
|
|
||||||
|
mini.bootloader.tool=avrdude
|
||||||
|
mini.bootloader.low_fuses=0xff
|
||||||
|
mini.bootloader.unlock_bits=0x3F
|
||||||
|
mini.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
mini.build.f_cpu=16000000L
|
||||||
|
mini.build.board=AVR_MINI
|
||||||
|
mini.build.core=arduino
|
||||||
|
mini.build.variant=eightanaloginputs
|
||||||
|
|
||||||
|
## Arduino Mini w/ ATmega328
|
||||||
|
## -------------------------
|
||||||
|
mini.menu.cpu.atmega328=ATmega328
|
||||||
|
|
||||||
|
mini.menu.cpu.atmega328.upload.maximum_size=28672
|
||||||
|
mini.menu.cpu.atmega328.upload.maximum_data_size=2048
|
||||||
|
mini.menu.cpu.atmega328.upload.speed=115200
|
||||||
|
|
||||||
|
mini.menu.cpu.atmega328.bootloader.high_fuses=0xd8
|
||||||
|
mini.menu.cpu.atmega328.bootloader.extended_fuses=0x05
|
||||||
|
mini.menu.cpu.atmega328.bootloader.file=optiboot/optiboot_atmega328-Mini.hex
|
||||||
|
|
||||||
|
mini.menu.cpu.atmega328.build.mcu=atmega328p
|
||||||
|
|
||||||
|
## Arduino Mini w/ ATmega168
|
||||||
|
## -------------------------
|
||||||
|
mini.menu.cpu.atmega168=ATmega168
|
||||||
|
|
||||||
|
mini.menu.cpu.atmega168.upload.maximum_size=14336
|
||||||
|
mini.menu.cpu.atmega168.upload.maximum_data_size=1024
|
||||||
|
mini.menu.cpu.atmega168.upload.speed=19200
|
||||||
|
|
||||||
|
mini.menu.cpu.atmega168.bootloader.high_fuses=0xdd
|
||||||
|
mini.menu.cpu.atmega168.bootloader.extended_fuses=0x00
|
||||||
|
mini.menu.cpu.atmega168.bootloader.file=atmega/ATmegaBOOT_168_ng.hex
|
||||||
|
|
||||||
|
mini.menu.cpu.atmega168.build.mcu=atmega168
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
ethernet.name=Arduino Ethernet
|
||||||
|
|
||||||
|
ethernet.upload.tool=avrdude
|
||||||
|
ethernet.upload.protocol=arduino
|
||||||
|
ethernet.upload.maximum_size=32256
|
||||||
|
ethernet.upload.maximum_data_size=2048
|
||||||
|
ethernet.upload.speed=115200
|
||||||
|
|
||||||
|
ethernet.bootloader.tool=avrdude
|
||||||
|
ethernet.bootloader.low_fuses=0xff
|
||||||
|
ethernet.bootloader.high_fuses=0xde
|
||||||
|
ethernet.bootloader.extended_fuses=0x05
|
||||||
|
ethernet.bootloader.file=optiboot/optiboot_atmega328.hex
|
||||||
|
ethernet.bootloader.unlock_bits=0x3F
|
||||||
|
ethernet.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
ethernet.build.variant=ethernet
|
||||||
|
ethernet.build.mcu=atmega328p
|
||||||
|
ethernet.build.f_cpu=16000000L
|
||||||
|
ethernet.build.board=AVR_ETHERNET
|
||||||
|
ethernet.build.core=arduino
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
fio.name=Arduino Fio
|
||||||
|
|
||||||
|
fio.upload.tool=avrdude
|
||||||
|
fio.upload.protocol=arduino
|
||||||
|
fio.upload.maximum_size=30720
|
||||||
|
fio.upload.maximum_data_size=2048
|
||||||
|
fio.upload.speed=57600
|
||||||
|
|
||||||
|
fio.bootloader.tool=avrdude
|
||||||
|
fio.bootloader.low_fuses=0xFF
|
||||||
|
fio.bootloader.high_fuses=0xDA
|
||||||
|
fio.bootloader.extended_fuses=0x05
|
||||||
|
fio.bootloader.file=atmega/ATmegaBOOT_168_atmega328_pro_8MHz.hex
|
||||||
|
fio.bootloader.unlock_bits=0x3F
|
||||||
|
fio.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
fio.build.mcu=atmega328p
|
||||||
|
fio.build.f_cpu=8000000L
|
||||||
|
fio.build.board=AVR_FIO
|
||||||
|
fio.build.core=arduino
|
||||||
|
fio.build.variant=eightanaloginputs
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
bt.name=Arduino BT
|
||||||
|
|
||||||
|
bt.upload.tool=avrdude
|
||||||
|
bt.upload.protocol=arduino
|
||||||
|
bt.upload.speed=19200
|
||||||
|
bt.upload.disable_flushing=true
|
||||||
|
|
||||||
|
bt.bootloader.tool=avrdude
|
||||||
|
bt.bootloader.low_fuses=0xff
|
||||||
|
bt.bootloader.unlock_bits=0x3F
|
||||||
|
bt.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
bt.build.f_cpu=16000000L
|
||||||
|
bt.build.board=AVR_BT
|
||||||
|
bt.build.core=arduino
|
||||||
|
bt.build.variant=eightanaloginputs
|
||||||
|
|
||||||
|
## Arduino BT w/ ATmega328
|
||||||
|
## -----------------------
|
||||||
|
bt.menu.cpu.atmega328=ATmega328
|
||||||
|
bt.menu.cpu.atmega328.upload.maximum_size=28672
|
||||||
|
bt.menu.cpu.atmega328.upload.maximum_data_size=2048
|
||||||
|
|
||||||
|
bt.menu.cpu.atmega328.bootloader.high_fuses=0xd8
|
||||||
|
bt.menu.cpu.atmega328.bootloader.extended_fuses=0x05
|
||||||
|
bt.menu.cpu.atmega328.bootloader.file=bt/ATmegaBOOT_168_atmega328_bt.hex
|
||||||
|
|
||||||
|
bt.menu.cpu.atmega328.build.mcu=atmega328p
|
||||||
|
|
||||||
|
## Arduino BT w/ ATmega168
|
||||||
|
## -----------------------
|
||||||
|
bt.menu.cpu.atmega168=ATmega168
|
||||||
|
bt.menu.cpu.atmega168.upload.maximum_size=14336
|
||||||
|
bt.menu.cpu.atmega168.upload.maximum_data_size=1024
|
||||||
|
|
||||||
|
bt.menu.cpu.atmega168.bootloader.high_fuses=0xdd
|
||||||
|
bt.menu.cpu.atmega168.bootloader.extended_fuses=0x00
|
||||||
|
bt.menu.cpu.atmega168.bootloader.file=bt/ATmegaBOOT_168.hex
|
||||||
|
|
||||||
|
bt.menu.cpu.atmega168.build.mcu=atmega168
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
LilyPadUSB.name=LilyPad Arduino USB
|
||||||
|
LilyPadUSB.vid.0=0x1B4F
|
||||||
|
LilyPadUSB.pid.0=0x9207
|
||||||
|
LilyPadUSB.vid.1=0x1B4F
|
||||||
|
LilyPadUSB.pid.1=0x9208
|
||||||
|
|
||||||
|
LilyPadUSB.upload.tool=avrdude
|
||||||
|
LilyPadUSB.upload.protocol=avr109
|
||||||
|
LilyPadUSB.upload.maximum_size=28672
|
||||||
|
LilyPadUSB.upload.maximum_data_size=2560
|
||||||
|
LilyPadUSB.upload.speed=57600
|
||||||
|
LilyPadUSB.upload.disable_flushing=true
|
||||||
|
LilyPadUSB.upload.use_1200bps_touch=true
|
||||||
|
LilyPadUSB.upload.wait_for_upload_port=true
|
||||||
|
|
||||||
|
LilyPadUSB.bootloader.tool=avrdude
|
||||||
|
LilyPadUSB.bootloader.low_fuses=0xff
|
||||||
|
LilyPadUSB.bootloader.high_fuses=0xd8
|
||||||
|
LilyPadUSB.bootloader.extended_fuses=0xce
|
||||||
|
LilyPadUSB.bootloader.file=caterina-LilyPadUSB/Caterina-LilyPadUSB.hex
|
||||||
|
LilyPadUSB.bootloader.unlock_bits=0x3F
|
||||||
|
LilyPadUSB.bootloader.lock_bits=0x2F
|
||||||
|
|
||||||
|
LilyPadUSB.build.mcu=atmega32u4
|
||||||
|
LilyPadUSB.build.f_cpu=8000000L
|
||||||
|
LilyPadUSB.build.vid=0x1B4F
|
||||||
|
LilyPadUSB.build.pid=0x9208
|
||||||
|
LilyPadUSB.build.usb_product="LilyPad USB"
|
||||||
|
LilyPadUSB.build.board=AVR_LILYPAD_USB
|
||||||
|
LilyPadUSB.build.core=arduino
|
||||||
|
LilyPadUSB.build.variant=leonardo
|
||||||
|
LilyPadUSB.build.extra_flags={build.usb_flags}
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
lilypad.name=LilyPad Arduino
|
||||||
|
|
||||||
|
lilypad.upload.tool=avrdude
|
||||||
|
lilypad.upload.protocol=arduino
|
||||||
|
|
||||||
|
lilypad.bootloader.tool=avrdude
|
||||||
|
lilypad.bootloader.unlock_bits=0x3F
|
||||||
|
lilypad.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
lilypad.build.f_cpu=8000000L
|
||||||
|
lilypad.build.board=AVR_LILYPAD
|
||||||
|
lilypad.build.core=arduino
|
||||||
|
lilypad.build.variant=standard
|
||||||
|
|
||||||
|
## LilyPad Arduino w/ ATmega328
|
||||||
|
## ----------------------------
|
||||||
|
lilypad.menu.cpu.atmega328=ATmega328
|
||||||
|
|
||||||
|
lilypad.menu.cpu.atmega328.upload.maximum_size=30720
|
||||||
|
lilypad.menu.cpu.atmega328.upload.maximum_data_size=2048
|
||||||
|
lilypad.menu.cpu.atmega328.upload.speed=57600
|
||||||
|
|
||||||
|
lilypad.menu.cpu.atmega328.bootloader.low_fuses=0xFF
|
||||||
|
lilypad.menu.cpu.atmega328.bootloader.high_fuses=0xDA
|
||||||
|
lilypad.menu.cpu.atmega328.bootloader.extended_fuses=0x05
|
||||||
|
lilypad.menu.cpu.atmega328.bootloader.file=atmega/ATmegaBOOT_168_atmega328_pro_8MHz.hex
|
||||||
|
|
||||||
|
lilypad.menu.cpu.atmega328.build.mcu=atmega328p
|
||||||
|
|
||||||
|
## LilyPad Arduino w/ ATmega168
|
||||||
|
## ----------------------------
|
||||||
|
lilypad.menu.cpu.atmega168=ATmega168
|
||||||
|
|
||||||
|
lilypad.menu.cpu.atmega168.upload.maximum_size=14336
|
||||||
|
lilypad.menu.cpu.atmega168.upload.maximum_data_size=1024
|
||||||
|
lilypad.menu.cpu.atmega168.upload.speed=19200
|
||||||
|
|
||||||
|
lilypad.menu.cpu.atmega168.bootloader.low_fuses=0xe2
|
||||||
|
lilypad.menu.cpu.atmega168.bootloader.high_fuses=0xdd
|
||||||
|
lilypad.menu.cpu.atmega168.bootloader.extended_fuses=0x00
|
||||||
|
lilypad.menu.cpu.atmega168.bootloader.file=lilypad/LilyPadBOOT_168.hex
|
||||||
|
|
||||||
|
lilypad.menu.cpu.atmega168.build.mcu=atmega168
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
pro.name=Arduino Pro or Pro Mini
|
||||||
|
|
||||||
|
pro.upload.tool=avrdude
|
||||||
|
pro.upload.protocol=arduino
|
||||||
|
|
||||||
|
pro.bootloader.tool=avrdude
|
||||||
|
pro.bootloader.unlock_bits=0x3F
|
||||||
|
pro.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
pro.build.board=AVR_PRO
|
||||||
|
pro.build.core=arduino
|
||||||
|
pro.build.variant=eightanaloginputs
|
||||||
|
|
||||||
|
## Arduino Pro or Pro Mini (5V, 16 MHz) w/ ATmega328
|
||||||
|
## -------------------------------------------------
|
||||||
|
pro.menu.cpu.16MHzatmega328=ATmega328 (5V, 16 MHz)
|
||||||
|
|
||||||
|
pro.menu.cpu.16MHzatmega328.upload.maximum_size=30720
|
||||||
|
pro.menu.cpu.16MHzatmega328.upload.maximum_data_size=2048
|
||||||
|
pro.menu.cpu.16MHzatmega328.upload.speed=57600
|
||||||
|
|
||||||
|
pro.menu.cpu.16MHzatmega328.bootloader.low_fuses=0xFF
|
||||||
|
pro.menu.cpu.16MHzatmega328.bootloader.high_fuses=0xDA
|
||||||
|
pro.menu.cpu.16MHzatmega328.bootloader.extended_fuses=0x05
|
||||||
|
pro.menu.cpu.16MHzatmega328.bootloader.file=atmega/ATmegaBOOT_168_atmega328.hex
|
||||||
|
|
||||||
|
pro.menu.cpu.16MHzatmega328.build.mcu=atmega328p
|
||||||
|
pro.menu.cpu.16MHzatmega328.build.f_cpu=16000000L
|
||||||
|
|
||||||
|
## Arduino Pro or Pro Mini (3.3V, 8 MHz) w/ ATmega328
|
||||||
|
## --------------------------------------------------
|
||||||
|
pro.menu.cpu.8MHzatmega328=ATmega328 (3.3V, 8 MHz)
|
||||||
|
|
||||||
|
pro.menu.cpu.8MHzatmega328.upload.maximum_size=30720
|
||||||
|
pro.menu.cpu.8MHzatmega328.upload.maximum_data_size=2048
|
||||||
|
pro.menu.cpu.8MHzatmega328.upload.speed=57600
|
||||||
|
|
||||||
|
pro.menu.cpu.8MHzatmega328.bootloader.low_fuses=0xFF
|
||||||
|
pro.menu.cpu.8MHzatmega328.bootloader.high_fuses=0xDA
|
||||||
|
pro.menu.cpu.8MHzatmega328.bootloader.extended_fuses=0x05
|
||||||
|
pro.menu.cpu.8MHzatmega328.bootloader.file=atmega/ATmegaBOOT_168_atmega328_pro_8MHz.hex
|
||||||
|
|
||||||
|
pro.menu.cpu.8MHzatmega328.build.mcu=atmega328p
|
||||||
|
pro.menu.cpu.8MHzatmega328.build.f_cpu=8000000L
|
||||||
|
|
||||||
|
## Arduino Pro or Pro Mini (5V, 16 MHz) w/ ATmega168
|
||||||
|
## -------------------------------------------------
|
||||||
|
pro.menu.cpu.16MHzatmega168=ATmega168 (5V, 16 MHz)
|
||||||
|
|
||||||
|
pro.menu.cpu.16MHzatmega168.upload.maximum_size=14336
|
||||||
|
pro.menu.cpu.16MHzatmega168.upload.maximum_data_size=1024
|
||||||
|
pro.menu.cpu.16MHzatmega168.upload.speed=19200
|
||||||
|
|
||||||
|
pro.menu.cpu.16MHzatmega168.bootloader.low_fuses=0xff
|
||||||
|
pro.menu.cpu.16MHzatmega168.bootloader.high_fuses=0xdd
|
||||||
|
pro.menu.cpu.16MHzatmega168.bootloader.extended_fuses=0x00
|
||||||
|
pro.menu.cpu.16MHzatmega168.bootloader.file=atmega/ATmegaBOOT_168_diecimila.hex
|
||||||
|
|
||||||
|
pro.menu.cpu.16MHzatmega168.build.mcu=atmega168
|
||||||
|
pro.menu.cpu.16MHzatmega168.build.f_cpu=16000000L
|
||||||
|
|
||||||
|
## Arduino Pro or Pro Mini (3.3V, 8 MHz) w/ ATmega168
|
||||||
|
## --------------------------------------------------
|
||||||
|
pro.menu.cpu.8MHzatmega168=ATmega168 (3.3V, 8 MHz)
|
||||||
|
|
||||||
|
pro.menu.cpu.8MHzatmega168.upload.maximum_size=14336
|
||||||
|
pro.menu.cpu.8MHzatmega168.upload.maximum_data_size=1024
|
||||||
|
pro.menu.cpu.8MHzatmega168.upload.speed=19200
|
||||||
|
|
||||||
|
pro.menu.cpu.8MHzatmega168.bootloader.low_fuses=0xc6
|
||||||
|
pro.menu.cpu.8MHzatmega168.bootloader.high_fuses=0xdd
|
||||||
|
pro.menu.cpu.8MHzatmega168.bootloader.extended_fuses=0x00
|
||||||
|
pro.menu.cpu.8MHzatmega168.bootloader.file=atmega/ATmegaBOOT_168_pro_8MHz.hex
|
||||||
|
|
||||||
|
pro.menu.cpu.8MHzatmega168.build.mcu=atmega168
|
||||||
|
pro.menu.cpu.8MHzatmega168.build.f_cpu=8000000L
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
atmegang.name=Arduino NG or older
|
||||||
|
|
||||||
|
atmegang.upload.tool=avrdude
|
||||||
|
atmegang.upload.protocol=arduino
|
||||||
|
atmegang.upload.speed=19200
|
||||||
|
|
||||||
|
atmegang.bootloader.tool=avrdude
|
||||||
|
atmegang.bootloader.unlock_bits=0x3F
|
||||||
|
atmegang.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
atmegang.build.mcu=atmegang
|
||||||
|
atmegang.build.f_cpu=16000000L
|
||||||
|
atmegang.build.board=AVR_NG
|
||||||
|
atmegang.build.core=arduino
|
||||||
|
atmegang.build.variant=standard
|
||||||
|
|
||||||
|
## Arduino NG or older w/ ATmega168
|
||||||
|
## --------------------------------
|
||||||
|
atmegang.menu.cpu.atmega168=ATmega168
|
||||||
|
|
||||||
|
atmegang.menu.cpu.atmega168.upload.maximum_size=14336
|
||||||
|
atmegang.menu.cpu.atmega168.upload.maximum_data_size=1024
|
||||||
|
|
||||||
|
atmegang.menu.cpu.atmega168.bootloader.low_fuses=0xff
|
||||||
|
atmegang.menu.cpu.atmega168.bootloader.high_fuses=0xdd
|
||||||
|
atmegang.menu.cpu.atmega168.bootloader.extended_fuses=0x00
|
||||||
|
atmegang.menu.cpu.atmega168.bootloader.file=atmega/ATmegaBOOT_168_ng.hex
|
||||||
|
|
||||||
|
atmegang.menu.cpu.atmega168.build.mcu=atmega168
|
||||||
|
|
||||||
|
## Arduino NG or older w/ ATmega8
|
||||||
|
## ------------------------------
|
||||||
|
atmegang.menu.cpu.atmega8=ATmega8
|
||||||
|
|
||||||
|
atmegang.menu.cpu.atmega8.upload.maximum_size=7168
|
||||||
|
atmegang.menu.cpu.atmega8.upload.maximum_data_size=1024
|
||||||
|
|
||||||
|
atmegang.menu.cpu.atmega8.bootloader.low_fuses=0xdf
|
||||||
|
atmegang.menu.cpu.atmega8.bootloader.high_fuses=0xca
|
||||||
|
atmegang.menu.cpu.atmega8.bootloader.file=atmega8/ATmegaBOOT-prod-firmware-2009-11-07.hex
|
||||||
|
|
||||||
|
atmegang.menu.cpu.atmega8.build.mcu=atmega8
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
robotControl.name=Arduino Robot Control
|
||||||
|
robotControl.vid.0=0x2341
|
||||||
|
robotControl.pid.0=0x0038
|
||||||
|
robotControl.vid.1=0x2341
|
||||||
|
robotControl.pid.1=0x8038
|
||||||
|
robotControl.vid.2=0x2A03
|
||||||
|
robotControl.pid.2=0x0038
|
||||||
|
robotControl.vid.3=0x2A03
|
||||||
|
robotControl.pid.3=0x8038
|
||||||
|
|
||||||
|
robotControl.upload.tool=avrdude
|
||||||
|
robotControl.upload.protocol=avr109
|
||||||
|
robotControl.upload.maximum_size=28672
|
||||||
|
robotControl.upload.maximum_data_size=2560
|
||||||
|
robotControl.upload.speed=57600
|
||||||
|
robotControl.upload.disable_flushing=true
|
||||||
|
robotControl.upload.use_1200bps_touch=true
|
||||||
|
robotControl.upload.wait_for_upload_port=true
|
||||||
|
|
||||||
|
robotControl.bootloader.tool=avrdude
|
||||||
|
robotControl.bootloader.low_fuses=0xff
|
||||||
|
robotControl.bootloader.high_fuses=0xd8
|
||||||
|
robotControl.bootloader.extended_fuses=0xcb
|
||||||
|
robotControl.bootloader.file=caterina-Arduino_Robot/Caterina-Robot-Control.hex
|
||||||
|
robotControl.bootloader.unlock_bits=0x3F
|
||||||
|
robotControl.bootloader.lock_bits=0x2F
|
||||||
|
|
||||||
|
robotControl.build.mcu=atmega32u4
|
||||||
|
robotControl.build.f_cpu=16000000L
|
||||||
|
robotControl.build.vid=0x2341
|
||||||
|
robotControl.build.pid=0x8038
|
||||||
|
robotControl.build.usb_product="Robot Control"
|
||||||
|
robotControl.build.board=AVR_ROBOT_CONTROL
|
||||||
|
robotControl.build.core=arduino
|
||||||
|
robotControl.build.variant=robot_control
|
||||||
|
robotControl.build.extra_flags={build.usb_flags}
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
robotMotor.name=Arduino Robot Motor
|
||||||
|
robotMotor.vid.0=0x2341
|
||||||
|
robotMotor.pid.0=0x0039
|
||||||
|
robotMotor.vid.1=0x2341
|
||||||
|
robotMotor.pid.1=0x8039
|
||||||
|
robotMotor.vid.2=0x2A03
|
||||||
|
robotMotor.pid.2=0x0039
|
||||||
|
robotMotor.vid.3=0x2A03
|
||||||
|
robotMotor.pid.3=0x8039
|
||||||
|
|
||||||
|
robotMotor.upload.tool=avrdude
|
||||||
|
robotMotor.upload.protocol=avr109
|
||||||
|
robotMotor.upload.maximum_size=28672
|
||||||
|
robotMotor.upload.maximum_data_size=2560
|
||||||
|
robotMotor.upload.speed=57600
|
||||||
|
robotMotor.upload.disable_flushing=true
|
||||||
|
robotMotor.upload.use_1200bps_touch=true
|
||||||
|
robotMotor.upload.wait_for_upload_port=true
|
||||||
|
|
||||||
|
robotMotor.bootloader.tool=avrdude
|
||||||
|
robotMotor.bootloader.low_fuses=0xff
|
||||||
|
robotMotor.bootloader.high_fuses=0xd8
|
||||||
|
robotMotor.bootloader.extended_fuses=0xcb
|
||||||
|
robotMotor.bootloader.file=caterina-Arduino_Robot/Caterina-Robot-Motor.hex
|
||||||
|
robotMotor.bootloader.unlock_bits=0x3F
|
||||||
|
robotMotor.bootloader.lock_bits=0x2F
|
||||||
|
|
||||||
|
robotMotor.build.mcu=atmega32u4
|
||||||
|
robotMotor.build.f_cpu=16000000L
|
||||||
|
robotMotor.build.vid=0x2341
|
||||||
|
robotMotor.build.pid=0x8039
|
||||||
|
robotMotor.build.usb_product="Robot Motor"
|
||||||
|
robotMotor.build.board=AVR_ROBOT_MOTOR
|
||||||
|
robotMotor.build.core=arduino
|
||||||
|
robotMotor.build.variant=robot_motor
|
||||||
|
robotMotor.build.extra_flags={build.usb_flags}
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
gemma.vid.0=0x2341
|
||||||
|
gemma.pid.0=0x0c9f
|
||||||
|
|
||||||
|
gemma.name=Arduino Gemma
|
||||||
|
|
||||||
|
gemma.bootloader.low_fuses=0xF1
|
||||||
|
gemma.bootloader.high_fuses=0xD5
|
||||||
|
gemma.bootloader.extended_fuses=0xFE
|
||||||
|
gemma.bootloader.tool=avrdude
|
||||||
|
gemma.bootloader.lock_bits=
|
||||||
|
gemma.bootloader.unlock_bits=
|
||||||
|
gemma.bootloader.file=gemma/gemma_v1.hex
|
||||||
|
|
||||||
|
gemma.build.mcu=attiny85
|
||||||
|
gemma.build.f_cpu=8000000L
|
||||||
|
gemma.build.core=arduino
|
||||||
|
gemma.build.variant=gemma
|
||||||
|
gemma.build.board=AVR_GEMMA
|
||||||
|
|
||||||
|
gemma.upload.tool=avrdude
|
||||||
|
gemma.upload.maximum_size=5310
|
||||||
|
|
||||||
|
##############################################################
|
||||||
|
|
||||||
|
## Multi 4-in-1 (3.3V, 16 MHz) w/ ATmega328
|
||||||
|
## --------------------------------------------------
|
||||||
|
multi.name=Multi 4-in-1
|
||||||
|
|
||||||
|
multi.upload.tool=avrdude
|
||||||
|
multi.upload.protocol=arduino
|
||||||
|
|
||||||
|
multi.bootloader.tool=avrdude
|
||||||
|
multi.bootloader.unlock_bits=0x3F
|
||||||
|
multi.bootloader.lock_bits=0x0F
|
||||||
|
|
||||||
|
multi.build.board=AVR_PRO
|
||||||
|
multi.build.core=arduino
|
||||||
|
multi.build.variant=eightanaloginputs
|
||||||
|
|
||||||
|
multi.menu.cpu.16MHzatmega328=ATmega328 (3.3V, 16 MHz)
|
||||||
|
|
||||||
|
multi.menu.cpu.16MHzatmega328.upload.maximum_size=32768
|
||||||
|
multi.menu.cpu.16MHzatmega328.upload.maximum_data_size=2048
|
||||||
|
multi.menu.cpu.16MHzatmega328.upload.speed=57600
|
||||||
|
multi.menu.cpu.16MHzatmega328.upload.using=arduino:arduinoisp
|
||||||
|
|
||||||
|
multi.menu.cpu.16MHzatmega328.bootloader.low_fuses=0xFF
|
||||||
|
multi.menu.cpu.16MHzatmega328.bootloader.high_fuses=0xD2
|
||||||
|
multi.menu.cpu.16MHzatmega328.bootloader.extended_fuses=0xFD
|
||||||
|
multi.menu.cpu.16MHzatmega328.bootloader.file=atmega/ATmegaBOOT_168_atmega328_pro_8MHz.hex
|
||||||
|
|
||||||
|
multi.menu.cpu.16MHzatmega328.build.mcu=atmega328p
|
||||||
|
multi.menu.cpu.16MHzatmega328.build.f_cpu=16000000L
|
||||||
|
## --------------------------------------------------
|
||||||
|
multi.menu.cpu.16MHzatmega328opti=ATmega328 Opti (3.3V, 16 MHz)
|
||||||
|
|
||||||
|
multi.menu.cpu.16MHzatmega328opti.upload.maximum_size=32768
|
||||||
|
multi.menu.cpu.16MHzatmega328opti.upload.maximum_data_size=2048
|
||||||
|
multi.menu.cpu.16MHzatmega328opti.upload.speed=57600
|
||||||
|
multi.menu.cpu.16MHzatmega328opti.upload.using=arduino:arduinoisp
|
||||||
|
|
||||||
|
multi.menu.cpu.16MHzatmega328opti.bootloader.low_fuses=0xFF
|
||||||
|
multi.menu.cpu.16MHzatmega328opti.bootloader.high_fuses=0xD6
|
||||||
|
multi.menu.cpu.16MHzatmega328opti.bootloader.extended_fuses=0xFD
|
||||||
|
multi.menu.cpu.16MHzatmega328opti.bootloader.file=atmega/optiboot_atmega328_16.hex
|
||||||
|
|
||||||
|
multi.menu.cpu.16MHzatmega328opti.build.mcu=atmega328p
|
||||||
|
multi.menu.cpu.16MHzatmega328opti.build.f_cpu=16000000L
|
||||||
|
|
||||||
|
##############################################################
|
@ -90,6 +90,7 @@ enum A7105_MASK {
|
|||||||
enum {
|
enum {
|
||||||
INIT_FLYSKY,
|
INIT_FLYSKY,
|
||||||
INIT_FLYSKY_AFHDS2A,
|
INIT_FLYSKY_AFHDS2A,
|
||||||
|
INIT_JOYSWAY,
|
||||||
INIT_HUBSAN
|
INIT_HUBSAN
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -13,16 +13,6 @@
|
|||||||
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// Check selected board type
|
|
||||||
#ifndef XMEGA
|
|
||||||
#if not defined(ARDUINO_AVR_PRO) && not defined(ARDUINO_AVR_MINI) && not defined(ARDUINO_AVR_NANO)
|
|
||||||
// #error You must select the board type "Arduino Pro or Pro Mini" or "Arduino Mini"
|
|
||||||
#endif
|
|
||||||
#if F_CPU != 16000000L || not defined(__AVR_ATmega328P__)
|
|
||||||
#error You must select the processor type "ATmega328(5V, 16MHz)"
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//******************
|
//******************
|
||||||
// Protocols max 31 x2
|
// Protocols max 31 x2
|
||||||
//******************
|
//******************
|
||||||
@ -168,7 +158,10 @@ enum FY326
|
|||||||
FY326 = 0,
|
FY326 = 0,
|
||||||
FY319 = 1
|
FY319 = 1
|
||||||
};
|
};
|
||||||
|
enum{
|
||||||
|
FORMAT_V202 = 0,
|
||||||
|
FORMAT_JXD506 = 1,
|
||||||
|
};
|
||||||
enum WK2X01
|
enum WK2X01
|
||||||
{
|
{
|
||||||
WK2801 = 0,
|
WK2801 = 0,
|
||||||
@ -207,27 +200,6 @@ struct PPM_Parameters
|
|||||||
// Macros
|
// Macros
|
||||||
#define NOP() __asm__ __volatile__("nop")
|
#define NOP() __asm__ __volatile__("nop")
|
||||||
|
|
||||||
//*******************
|
|
||||||
//*** Timer ***
|
|
||||||
//*******************
|
|
||||||
#ifdef XMEGA
|
|
||||||
#define TIFR1 TCC1.INTFLAGS
|
|
||||||
#define OCF1A_bm TC1_CCAIF_bm
|
|
||||||
#define OCR1A TCC1.CCA
|
|
||||||
#define TCNT1 TCC1.CNT
|
|
||||||
#define UDR0 USARTC0.DATA
|
|
||||||
#define OCF1B_bm TC1_CCBIF_bm
|
|
||||||
#define OCR1B TCC1.CCB
|
|
||||||
#define TIMSK1 TCC1.INTCTRLB
|
|
||||||
#define SET_TIMSK1_OCIE1B TIMSK1 = (TIMSK1 & 0xF3) | 0x04
|
|
||||||
#define CLR_TIMSK1_OCIE1B TIMSK1 &= 0xF3
|
|
||||||
#else
|
|
||||||
#define OCF1A_bm _BV(OCF1A)
|
|
||||||
#define OCF1B_bm _BV(OCF1B)
|
|
||||||
#define SET_TIMSK1_OCIE1B TIMSK1 |= _BV(OCIE1B)
|
|
||||||
#define CLR_TIMSK1_OCIE1B TIMSK1 &=~_BV(OCIE1B)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//***************
|
//***************
|
||||||
//*** Flags ***
|
//*** Flags ***
|
||||||
//***************
|
//***************
|
||||||
@ -522,9 +494,9 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p --
|
|||||||
CH_16 0
|
CH_16 0
|
||||||
CH_8 1
|
CH_8 1
|
||||||
sub_protocol==HONTAI
|
sub_protocol==HONTAI
|
||||||
HONTAI 0
|
FORMAT_HONTAI 0
|
||||||
JJRCX1 1
|
FORMAT_JJRCX1 1
|
||||||
X5C1 2
|
FORMAT_X5C1 2
|
||||||
FQ777-521 3
|
FQ777-521 3
|
||||||
Power value => 0x80 0=High/1=Low
|
Power value => 0x80 0=High/1=Low
|
||||||
Stream[3] = option_protocol;
|
Stream[3] = option_protocol;
|
||||||
|
Loading…
x
Reference in New Issue
Block a user