gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-02-04 18:38:13 +00:00
Code Issues Projects Releases Wiki Activity

Fix AFHDS2A

Browse Source 
Fix bind issue with AFHDS2A
Add frskyd hub telemetry voltage 2 from external battery
Removed cflie warnings...
This commit is contained in:
Pascal Langer 2018-07-20 15:24:33 +02:00
parent dde5f6e119
commit f9f265271a
4 changed files with 186 additions and 215 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Multiprotocol/AFHDS2A_a7105.ino
View File

@ -81,6 +81,7 @@ enum{
AFHDS2A_SENSOR_RX_RSSI = 0xfc, AFHDS2A_SENSOR_RX_RSSI = 0xfc,
AFHDS2A_SENSOR_RX_NOISE = 0xfb, AFHDS2A_SENSOR_RX_NOISE = 0xfb,
AFHDS2A_SENSOR_RX_SNR = 0xfa, AFHDS2A_SENSOR_RX_SNR = 0xfa,
AFHDS2A_SENSOR_A3_VOLTAGE = 0x03,
}; };
static void AFHDS2A_update_telemetry() static void AFHDS2A_update_telemetry()
@ -111,6 +112,10 @@ static void AFHDS2A_update_telemetry()
v_lipo1 = packet[index+2]; v_lipo1 = packet[index+2];
telemetry_link=1; telemetry_link=1;
break; break;
case AFHDS2A_SENSOR_A3_VOLTAGE:
v_lipo2 = (packet[index+3]<<5) | (packet[index+2]>>3); // allows to read voltage up to 4S
telemetry_link=1;
break;
case AFHDS2A_SENSOR_RX_ERR_RATE: case AFHDS2A_SENSOR_RX_ERR_RATE:
RX_LQI=packet[index+2]; RX_LQI=packet[index+2];
break; break;
@ -244,12 +249,17 @@ uint16_t ReadAFHDS2A()
if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6))) if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)))
{ // FECF+CRCF Ok { // FECF+CRCF Ok
A7105_ReadData(AFHDS2A_RXPACKET_SIZE); A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
if(packet[0] == 0xbc) if(packet[0] == 0xbc && packet[9] == 0x01)
{ {
uint8_t temp=AFHDS2A_EEPROM_OFFSET+RX_num*4;
for(uint8_t i=0; i<4; i++) for(uint8_t i=0; i<4; i++)
{
rx_id[i] = packet[5+i]; rx_id[i] = packet[5+i];
if(packet[9] == 0x01) eeprom_write_byte((EE_ADDR)(temp+i),rx_id[i]);
phase = AFHDS2A_BIND4; }
phase = AFHDS2A_BIND4;
packet_count++;
return 3850;
} }
} }
packet_count++; packet_count++;
@ -278,9 +288,6 @@ uint16_t ReadAFHDS2A()
bind_phase++; bind_phase++;
if(bind_phase>=4) if(bind_phase>=4)
{ {
uint8_t eeadr=AFHDS2A_EEPROM_OFFSET+RX_num*4;
for(uint8_t i=0; i<4; i++)
eeprom_write_byte((EE_ADDR)(eeadr+i),rx_id[i]);
hopping_frequency_no=1; hopping_frequency_no=1;
phase = AFHDS2A_DATA_INIT; phase = AFHDS2A_DATA_INIT;
BIND_DONE; BIND_DONE;
@ -289,6 +296,7 @@ uint16_t ReadAFHDS2A()
case AFHDS2A_DATA_INIT: case AFHDS2A_DATA_INIT:
packet_counter=0; packet_counter=0;
packet_type = AFHDS2A_PACKET_STICKS; packet_type = AFHDS2A_PACKET_STICKS;
phase = AFHDS2A_DATA;
case AFHDS2A_DATA: case AFHDS2A_DATA:
AFHDS2A_build_packet(packet_type); AFHDS2A_build_packet(packet_type);
if((A7105_ReadReg(A7105_00_MODE) & 0x01)) // Check if something has been received... if((A7105_ReadReg(A7105_00_MODE) & 0x01)) // Check if something has been received...

351
Multiprotocol/CFlie_nrf24l01.ino
View File

@ -105,14 +105,13 @@ static inline uint8_t crtp_create_header(uint8_t port, uint8_t channel)
// supports a total of 12 channels. R,P,Y,T leaves 8 aux channels left // supports a total of 12 channels. R,P,Y,T leaves 8 aux channels left
#define MAX_CPPM_AUX_CHANNELS 8 #define MAX_CPPM_AUX_CHANNELS 8
static uint8_t tx_payload_len = 0; // Length of the packet stored in tx_packet static uint8_t tx_payload_len = 0; // Length of the packet stored in packet
static uint8_t tx_packet[MAX_PACKET_SIZE]; // For writing Tx payloads
static uint8_t rx_payload_len = 0; // Length of the packet stored in rx_packet static uint8_t rx_payload_len = 0; // Length of the packet stored in rx_packet
static uint8_t rx_packet[MAX_PACKET_SIZE]; // For reading in ACK payloads static uint8_t rx_packet[MAX_PACKET_SIZE]; // For reading in ACK payloads
static uint16_t cflie_counter; static uint16_t cflie_counter;
static uint32_t packet_counter; static uint32_t packet_counter;
static uint8_t tx_power, data_rate, rf_channel; static uint8_t data_rate;
enum { enum {
CFLIE_INIT_SEARCH = 0, CFLIE_INIT_SEARCH = 0,
@ -183,7 +182,7 @@ enum {
#define PACKET_CHKTIME 500 // time to wait if packet not yet acknowledged or timed out #define PACKET_CHKTIME 500 // time to wait if packet not yet acknowledged or timed out
// Helper for sending a packet // Helper for sending a packet
// Assumes packet data has been put in tx_packet // Assumes packet data has been put in packet
// and tx_payload_len has been set correctly // and tx_payload_len has been set correctly
static void send_packet() static void send_packet()
{ {
@ -193,77 +192,71 @@ static void send_packet()
NRF24L01_FlushRx(); NRF24L01_FlushRx();
// Transmit the payload // Transmit the payload
NRF24L01_WritePayload(tx_packet, tx_payload_len); NRF24L01_WritePayload(packet, tx_payload_len);
++packet_counter; ++packet_counter;
// // Check and adjust transmission power. We do this after // // Check and adjust transmission power.
// // transmission to not bother with timeout after power NRF24L01_SetPower();
// // settings change - we have plenty of time until next
// // packet.
// if (tx_power != Model.tx_power) {
// //Keep transmit power updated
// tx_power = Model.tx_power;
// NRF24L01_SetPower(tx_power);
// }
NRF24L01_SetPower(); // hack? not sure if required...
} }
static uint16_t dbg_cnt = 0; static uint16_t dbg_cnt = 0;
static uint8_t packet_ack() static uint8_t packet_ack()
{ {
if (++dbg_cnt > 50) { if (++dbg_cnt > 50)
// debugln("S: %02x\n", NRF24L01_ReadReg(NRF24L01_07_STATUS)); {
dbg_cnt = 0; // debugln("S: %02x\n", NRF24L01_ReadReg(NRF24L01_07_STATUS));
} dbg_cnt = 0;
switch (NRF24L01_ReadReg(NRF24L01_07_STATUS) & (BV(NRF24L01_07_TX_DS) | BV(NRF24L01_07_MAX_RT))) { }
case BV(NRF24L01_07_TX_DS): switch (NRF24L01_ReadReg(NRF24L01_07_STATUS) & (BV(NRF24L01_07_TX_DS) | BV(NRF24L01_07_MAX_RT)))
rx_payload_len = NRF24L01_GetDynamicPayloadSize(); {
if (rx_payload_len > MAX_PACKET_SIZE) { case BV(NRF24L01_07_TX_DS):
rx_payload_len = MAX_PACKET_SIZE; rx_payload_len = NRF24L01_GetDynamicPayloadSize();
} if (rx_payload_len > MAX_PACKET_SIZE)
NRF24L01_ReadPayload(rx_packet, rx_payload_len); rx_payload_len = MAX_PACKET_SIZE;
return PKT_ACKED; NRF24L01_ReadPayload(rx_packet, rx_payload_len);
case BV(NRF24L01_07_MAX_RT): return PKT_ACKED;
return PKT_TIMEOUT; case BV(NRF24L01_07_MAX_RT):
} return PKT_TIMEOUT;
return PKT_PENDING; }
return PKT_PENDING;
} }
static void set_rate_channel(uint8_t rate, uint8_t channel) static void set_rate_channel(uint8_t rate, uint8_t channel)
{ {
NRF24L01_WriteReg(NRF24L01_05_RF_CH, channel); // Defined by model id NRF24L01_WriteReg(NRF24L01_05_RF_CH, channel); // Defined by model id
NRF24L01_SetBitrate(rate); // Defined by model id NRF24L01_SetBitrate(rate); // Defined by model id
} }
static void send_search_packet() static void send_search_packet()
{ {
uint8_t buf[1]; uint8_t buf[1];
buf[0] = 0xff; buf[0] = 0xff;
// clear packet status bits and TX FIFO // clear packet status bits and TX FIFO
NRF24L01_WriteReg(NRF24L01_07_STATUS, (BV(NRF24L01_07_TX_DS) | BV(NRF24L01_07_MAX_RT))); NRF24L01_WriteReg(NRF24L01_07_STATUS, (BV(NRF24L01_07_TX_DS) | BV(NRF24L01_07_MAX_RT)));
NRF24L01_FlushTx(); NRF24L01_FlushTx();
if (rf_channel++ > 125) { if (rf_ch_num++ > 125)
rf_channel = 0; {
switch(data_rate) { rf_ch_num = 0;
case NRF24L01_BR_250K: switch(data_rate)
data_rate = NRF24L01_BR_1M; {
break; case NRF24L01_BR_250K:
case NRF24L01_BR_1M: data_rate = NRF24L01_BR_1M;
data_rate = NRF24L01_BR_2M; break;
break; case NRF24L01_BR_1M:
case NRF24L01_BR_2M: data_rate = NRF24L01_BR_2M;
data_rate = NRF24L01_BR_250K; break;
break; case NRF24L01_BR_2M:
} data_rate = NRF24L01_BR_250K;
} break;
}
}
set_rate_channel(data_rate, rf_ch_num);
set_rate_channel(data_rate, rf_channel); NRF24L01_WritePayload(buf, sizeof(buf));
NRF24L01_WritePayload(buf, sizeof(buf)); ++packet_counter;
++packet_counter;
} }
// Frac 16.16 // Frac 16.16
@ -273,100 +266,101 @@ static void send_search_packet()
// Convert fractional 16.16 to float32 // Convert fractional 16.16 to float32
static void frac2float(int32_t n, float* res) static void frac2float(int32_t n, float* res)
{ {
if (n == 0) { if (n == 0)
*res = 0.0; {
return; *res = 0.0;
} return;
uint32_t m = n < 0 ? -n : n; // Figure out mantissa? }
int i; uint32_t m = n < 0 ? -n : n; // Figure out mantissa?
for (i = (31-FRAC_MANTISSA); (m & 0x80000000) == 0; i--, m <<= 1); int i;
m <<= 1; // Clear implicit leftmost 1 for (i = (31-FRAC_MANTISSA); (m & 0x80000000) == 0; i--, m <<= 1);
m >>= 9; m <<= 1; // Clear implicit leftmost 1
uint32_t e = 127 + i; m >>= 9;
if (n < 0) m |= 0x80000000; uint32_t e = 127 + i;
m |= e << 23; if (n < 0) m |= 0x80000000;
*((uint32_t *) res) = m; m |= e << 23;
*((uint32_t *) res) = m;
} }
static void send_crtp_rpyt_packet() static void send_crtp_rpyt_packet()
{ {
int32_t f_roll; int32_t f_roll;
int32_t f_pitch; int32_t f_pitch;
int32_t f_yaw; int32_t f_yaw;
uint16_t thrust; uint16_t thrust;
uint16_t val; uint16_t val;
struct CommanderPacketRPYT struct CommanderPacketRPYT
{ {
float roll; float roll;
float pitch; float pitch;
float yaw; float yaw;
uint16_t thrust; uint16_t thrust;
}__attribute__((packed)) cpkt; }__attribute__((packed)) cpkt;
// Channels in AETR order // Channels in AETR order
// Roll, aka aileron, float +- 50.0 in degrees // Roll, aka aileron, float +- 50.0 in degrees
// float roll = -(float) Channels[0]*50.0/10000; // float roll = -(float) Channels[0]*50.0/10000;
val = convert_channel_16b_limit(AILERON, -10000, 10000); val = convert_channel_16b_limit(AILERON, -10000, 10000);
// f_roll = -Channels[0] * FRAC_SCALE / (10000 / 50); // f_roll = -Channels[0] * FRAC_SCALE / (10000 / 50);
f_roll = val * FRAC_SCALE / (10000 / 50); f_roll = val * FRAC_SCALE / (10000 / 50);
frac2float(f_roll, &cpkt.roll); // TODO: Remove this and use the correct Mode switch below... frac2float(f_roll, &cpkt.roll); // TODO: Remove this and use the correct Mode switch below...
// debugln("Roll: raw, converted: %d, %d, %d, %0.2f", Channel_data[AILERON], val, f_roll, cpkt.roll); // debugln("Roll: raw, converted: %d, %d, %d, %0.2f", Channel_data[AILERON], val, f_roll, cpkt.roll);
// Pitch, aka elevator, float +- 50.0 degrees // Pitch, aka elevator, float +- 50.0 degrees
//float pitch = -(float) Channels[1]*50.0/10000; //float pitch = -(float) Channels[1]*50.0/10000;
val = convert_channel_16b_limit(ELEVATOR, -10000, 10000); val = convert_channel_16b_limit(ELEVATOR, -10000, 10000);
// f_pitch = -Channels[1] * FRAC_SCALE / (10000 / 50); // f_pitch = -Channels[1] * FRAC_SCALE / (10000 / 50);
f_pitch = -val * FRAC_SCALE / (10000 / 50); f_pitch = -val * FRAC_SCALE / (10000 / 50);
frac2float(f_pitch, &cpkt.pitch); // TODO: Remove this and use the correct Mode switch below... frac2float(f_pitch, &cpkt.pitch); // TODO: Remove this and use the correct Mode switch below...
// debugln("Pitch: raw, converted: %d, %d, %d, %0.2f", Channel_data[ELEVATOR], val, f_pitch, cpkt.pitch); // debugln("Pitch: raw, converted: %d, %d, %d, %0.2f", Channel_data[ELEVATOR], val, f_pitch, cpkt.pitch);
// Thrust, aka throttle 0..65535, working range 5535..65535 // Thrust, aka throttle 0..65535, working range 5535..65535
// Android Crazyflie app puts out a throttle range of 0-80%: 0..52000 // Android Crazyflie app puts out a throttle range of 0-80%: 0..52000
thrust = convert_channel_16b_limit(THROTTLE, 0, 32767) * 2; thrust = convert_channel_16b_limit(THROTTLE, 0, 32767) * 2;
// Crazyflie needs zero thrust to unlock // Crazyflie needs zero thrust to unlock
if (thrust < 900) if (thrust < 900)
cpkt.thrust = 0; cpkt.thrust = 0;
else else
cpkt.thrust = thrust; cpkt.thrust = thrust;
// debugln("Thrust: raw, converted: %d, %u, %u", Channel_data[THROTTLE], thrust, cpkt.thrust); // debugln("Thrust: raw, converted: %d, %u, %u", Channel_data[THROTTLE], thrust, cpkt.thrust);
// Yaw, aka rudder, float +- 400.0 deg/s // Yaw, aka rudder, float +- 400.0 deg/s
// float yaw = -(float) Channels[3]*400.0/10000; // float yaw = -(float) Channels[3]*400.0/10000;
val = convert_channel_16b_limit(RUDDER, -10000, 10000); val = convert_channel_16b_limit(RUDDER, -10000, 10000);
// f_yaw = - Channels[3] * FRAC_SCALE / (10000 / 400); // f_yaw = - Channels[3] * FRAC_SCALE / (10000 / 400);
f_yaw = val * FRAC_SCALE / (10000 / 400); f_yaw = val * FRAC_SCALE / (10000 / 400);
frac2float(f_yaw, &cpkt.yaw); frac2float(f_yaw, &cpkt.yaw);
// debugln("Yaw: raw, converted: %d, %d, %d, %0.2f", Channel_data[RUDDER], val, f_yaw, cpkt.yaw); // debugln("Yaw: raw, converted: %d, %d, %d, %0.2f", Channel_data[RUDDER], val, f_yaw, cpkt.yaw);
// Switch on/off? // Switch on/off?
// TODO: Get X or + mode working again: // TODO: Get X or + mode working again:
// if (Channels[4] >= 0) { // if (Channels[4] >= 0) {
// frac2float(f_roll, &cpkt.roll); // frac2float(f_roll, &cpkt.roll);
// frac2float(f_pitch, &cpkt.pitch); // frac2float(f_pitch, &cpkt.pitch);
// } else { // } else {
// // Rotate 45 degrees going from X to + mode or opposite. // // Rotate 45 degrees going from X to + mode or opposite.
// // 181 / 256 = 0.70703125 ~= sqrt(2) / 2 // // 181 / 256 = 0.70703125 ~= sqrt(2) / 2
// int32_t f_x_roll = (f_roll + f_pitch) * 181 / 256; // int32_t f_x_roll = (f_roll + f_pitch) * 181 / 256;
// frac2float(f_x_roll, &cpkt.roll); // frac2float(f_x_roll, &cpkt.roll);
// int32_t f_x_pitch = (f_pitch - f_roll) * 181 / 256; // int32_t f_x_pitch = (f_pitch - f_roll) * 181 / 256;
// frac2float(f_x_pitch, &cpkt.pitch); // frac2float(f_x_pitch, &cpkt.pitch);
// } // }
// Construct and send packet // Construct and send packet
tx_packet[0] = crtp_create_header(CRTP_PORT_SETPOINT, 0); // Commander packet to channel 0 packet[0] = crtp_create_header(CRTP_PORT_SETPOINT, 0); // Commander packet to channel 0
memcpy(&tx_packet[1], (char*) &cpkt, sizeof(cpkt)); memcpy(&packet[1], (char*) &cpkt, sizeof(cpkt));
tx_payload_len = 1 + sizeof(cpkt); tx_payload_len = 1 + sizeof(cpkt);
send_packet(); send_packet();
} }
static void send_crtp_cppm_emu_packet() /*static void send_crtp_cppm_emu_packet()
{ {
struct CommanderPacketCppmEmu { struct CommanderPacketCppmEmu {
struct { struct {
@ -413,14 +407,14 @@ static void send_crtp_cppm_emu_packet()
uint8_t commanderPacketSize = 1 + 8 + (2*numAuxChannels); uint8_t commanderPacketSize = 1 + 8 + (2*numAuxChannels);
// Construct and send packet // Construct and send packet
tx_packet[0] = crtp_create_header(CRTP_PORT_SETPOINT_GENERIC, 0); // Generic setpoint packet to channel 0 packet[0] = crtp_create_header(CRTP_PORT_SETPOINT_GENERIC, 0); // Generic setpoint packet to channel 0
tx_packet[1] = CRTP_SETPOINT_GENERIC_CPPM_EMU_TYPE; packet[1] = CRTP_SETPOINT_GENERIC_CPPM_EMU_TYPE;
// Copy the header (1) plus 4 2-byte channels (8) plus whatever number of 2-byte aux channels are in use // Copy the header (1) plus 4 2-byte channels (8) plus whatever number of 2-byte aux channels are in use
memcpy(&tx_packet[2], (char*)&cpkt, commanderPacketSize); // Why not use sizeof(cpkt) here?? memcpy(&packet[2], (char*)&cpkt, commanderPacketSize); // Why not use sizeof(cpkt) here??
tx_payload_len = 2 + commanderPacketSize; // CRTP header, commander type, and packet tx_payload_len = 2 + commanderPacketSize; // CRTP header, commander type, and packet
send_packet(); send_packet();
} }*/
static void send_cmd_packet() static void send_cmd_packet()
{ {
@ -473,8 +467,8 @@ static uint8_t crtp_log_setup_state_machine()
// fallthrough // fallthrough
case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_INFO: case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_INFO:
crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CMD_GET_INFO; crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CMD_GET_INFO;
tx_packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_TOC); packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_TOC);
tx_packet[1] = CRTP_LOG_TOC_CMD_INFO; packet[1] = CRTP_LOG_TOC_CMD_INFO;
tx_payload_len = 2; tx_payload_len = 2;
send_packet(); send_packet();
break; break;
@ -506,9 +500,9 @@ static uint8_t crtp_log_setup_state_machine()
case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_ITEM: case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CMD_GET_ITEM:
crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CMD_GET_ITEM; crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CMD_GET_ITEM;
tx_packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_TOC); packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_TOC);
tx_packet[1] = CRTP_LOG_TOC_CMD_ELEMENT; packet[1] = CRTP_LOG_TOC_CMD_ELEMENT;
tx_packet[2] = next_toc_variable; packet[2] = next_toc_variable;
tx_payload_len = 3; tx_payload_len = 3;
send_packet(); send_packet();
break; break;
@ -577,15 +571,15 @@ static uint8_t crtp_log_setup_state_machine()
case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_CREATE_BLOCK: case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_CREATE_BLOCK:
crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CONTROL_CREATE_BLOCK; crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CONTROL_CREATE_BLOCK;
tx_packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_SETTINGS); packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_SETTINGS);
tx_packet[1] = CRTP_LOG_SETTINGS_CMD_CREATE_BLOCK; packet[1] = CRTP_LOG_SETTINGS_CMD_CREATE_BLOCK;
tx_packet[2] = CFLIE_TELEM_LOG_BLOCK_ID; // Log block ID packet[2] = CFLIE_TELEM_LOG_BLOCK_ID; // Log block ID
tx_packet[3] = vbat_var_type; // Variable type packet[3] = vbat_var_type; // Variable type
tx_packet[4] = vbat_var_id; // ID of the VBAT variable packet[4] = vbat_var_id; // ID of the VBAT variable
tx_packet[5] = extvbat_var_type; // Variable type packet[5] = extvbat_var_type; // Variable type
tx_packet[6] = extvbat_var_id; // ID of the ExtVBat variable packet[6] = extvbat_var_id; // ID of the ExtVBat variable
tx_packet[7] = rssi_var_type; // Variable type packet[7] = rssi_var_type; // Variable type
tx_packet[8] = rssi_var_id; // ID of the RSSI variable packet[8] = rssi_var_id; // ID of the RSSI variable
tx_payload_len = 9; tx_payload_len = 9;
send_packet(); send_packet();
break; break;
@ -615,10 +609,10 @@ static uint8_t crtp_log_setup_state_machine()
case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_START_BLOCK: case CFLIE_CRTP_LOG_SETUP_STATE_SEND_CONTROL_START_BLOCK:
crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CONTROL_START_BLOCK; crtp_log_setup_state = CFLIE_CRTP_LOG_SETUP_STATE_ACK_CONTROL_START_BLOCK;
tx_packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_SETTINGS); packet[0] = crtp_create_header(CRTP_PORT_LOG, CRTP_LOG_CHAN_SETTINGS);
tx_packet[1] = CRTP_LOG_SETTINGS_CMD_START_LOGGING; packet[1] = CRTP_LOG_SETTINGS_CMD_START_LOGGING;
tx_packet[2] = CFLIE_TELEM_LOG_BLOCK_ID; // Log block ID 1 packet[2] = CFLIE_TELEM_LOG_BLOCK_ID; // Log block ID 1
tx_packet[3] = CFLIE_TELEM_LOG_BLOCK_PERIOD_10MS; // Log frequency in 10ms units packet[3] = CFLIE_TELEM_LOG_BLOCK_PERIOD_10MS; // Log frequency in 10ms units
tx_payload_len = 4; tx_payload_len = 4;
send_packet(); send_packet();
break; break;
@ -667,7 +661,7 @@ static int cflie_init()
NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, TX_ADDR_SIZE-2); // 5-byte RX/TX address NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, TX_ADDR_SIZE-2); // 5-byte RX/TX address
NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x13); // 3 retransmits, 500us delay NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x13); // 3 retransmits, 500us delay
NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_channel); // Defined in initialize_rx_tx_addr NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_ch_num); // Defined in initialize_rx_tx_addr
NRF24L01_SetBitrate(data_rate); // Defined in initialize_rx_tx_addr NRF24L01_SetBitrate(data_rate); // Defined in initialize_rx_tx_addr
NRF24L01_SetPower(); NRF24L01_SetPower();
@ -684,43 +678,6 @@ static int cflie_init()
NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x01); // Enable Dynamic Payload Length on pipe 0 NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x01); // Enable Dynamic Payload Length on pipe 0
NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x06); // Enable Dynamic Payload Length, enable Payload with ACK NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x06); // Enable Dynamic Payload Length, enable Payload with ACK
// Check for Beken BK2421/BK2423 chip
// It is done by using Beken specific activate code, 0x53
// and checking that status register changed appropriately
// There is no harm to run it on nRF24L01 because following
// closing activate command changes state back even if it
// does something on nRF24L01
NRF24L01_Activate(0x53); // magic for BK2421 bank switch
// debugln("CFlie: Trying to switch banks\n");
if (NRF24L01_ReadReg(NRF24L01_07_STATUS) & 0x80) {
// debugln("CFlie: BK2421 detected\n");
long nul = 0;
// Beken registers don't have such nice names, so we just mention
// them by their numbers
// It's all magic, eavesdropped from real transfer and not even from the
// data sheet - it has slightly different values
NRF24L01_WriteRegisterMulti(0x00, (uint8_t *) "\x40\x4B\x01\xE2", 4);
NRF24L01_WriteRegisterMulti(0x01, (uint8_t *) "\xC0\x4B\x00\x00", 4);
NRF24L01_WriteRegisterMulti(0x02, (uint8_t *) "\xD0\xFC\x8C\x02", 4);
NRF24L01_WriteRegisterMulti(0x03, (uint8_t *) "\xF9\x00\x39\x21", 4);
NRF24L01_WriteRegisterMulti(0x04, (uint8_t *) "\xC1\x96\x9A\x1B", 4);
NRF24L01_WriteRegisterMulti(0x05, (uint8_t *) "\x24\x06\x7F\xA6", 4);
NRF24L01_WriteRegisterMulti(0x06, (uint8_t *) &nul, 4);
NRF24L01_WriteRegisterMulti(0x07, (uint8_t *) &nul, 4);
NRF24L01_WriteRegisterMulti(0x08, (uint8_t *) &nul, 4);
NRF24L01_WriteRegisterMulti(0x09, (uint8_t *) &nul, 4);
NRF24L01_WriteRegisterMulti(0x0A, (uint8_t *) &nul, 4);
NRF24L01_WriteRegisterMulti(0x0B, (uint8_t *) &nul, 4);
NRF24L01_WriteRegisterMulti(0x0C, (uint8_t *) "\x00\x12\x73\x00", 4);
NRF24L01_WriteRegisterMulti(0x0D, (uint8_t *) "\x46\xB4\x80\x00", 4);
NRF24L01_WriteRegisterMulti(0x0E, (uint8_t *) "\x41\x10\x04\x82\x20\x08\x08\xF2\x7D\xEF\xFF", 11);
NRF24L01_WriteRegisterMulti(0x04, (uint8_t *) "\xC7\x96\x9A\x1B", 4);
NRF24L01_WriteRegisterMulti(0x04, (uint8_t *) "\xC1\x96\x9A\x1B", 4);
} else {
// debugln("CFlie: nRF24L01 detected");
}
NRF24L01_Activate(0x53); // switch bank back
// 50ms delay in callback // 50ms delay in callback
return 50000; return 50000;
} }
@ -734,7 +691,7 @@ static int cflie_init()
// // Read and reset count of dropped packets // // Read and reset count of dropped packets
// frameloss += NRF24L01_ReadReg(NRF24L01_08_OBSERVE_TX) >> 4; // frameloss += NRF24L01_ReadReg(NRF24L01_08_OBSERVE_TX) >> 4;
// NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_channel); // reset packet loss counter // NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_ch_num); // reset packet loss counter
// Telemetry.value[TELEM_DSM_FLOG_FRAMELOSS] = frameloss; // Telemetry.value[TELEM_DSM_FLOG_FRAMELOSS] = frameloss;
// TELEMETRY_SetUpdated(TELEM_DSM_FLOG_FRAMELOSS); // TELEMETRY_SetUpdated(TELEM_DSM_FLOG_FRAMELOSS);
@ -774,7 +731,7 @@ static int cflie_init()
// // Read and reset count of dropped packets // // Read and reset count of dropped packets
// frameloss += NRF24L01_ReadReg(NRF24L01_08_OBSERVE_TX) >> 4; // frameloss += NRF24L01_ReadReg(NRF24L01_08_OBSERVE_TX) >> 4;
// NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_channel); // reset packet loss counter // NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_ch_num); // reset packet loss counter
// Telemetry.value[TELEM_DSM_FLOG_FRAMELOSS] = frameloss; // Telemetry.value[TELEM_DSM_FLOG_FRAMELOSS] = frameloss;
// TELEMETRY_SetUpdated(TELEM_DSM_FLOG_FRAMELOSS); // TELEMETRY_SetUpdated(TELEM_DSM_FLOG_FRAMELOSS);
@ -854,7 +811,7 @@ static uint8_t initialize_rx_tx_addr()
rx_tx_addr[4] = 0xE7; // CFlie uses fixed address rx_tx_addr[4] = 0xE7; // CFlie uses fixed address
// if (Model.fixed_id) { // if (Model.fixed_id) {
// rf_channel = Model.fixed_id % 100; // rf_ch_num = Model.fixed_id % 100;
// switch (Model.fixed_id / 100) { // switch (Model.fixed_id / 100) {
// case 0: // case 0:
// data_rate = NRF24L01_BR_250K; // data_rate = NRF24L01_BR_250K;
@ -876,17 +833,17 @@ static uint8_t initialize_rx_tx_addr()
// } // }
// } else { // } else {
// data_rate = NRF24L01_BR_250K; // data_rate = NRF24L01_BR_250K;
// rf_channel = 10; // rf_ch_num = 10;
// return CFLIE_INIT_SEARCH; // return CFLIE_INIT_SEARCH;
// } // }
// Default 1 // Default 1
data_rate = NRF24L01_BR_1M; data_rate = NRF24L01_BR_1M;
rf_channel = 10; rf_ch_num = 10;
// Default 2 // Default 2
// data_rate = NRF24L01_BR_2M; // data_rate = NRF24L01_BR_2M;
// rf_channel = 110; // rf_ch_num = 110;
return CFLIE_INIT_SEARCH; return CFLIE_INIT_SEARCH;
} }

28
Multiprotocol/Multiprotocol.ino
View File

@ -23,7 +23,7 @@
#include <avr/pgmspace.h> #include <avr/pgmspace.h>
//#define DEBUG_PIN // Use pin TX for AVR and SPI_CS for STM32 => DEBUG_PIN_on, DEBUG_PIN_off, DEBUG_PIN_toggle //#define DEBUG_PIN // Use pin TX for AVR and SPI_CS for STM32 => DEBUG_PIN_on, DEBUG_PIN_off, DEBUG_PIN_toggle
// #define DEBUG_SERIAL // Only for STM32_BOARD compiled with Upload method "Serial"->usart1, "STM32duino bootloader"->USB serial //#define DEBUG_SERIAL // Only for STM32_BOARD compiled with Upload method "Serial"->usart1, "STM32duino bootloader"->USB serial
#ifdef __arm__ // Let's automatically select the board if arm is selected #ifdef __arm__ // Let's automatically select the board if arm is selected
#define STM32_BOARD #define STM32_BOARD
@ -1191,6 +1191,13 @@ void update_serial_data()
{ {
RX_DONOTUPDATE_on; RX_DONOTUPDATE_on;
RX_FLAG_off; //data is being processed RX_FLAG_off; //data is being processed
#ifdef SAMSON // Extremely dangerous, do not enable this unless you know what you are doing...
if( rx_ok_buff[0]==0x55 && (rx_ok_buff[1]&0x1F)==PROTO_FRSKYD && rx_ok_buff[2]==0x7F && rx_ok_buff[24]==217 && rx_ok_buff[25]==202 )
{//proto==FRSKYD+sub==7+rx_num==7+CH15==73%+CH16==73%
rx_ok_buff[1]=(rx_ok_buff[1]&0xE0) | PROTO_FLYSKY; // change the protocol to Flysky
memcpy((void*)(rx_ok_buff+4),(void*)(rx_ok_buff+4+11),11); // reassign channels 9-16 to 1-8
}
#endif
if(rx_ok_buff[1]&0x20) //check range if(rx_ok_buff[1]&0x20) //check range
RANGE_FLAG_on; RANGE_FLAG_on;
else else
@ -1565,26 +1572,25 @@ static uint32_t random_id(uint16_t address, uint8_t create_new)
id|=eeprom_read_byte((EE_ADDR)address+i-1); id|=eeprom_read_byte((EE_ADDR)address+i-1);
} }
if(id!=0x2AD141A7) //ID with seed=0 if(id!=0x2AD141A7) //ID with seed=0
{ {
debugln("Read ID from EEPROM"); debugln("Read ID from EEPROM");
return id; return id;
} }
} }
// Generate a random ID // Generate a random ID
#if defined STM32_BOARD #if defined STM32_BOARD
#define STM32_UUID ((uint32_t *)0x1FFFF7E8) #define STM32_UUID ((uint32_t *)0x1FFFF7E8)
if (!create_new) { if (!create_new)
{
id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2]; id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2];
debugln("Generated ID from STM32 UUID"); debugln("Generated ID from STM32 UUID");
}
} else else
#endif #endif
id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16); id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16);
for(uint8_t i=0;i<4;i++) for(uint8_t i=0;i<4;i++)
{
eeprom_write_byte((EE_ADDR)address+i,id >> (i*8)); eeprom_write_byte((EE_ADDR)address+i,id >> (i*8));
}
eeprom_write_byte((EE_ADDR)(address+10),0xf0);//write bind flag in eeprom. eeprom_write_byte((EE_ADDR)(address+10),0xf0);//write bind flag in eeprom.
return id; return id;
#else #else

2
Multiprotocol/_Config.h
View File

@ -188,7 +188,7 @@
#define CABELL_NRF24L01_INO #define CABELL_NRF24L01_INO
#define ESKY150_NRF24L01_INO #define ESKY150_NRF24L01_INO
#define H8_3D_NRF24L01_INO #define H8_3D_NRF24L01_INO
#define CFLIE_NRF24L01_INO #define CFLIE_NRF24L01_INO
/**************************/ /**************************/