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

DEVO: full telemetry

Browse Source
This commit is contained in:
Pascal Langer 2021-03-01 13:47:44 +01:00
parent f19bb05c7a
commit d36af55b84
5 changed files with 153 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
Multiprotocol/Devo_cyrf6936.ino
View File

@ -163,6 +163,35 @@ static void __attribute__((unused)) DEVO_build_data_pkt()
}
#if defined DEVO_HUB_TELEMETRY
static uint32_t __attribute__((unused)) DEVO_text_to_int(uint8_t *ptr, uint8_t len)
{
uint32_t value = 0;
for(uint8_t i = 0; i < len; i++)
value = value * 10 + (ptr[i] - '0');
return value;
}
static void __attribute__((unused)) DEVO_float_to_ints(uint8_t *ptr, uint16_t *value, uint16_t *decimal)
{
bool seen_decimal = false;
*value = 0;
*decimal = 0;
for(uint8_t i = 0; i < 7; i++)
{
if(ptr[i] == '.')
{
seen_decimal = true;
continue;
}
if(ptr[i] == 0)
break;
if(seen_decimal)
*decimal = *decimal * 10 + (ptr[i] - '0');
else
*value = *value * 10 + (ptr[i] - '0');
}
}
static void __attribute__((unused)) DEVO_parse_telemetry_packet()
{
DEVO_scramble_pkt(); //This will unscramble the packet
@ -177,14 +206,55 @@ static void __attribute__((unused)) DEVO_parse_telemetry_packet()
RX_RSSI = TX_RSSI;
telemetry_link = 1;
//TODO: FW telemetry https://github.com/DeviationTX/deviation/blob/5efb6a28bea697af9a61b5a0ed2528cc8d203f90/src/protocol/devo_cyrf6936.c#L232
debug("P[0]=%02X",packet[0]);
if (packet[0] == 0x30) // Volt packet
//Telemetry https://github.com/DeviationTX/deviation/blob/5efb6a28bea697af9a61b5a0ed2528cc8d203f90/src/protocol/devo_cyrf6936.c#L232
uint16_t val, dec;
switch(packet[0])
{
v_lipo1 = packet[1] << 1;
v_lipo2 = packet[3] << 1;
case 0x30: // Volt and RPM packet
v_lipo1 = packet[1] << 1;
v_lipo2 = packet[3] << 1;
val = packet[7] * 120; //In RPM
frsky_send_user_frame(0x03, val, val>>8); // RPM
break;
case 0x31: // Temperature packet
if(packet[1]!=0xff)
frsky_send_user_frame(0x02, packet[1], 0x00); // temp1
if(packet[2]!=0xff)
frsky_send_user_frame(0x05, packet[2], 0x00); // temp2
break;
// GPS Data
case 0x32: // Longitude
val = DEVO_text_to_int(&packet[1], 3)*100 + DEVO_text_to_int(&packet[4], 2); // dddmm
frsky_send_user_frame(0x12 , val, val>>8);
val = DEVO_text_to_int(&packet[7], 4); // .mmmm
frsky_send_user_frame(0x12+8, val, val>>8);
frsky_send_user_frame(0x1A+8, packet[11], 0x00); // 'E'/'W'
break;
case 0x33: // Latitude
val = DEVO_text_to_int(&packet[1], 2)*100 + DEVO_text_to_int(&packet[3], 2); // ddmm
frsky_send_user_frame(0x13 , val, val>>8);
val = DEVO_text_to_int(&packet[6], 4); // .mmmm
frsky_send_user_frame(0x13+8, val, val>>8);
frsky_send_user_frame(0x1B+8, packet[10], 0x00); // 'N'/'S'
break;
case 0x34: // Altitude
DEVO_float_to_ints(&packet[1], &val, &dec);
frsky_send_user_frame(0x10, val, val>>8);
frsky_send_user_frame(0x21, dec, dec>>8);
break;
case 0x35: // Speed
DEVO_float_to_ints(&packet[7], &val, &dec);
frsky_send_user_frame(0x11 , val, val>>8);
frsky_send_user_frame(0x11+8, dec, dec>>8);
break;
case 0x36: // Time
frsky_send_user_frame(0x15, DEVO_text_to_int(&packet[9], 2), DEVO_text_to_int(&packet[7], 2)); // month, day
frsky_send_user_frame(0x16, DEVO_text_to_int(&packet[11], 2), 0x00); // year
frsky_send_user_frame(0x17, DEVO_text_to_int(&packet[1], 2), DEVO_text_to_int(&packet[3], 2)); // hour, min
frsky_send_user_frame(0x18, DEVO_text_to_int(&packet[5], 2), 0x00); // second
break;
}
}
#endif

3
Multiprotocol/Multi_Protos.ino
View File

@ -173,6 +173,9 @@ enum
const mm_protocol_definition multi_protocols[] = {
// Protocol number, Protocol String, Sub_protocol strings, Number of sub_protocols, Option type, Failsafe, ChMap, RF switch, Init, Callback
#if defined(MULTI_CONFIG_INO)
{PROTO_CONFIG, STR_CONFIG, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, 0, CONFIG_init, CONFIG_callback },
#endif
#if defined(ASSAN_NRF24L01_INO)
{PROTO_ASSAN, STR_ASSAN, NO_SUBTYPE, 0, OPTION_NONE, 0, 0, SW_NRF, ASSAN_init, ASSAN_callback },
#endif

7
Multiprotocol/Multiprotocol.h
View File

@ -19,14 +19,16 @@
#define VERSION_MAJOR 1
#define VERSION_MINOR 3
#define VERSION_REVISION 2
#define VERSION_PATCH_LEVEL 51
#define VERSION_PATCH_LEVEL 52
#define MODE_SERIAL 0
//******************
// Protocols
//******************
enum PROTOCOLS
{
MODE_SERIAL = 0, // Serial commands
PROTO_CONFIG = 0, // Module config
PROTO_FLYSKY = 1, // =>A7105
PROTO_HUBSAN = 2, // =>A7105
PROTO_FRSKYD = 3, // =>CC2500
@ -489,6 +491,7 @@ enum MultiPacketTypes
MULTI_TELEMETRY_RX_CHANNELS = 13,
MULTI_TELEMETRY_HOTT = 14,
MULTI_TELEMETRY_MLINK = 15,
MULTI_TELEMETRY_CONFIG = 16,
};
// Macros

11
Multiprotocol/Multiprotocol.ino
View File

@ -257,6 +257,10 @@ uint8_t packet_in[TELEMETRY_BUFFER_SIZE];//telemetry receiving packets
uint8_t DSM_SerialRX_val[7];
bool DSM_SerialRX=false;
#endif
#ifdef MULTI_CONFIG_INO
uint8_t CONFIG_SerialRX_val[7];
bool CONFIG_SerialRX=false;
#endif
#endif // TELEMETRY
uint8_t multi_protocols_index=0xFF;
@ -1503,6 +1507,13 @@ void update_serial_data()
DSM_SerialRX=true;
}
#endif
#ifdef MULTI_CONFIG_INO
if(protocol==PROTO_CONFIG && rx_len==27+7)
{//Protocol waiting for 7 bytes
memcpy(CONFIG_SerialRX_val, (const void *)&rx_ok_buff[27],7);
CONFIG_SerialRX=true;
}
#endif
}
RX_DONOTUPDATE_off;

64
Multiprotocol/Telemetry.ino
View File

@ -53,6 +53,14 @@ uint8_t RetrySequence ;
#if defined HUB_TELEMETRY
#define USER_MAX_BYTES 6
uint8_t prev_index;
struct t_FrSkyD_User_Frame
{
uint8_t ID;
uint8_t low;
uint8_t high;
} FrSkyD_User_Frame[8];
uint8_t FrSkyD_User_Frame_Start=0, FrSkyD_User_Frame_End=0;
#endif // HUB_TELEMETRY
#define START_STOP 0x7e
@ -223,6 +231,15 @@ static void multi_send_status()
}
}
#ifdef MULTI_CONFIG_INO
void CONFIG_frame()
{
multi_send_header(MULTI_TELEMETRY_CONFIG, packet_in[0]);
for (uint8_t i = 1; i <= packet_in[0]; i++) // config data
Serial_write(packet_in[i]);
}
#endif
#ifdef MLINK_FW_TELEMETRY
void MLINK_frame()
{
@ -498,6 +515,9 @@ void init_frskyd_link_telemetry()
TX_RSSI=0;
RX_LQI=0;
TX_LQI=0;
#if defined HUB_TELEMETRY
FrSkyD_User_Frame_Start=FrSkyD_User_Frame_End=0;
#endif
}
void frsky_link_frame()
@ -575,7 +595,7 @@ packet_in[6]|(counter++)|00 01 02 03 04 05 06 07 08 09
0A 0F 5E 3A 06 00 5E 5E 3B 09 00 5E
05 10 5E 06 16 72 5E 5E 3A 06 00 5E
*/
static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t low, uint8_t high)
static void __attribute__((unused)) frsky_write_user_frame(uint8_t ID, uint8_t low, uint8_t high)
{
telemetry_in_buffer[6] = 0x04; // number of bytes in the payload
telemetry_in_buffer[7] = 0x00; // unknown?
@ -596,7 +616,32 @@ static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t lo
telemetry_in_buffer[pos++] = value;
value = high;
}
telemetry_link |= 2; // request to send frame
telemetry_link |= 2; // request to send frame
}
static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t low, uint8_t high)
{
if(telemetry_link&2)
{ // add to buffer
uint8_t test = (FrSkyD_User_Frame_End + 1) & 0x07;
if(test == FrSkyD_User_Frame_Start)
return; // buffer full...
FrSkyD_User_Frame_End = test;
FrSkyD_User_Frame[FrSkyD_User_Frame_End].ID = ID;
FrSkyD_User_Frame[FrSkyD_User_Frame_End].low = low;
FrSkyD_User_Frame[FrSkyD_User_Frame_End].high = high;
}
else // send to TX direct
frsky_write_user_frame(ID, low, high);
}
static void __attribute__((unused)) frsky_check_user_frame()
{
if(telemetry_link&2 || FrSkyD_User_Frame_Start == FrSkyD_User_Frame_End)
return; // need to wait that the last frame is sent or buffer is empty
frsky_write_user_frame(FrSkyD_User_Frame[FrSkyD_User_Frame_Start].ID, FrSkyD_User_Frame[FrSkyD_User_Frame_Start].low, FrSkyD_User_Frame[FrSkyD_User_Frame_Start].high);
FrSkyD_User_Frame_Start++;
FrSkyD_User_Frame_Start &= 0x07;
}
#endif
@ -905,6 +950,14 @@ void TelemetryUpdate()
#endif // SPORT_TELEMETRY
#ifdef MULTI_TELEMETRY
#if defined MULTI_CONFIG_INO
if(telemetry_link && protocol == PROTO_CONFIG)
{
CONFIG_frame();
telemetry_link=0;
return;
}
#endif
#if defined MLINK_FW_TELEMETRY
if(telemetry_link && protocol == PROTO_MLINK)
{
@ -964,15 +1017,16 @@ void TelemetryUpdate()
#endif //MULTI_TELEMETRY
if( telemetry_link & 1 )
{ // FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell + Hitec + Bugs + BugsMini + NCC1701 + PROPEL + RLINK + OMP + MLINK
{ // FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell + Hitec + Bugs + BugsMini + NCC1701 + PROPEL + RLINK + OMP + MLINK + DEVO
// FrSkyX telemetry if in PPM
frsky_link_frame();
return;
}
#if defined HUB_TELEMETRY
if((telemetry_link & 2) && ( protocol == PROTO_FRSKYD || protocol == PROTO_BAYANG || protocol == PROTO_MLINK ) )
{ // FrSkyD + Bayang + MLINK
if((telemetry_link & 2) && ( protocol == PROTO_FRSKYD || protocol == PROTO_BAYANG || protocol == PROTO_MLINK || protocol == PROTO_DEVO ) )
{ // FrSkyD + Bayang + MLINK + DEVO
frsky_user_frame();
frsky_check_user_frame();
return;
}
#endif