Skip to content

Commit 5d69021

Browse files
AlessandroAUAlessandroAU
committed
add gps tlm processing
1 parent afc7009 commit 5d69021

6 files changed

Lines changed: 137 additions & 130 deletions

File tree

‎src/lib/CRSF/CRSF.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,7 @@ volatile uint8_t CRSF::ParameterUpdateData[2] = {0};
8080
volatile crsf_channels_s CRSF::PackedRCdataOut;
8181
volatile crsfPayloadLinkstatistics_s CRSF::LinkStatistics;
8282
volatile crsf_sensor_battery_s CRSF::TLMbattSensor;
83+
volatile crsf_sensor_gps_s CRSF::TLMGPSsensor;
8384

8485
volatile uint32_t CRSF::RCdataLastRecv = 0;
8586
volatile int32_t CRSF::OpenTXsyncOffset = 0;

‎src/lib/CRSF/CRSF.h‎

Lines changed: 12 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -207,8 +207,6 @@ union inBuffer_U
207207
rcPacket_t asRCPacket_t; // access the memory as RC data
208208
// add other packet types here
209209
};
210-
211-
212210
typedef struct crsf_channels_s crsf_channels_t;
213211

214212
// Used by extended header frames (type in range 0x28 to 0x96)
@@ -219,9 +217,19 @@ typedef struct crsf_sensor_battery_s
219217
unsigned capacity : 24; // mah
220218
unsigned remaining : 8; // %
221219
} PACKED crsf_sensor_battery_s;
222-
223220
typedef struct crsf_sensor_battery_s crsf_sensor_battery_t;
224221

222+
typedef struct crsf_sensor_gps_s
223+
{
224+
int32_t latitude : 32;
225+
int32_t longitude : 32;
226+
uint16_t speed : 16;
227+
uint16_t headng : 16;
228+
uint16_t alt : 16;
229+
uint8_t sats : 8;
230+
} PACKED crsf_sensor_gps_s;
231+
typedef struct crsf_sensor_gps_s crsf_sensor_gps_t;
232+
225233
/*
226234
* 0x14 Link statistics
227235
* Payload:
@@ -350,6 +358,7 @@ class CRSF
350358
static volatile crsf_channels_s PackedRCdataOut; // RC data in packed format for output.
351359
static volatile crsfPayloadLinkstatistics_s LinkStatistics; // Link Statisitics Stored as Struct
352360
static volatile crsf_sensor_battery_s TLMbattSensor;
361+
static volatile crsf_sensor_gps_s TLMGPSsensor;
353362

354363
static void Begin(); //setup timers etc
355364
static void End(); //stop timers etc

‎src/src/STM32_UARTinHandler.h‎

Lines changed: 0 additions & 125 deletions
This file was deleted.

‎src/src/rx_main.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ SX1280Driver Radio;
2929
#endif
3030

3131
#ifdef PLATFORM_STM32
32-
#include "STM32_UARTinHandler.h"
32+
#include "rx_uart_in.h"
3333
#endif
3434

3535
#ifdef TARGET_RX_GHOST_ATTO_V1

‎src/src/rx_uart_in.h‎

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
1+
#include "CRSF.h"
2+
#include "targets.h"
3+
4+
extern CRSF crsf;
5+
6+
uint8_t UARTinPacketPtr;
7+
uint8_t UARTinPacketLen;
8+
uint32_t UARTinLastDataTime;
9+
uint32_t UARTinLastPacketTime;
10+
bool UARTframeActive = false;
11+
12+
uint8_t UARTinBuffer[256];
13+
14+
void RX_UARTinProcessPacket()
15+
{
16+
if (UARTinBuffer[2] == CRSF_FRAMETYPE_COMMAND)
17+
{
18+
#ifdef PLATFORM_STM32
19+
Serial.println("Got CMD Packet");
20+
if (UARTinBuffer[3] == 0x62 && UARTinBuffer[4] == 0x6c)
21+
{
22+
delay(100);
23+
Serial.println("Jumping to Bootloader...");
24+
delay(100);
25+
HAL_NVIC_SystemReset();
26+
}
27+
#endif
28+
}
29+
30+
if (UARTinBuffer[2] == CRSF_FRAMETYPE_BATTERY_SENSOR)
31+
{
32+
crsf.TLMbattSensor.voltage = (UARTinBuffer[3] << 8) + UARTinBuffer[4];
33+
crsf.TLMbattSensor.current = (UARTinBuffer[5] << 8) + UARTinBuffer[6];
34+
crsf.TLMbattSensor.capacity = (UARTinBuffer[7] << 16) + (UARTinBuffer[8] << 8) + UARTinBuffer[9];
35+
crsf.TLMbattSensor.remaining = UARTinBuffer[9];
36+
}
37+
38+
if (UARTinBuffer[2] == CRSF_FRAMETYPE_GPS)
39+
{
40+
crsf.TLMGPSsensor.latitude = (UARTinBuffer[3] << 24) + (UARTinBuffer[4] << 16)+ (UARTinBuffer[5] << 8) + (UARTinBuffer[6] << 0);
41+
crsf.TLMGPSsensor.longitude = (UARTinBuffer[7] << 24) + (UARTinBuffer[8] << 16)+ (UARTinBuffer[9] << 8) + (UARTinBuffer[10] << 0);
42+
crsf.TLMGPSsensor.speed = (UARTinBuffer[11] << 8) + (UARTinBuffer[12] << 0);
43+
crsf.TLMGPSsensor.headng = (UARTinBuffer[13] << 8) + (UARTinBuffer[14] << 0);
44+
crsf.TLMGPSsensor.alt = (UARTinBuffer[15] << 8) + (UARTinBuffer[16] << 0);
45+
crsf.TLMGPSsensor.sats = (UARTinBuffer[17]);
46+
}
47+
48+
}
49+
50+
void RX_UARTinHandle()
51+
{
52+
while (Serial.available())
53+
{
54+
UARTinLastDataTime = millis();
55+
char inChar = Serial.read();
56+
57+
if (UARTframeActive == false)
58+
{
59+
// stage 1 wait for sync byte //
60+
if (inChar == CRSF_ADDRESS_CRSF_TRANSMITTER || inChar == CRSF_SYNC_BYTE) // we got sync, reset write pointer
61+
{
62+
UARTinPacketPtr = 0;
63+
UARTinPacketLen = 0;
64+
UARTframeActive = true;
65+
UARTinBuffer[UARTinPacketPtr] = inChar;
66+
UARTinPacketPtr++;
67+
}
68+
}
69+
else // frame is active so we do the processing
70+
{
71+
// first if things have gone wrong //
72+
if (UARTinPacketPtr > CRSF_MAX_PACKET_LEN - 1) // we reached the maximum allowable packet length, so start again because shit fucked up hey.
73+
{
74+
UARTinPacketPtr = 0;
75+
UARTframeActive = false;
76+
return;
77+
}
78+
79+
// special case where we save the expected pkt len to buffer //
80+
if (UARTinPacketPtr == 1)
81+
{
82+
if (inChar <= CRSF_MAX_PACKET_LEN)
83+
{
84+
UARTinPacketLen = inChar;
85+
}
86+
else
87+
{
88+
UARTinPacketPtr = 0;
89+
UARTinPacketLen = 0;
90+
UARTframeActive = false;
91+
return;
92+
}
93+
}
94+
95+
UARTinBuffer[UARTinPacketPtr] = inChar;
96+
UARTinPacketPtr++;
97+
98+
if (UARTinPacketPtr == UARTinPacketLen + 2) // plus 2 because the packlen is referenced from the start of the 'type' flag, IE there are an extra 2 bytes.
99+
{
100+
char CalculatedCRC = CalcCRC((uint8_t *)UARTinBuffer + 2, UARTinPacketPtr - 3);
101+
102+
if (CalculatedCRC == inChar)
103+
{
104+
UARTinLastPacketTime = millis();
105+
RX_UARTinProcessPacket();
106+
UARTinPacketPtr = 0;
107+
UARTframeActive = false;
108+
}
109+
else
110+
{
111+
UARTframeActive = false;
112+
UARTinPacketPtr = 0;
113+
Serial.println("UART in CRC failure");
114+
while (Serial.available())
115+
{
116+
Serial.read(); // empty the read buffer
117+
}
118+
}
119+
}
120+
}
121+
}
122+
}

‎src/user_defines.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515

1616
### REGULATORY DOMAIN: ###
1717

18-
#-DRegulatory_Domain_AU_915
18+
-DRegulatory_Domain_AU_915
1919
#-DRegulatory_Domain_EU_868
2020
#-DRegulatory_Domain_AU_433
2121
#-DRegulatory_Domain_EU_433

0 commit comments

Comments
 (0)