Skip to content

Commit f512a5d

Browse files
chswchsw
committed
Respond to telemetry sensor reqest
Updated code so that it responds with the correct sesnor information for the sensor id's I know of. This change plus the addition of a total cell count fixes the cell/cells-problem introduced in the later open-tx version.
1 parent 823164a commit f512a5d

File tree

3 files changed

+172
-78
lines changed

3 files changed

+172
-78
lines changed

‎MavLink_FrSkySPort/FrSkySPort.h

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,9 @@
11
// Frsky Sensor-ID to use.
2-
#define SENSOR_ID1 0x1B // ID of sensor. Must be something that is polled by FrSky RX
3-
#define SENSOR_ID2 0x0D
4-
#define SENSOR_ID3 0x34
5-
#define SENSOR_ID4 0x67
2+
#define SENSOR_ID_FLVSS 0x1B
3+
#define SENSOR_ID_VARIO 0x00
4+
#define SENSOR_ID_FAS 0x22
5+
#define SENSOR_ID_GPS 0x83
6+
#define SENSOR_ID_RPM 0xE4
67
// Frsky-specific
78
#define START_STOP 0x7e
89
#define DATA_FRAME 0x10

‎MavLink_FrSkySPort/FrSkySPort.ino

Lines changed: 166 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -4,16 +4,19 @@
44
#define _FrSkySPort_C1 UART0_C1
55
#define _FrSkySPort_C3 UART0_C3
66
#define _FrSkySPort_S2 UART0_S2
7-
#define _FrSkySPort_BAUD 57600
8-
#define MAX_ID_COUNT 19
7+
#define _FrSkySPort_BAUD 57600
98

109
short crc; // used for crc calc of frsky-packet
11-
uint8_t lastRx;
12-
uint32_t FR_ID_count = 0;
10+
boolean waitingForSensorId = false;
1311
uint8_t cell_count = 0;
1412
uint8_t latlong_flag = 0;
1513
uint32_t latlong = 0;
16-
uint8_t first=0;
14+
15+
uint8_t nextFLVSS = 0;
16+
uint8_t nextFAS = 0;
17+
uint8_t nextVARIO = 0;
18+
uint8_t nextGPS = 0;
19+
uint8_t nextDefault = 0;
1720
// ***********************************************************************
1821
void FrSkySPort_Init(void) {
1922
_FrSkySPort_Serial.begin(_FrSkySPort_BAUD);
@@ -31,25 +34,101 @@ void FrSkySPort_Process(void) {
3134
while ( _FrSkySPort_Serial.available())
3235
{
3336
data = _FrSkySPort_Serial.read();
34-
if (lastRx == START_STOP && ((data == SENSOR_ID1) || (data == SENSOR_ID2) || (data == SENSOR_ID3) || (data == SENSOR_ID4)))
37+
38+
if(data == START_STOP)
3539
{
40+
waitingForSensorId = true;
41+
continue;
42+
}
43+
if(!waitingForSensorId)
44+
continue;
3645

37-
switch(FR_ID_count) {
46+
FrSkySPort_ProcessSensorRequest(data);
47+
48+
waitingForSensorId = false;
49+
}
50+
}
51+
52+
// ***********************************************************************
53+
void FrSkySPort_ProcessSensorRequest(uint8_t sensorId)
54+
{
55+
uint32_t temp=0;
56+
uint8_t offset;
57+
switch(sensorId)
58+
{
59+
case SENSOR_ID_FLVSS:
60+
{
61+
printDebugPackageSend("FLVSS", nextFLVSS+1, 3);
62+
switch(nextFLVSS)
63+
{
3864
case 0:
39-
if(ap_fixtype==3) {
40-
FrSkySPort_SendPackage(FR_ID_SPEED,ap_groundspeed *20 ); // from GPS converted to km/h
65+
if(ap_cell_count > 0)
66+
{
67+
// First 2 cells
68+
offset = 0x00 | ((ap_cell_count & 0xF)<<4);
69+
temp=((ap_voltage_battery/(ap_cell_count * 2)) & 0xFFF);
70+
FrSkySPort_SendPackage(FR_ID_CELLS,(temp << 20) | (temp << 8) | offset); // Battery cell 0,1
4171
}
4272
break;
43-
case 1:
44-
FrSkySPort_SendPackage(FR_ID_RPM,ap_throttle * 2); // * 2 if number of blades on Taranis is set to 2
73+
case 1:
74+
// Optional 3 and 4 Cells
75+
if(ap_cell_count > 2) {
76+
offset = 0x02 | ((ap_cell_count & 0xF)<<4);
77+
temp=((ap_voltage_battery/(ap_cell_count * 2)) & 0xFFF);
78+
FrSkySPort_SendPackage(FR_ID_CELLS,(temp << 20) | (temp << 8) | offset); // Battery cell 2,3
79+
}
4580
break;
46-
case 2:
47-
FrSkySPort_SendPackage(FR_ID_CURRENT,ap_current_battery / 10);
48-
break;
49-
case 3: // Sends the altitude value from barometer, first sent value used as zero altitude
81+
case 2: // Optional 5 and 6 Cells
82+
if(ap_cell_count > 4) {
83+
offset = 0x04 | ((ap_cell_count & 0xF)<<4);
84+
temp=((ap_voltage_battery/(ap_cell_count * 2)) & 0xFFF);
85+
FrSkySPort_SendPackage(FR_ID_CELLS,(temp << 20) | (temp << 8) | offset); // Battery cell 2,3
86+
}
87+
break;
88+
}
89+
nextFLVSS++;
90+
if(nextFLVSS>2)
91+
nextFLVSS=0;
92+
}
93+
break;
94+
case SENSOR_ID_VARIO:
95+
{
96+
printDebugPackageSend("VARIO", nextVARIO+1, 2);
97+
switch(nextVARIO)
98+
{
99+
case 0:
100+
FrSkySPort_SendPackage(FR_ID_VARIO,ap_climb_rate ); // 100 = 1m/s
101+
break;
102+
case 1:
50103
FrSkySPort_SendPackage(FR_ID_ALTITUDE,ap_bar_altitude); // from barometer, 100 = 1m
51-
break;
52-
case 4: // Sends the ap_longitude value, setting bit 31 high
104+
break;
105+
}
106+
if(++nextVARIO > 1)
107+
nextVARIO = 0;
108+
}
109+
break;
110+
case SENSOR_ID_FAS:
111+
{
112+
printDebugPackageSend("FAS", nextFAS+1, 2);
113+
switch(nextFAS)
114+
{
115+
case 0:
116+
FrSkySPort_SendPackage(FR_ID_CURRENT,ap_current_battery / 10);
117+
break;
118+
case 1:
119+
FrSkySPort_SendPackage(FR_ID_VFAS,ap_voltage_battery/10); // Sends voltage as a VFAS value
120+
break;
121+
}
122+
if(++nextFAS > 1)
123+
nextFAS = 0;
124+
}
125+
break;
126+
case SENSOR_ID_GPS:
127+
{
128+
printDebugPackageSend("GPS", nextGPS+1, 4);
129+
switch(nextGPS)
130+
{
131+
case 0: // Sends the ap_longitude value, setting bit 31 high
53132
if(ap_fixtype==3) {
54133
if(ap_longitude < 0)
55134
latlong=((abs(ap_longitude)/100)*6) | 0xC0000000;
@@ -58,7 +137,7 @@ void FrSkySPort_Process(void) {
58137
FrSkySPort_SendPackage(FR_ID_LATLONG,latlong);
59138
}
60139
break;
61-
case 5: // Sends the ap_latitude value, setting bit 31 low
140+
case 1: // Sends the ap_latitude value, setting bit 31 low
62141
if(ap_fixtype==3) {
63142
if(ap_latitude < 0 )
64143
latlong=((abs(ap_latitude)/100)*6) | 0x40000000;
@@ -67,68 +146,82 @@ void FrSkySPort_Process(void) {
67146
FrSkySPort_SendPackage(FR_ID_LATLONG,latlong);
68147
}
69148
break;
70-
case 6: // Sends the compass heading
71-
FrSkySPort_SendPackage(FR_ID_HEADING,ap_heading * 100); // 10000 = 100 deg
72-
break;
73-
case 7: // Sends the analog value from input A0 on Teensy 3.1
74-
FrSkySPort_SendPackage(FR_ID_ADC2,adc2);
75-
break;
76-
case 8: // First 2 cells
77-
temp=((ap_voltage_battery/(ap_cell_count * 2)) & 0xFFF);
78-
FrSkySPort_SendPackage(FR_ID_CELLS,(temp << 20) | (temp << 8)); // Battery cell 0,1
79-
break;
80-
case 9: // Optional 3 and 4 Cells
81-
if(ap_cell_count > 2) {
82-
offset = ap_cell_count > 3 ? 0x02: 0x01;
83-
temp=((ap_voltage_battery/(ap_cell_count * 2)) & 0xFFF);
84-
FrSkySPort_SendPackage(FR_ID_CELLS,(temp << 20) | (temp << 8) | offset); // Battery cell 2,3
149+
case 2:
150+
if(ap_fixtype==3) {
151+
FrSkySPort_SendPackage(FR_ID_GPS_ALT,ap_gps_altitude / 10); // from GPS, 100=1m
85152
}
86-
break;
87-
case 10: // Optional 5 and 6 Cells
88-
if(ap_cell_count > 4) {
89-
offset = ap_cell_count > 5 ? 0x04: 0x03;
90-
temp=((ap_voltage_battery/(ap_cell_count * 2)) & 0xFFF);
91-
FrSkySPort_SendPackage(FR_ID_CELLS,(temp << 20) | (temp << 8) | offset); // Battery cell 2,3
153+
case 3:
154+
if(ap_fixtype==3) {
155+
// FrSkySPort_SendPackage(FR_ID_SPEED,ap_groundspeed *20 ); // from GPS converted to km/h
156+
FrSkySPort_SendPackage(FR_ID_SPEED,ap_gps_speed *20 ); // from GPS converted to km/h
92157
}
93-
break;
94-
case 11:
95-
FrSkySPort_SendPackage(FR_ID_ACCX,ap_accX_old - ap_accX);
96-
break;
97-
case 12:
98-
FrSkySPort_SendPackage(FR_ID_ACCY,ap_accY_old - ap_accY);
99-
break;
100-
case 13:
101-
FrSkySPort_SendPackage(FR_ID_ACCZ,ap_accZ_old - ap_accZ );
102-
break;
103-
case 14: // Sends voltage as a VFAS value
104-
FrSkySPort_SendPackage(FR_ID_VFAS,ap_voltage_battery/10);
105-
break;
106-
case 15:
107-
FrSkySPort_SendPackage(FR_ID_T1,gps_status);
108-
break;
109-
case 16:
110-
FrSkySPort_SendPackage(FR_ID_T2,ap_base_mode);
111-
break;
112-
case 17:
113-
FrSkySPort_SendPackage(FR_ID_VARIO,ap_climb_rate ); // 100 = 1m/s
114-
break;
115-
case 18:
116-
//if(ap_fixtype==3) {
117-
FrSkySPort_SendPackage(FR_ID_GPS_ALT,ap_gps_altitude / 10); // from GPS, 100=1m
118-
// }
119-
break;
120-
case 19:
121-
FrSkySPort_SendPackage(FR_ID_FUEL,ap_custom_mode);
122-
break;
123-
124158
}
125-
FR_ID_count++;
126-
if(FR_ID_count > MAX_ID_COUNT) FR_ID_count = 0;
159+
if(++nextGPS > 3)
160+
nextGPS = 0;
161+
}
162+
break;
163+
case SENSOR_ID_RPM:
164+
printDebugPackageSend("RPM", 1, 1);
165+
FrSkySPort_SendPackage(FR_ID_RPM,ap_throttle * 2); // * 2 if number of blades on Taranis is set to 2
166+
break;
167+
// Since I don't know the app-id for these values, I just use these two "random"
168+
case 0x45:
169+
case 0xC6:
170+
switch(nextDefault)
171+
{
172+
case 0: // Sends the compass heading
173+
FrSkySPort_SendPackage(FR_ID_HEADING,ap_heading * 100); // 10000 = 100 deg
174+
break;
175+
case 1: // Sends the analog value from input A0 on Teensy 3.1
176+
FrSkySPort_SendPackage(FR_ID_ADC2,adc2);
177+
break;
178+
case 2:
179+
FrSkySPort_SendPackage(FR_ID_ACCX,ap_accX_old - ap_accX);
180+
break;
181+
case 3:
182+
FrSkySPort_SendPackage(FR_ID_ACCY,ap_accY_old - ap_accY);
183+
break;
184+
case 4:
185+
FrSkySPort_SendPackage(FR_ID_ACCZ,ap_accZ_old - ap_accZ);
186+
break;
187+
case 5:
188+
FrSkySPort_SendPackage(FR_ID_T1,gps_status);
189+
break;
190+
case 6:
191+
FrSkySPort_SendPackage(FR_ID_T2,ap_base_mode);
192+
break;
193+
case 7:
194+
FrSkySPort_SendPackage(FR_ID_FUEL,ap_custom_mode);
195+
break;
127196
}
128-
lastRx=data;
197+
if(++nextDefault > 7)
198+
nextDefault = 0;
199+
default:
200+
#ifdef DEBUG_FRSKY_SENSOR_REQUEST
201+
debugSerial.print(millis());
202+
debugSerial.print("\tRequested data for unsupported appId: ");
203+
debugSerial.print(sensorId, HEX);
204+
debugSerial.println();
205+
#endif
129206
}
130207
}
131208

209+
// ***********************************************************************
210+
void printDebugPackageSend(char* pkg_name, uint8_t pkg_nr, uint8_t pkg_max)
211+
{
212+
#ifdef DEBUG_FRSKY_SENSOR_REQUEST
213+
debugSerial.print(millis());
214+
debugSerial.print("\tCreating frsky package for ");
215+
debugSerial.print(pkg_name);
216+
debugSerial.print(" (");
217+
debugSerial.print(pkg_nr);
218+
debugSerial.print("/");
219+
debugSerial.print(pkg_max);
220+
debugSerial.print(")");
221+
debugSerial.println();
222+
#endif
223+
}
224+
132225

133226
// ***********************************************************************
134227
void FrSkySPort_SendByte(uint8_t byte) {
@@ -172,6 +265,5 @@ void FrSkySPort_SendPackage(uint16_t id, uint32_t value) {
172265
_FrSkySPort_C3 ^= 32; // Transmit direction, from S.Port
173266

174267
digitalWrite(led,LOW);
175-
176268
}
177269

‎MavLink_FrSkySPort/MavLink_FrSkySPort.ino

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,7 @@ APM2.5 Mavlink to FrSky X8R SPort interface using Teensy 3.1 http://www.pjrc.co
5656
#define DEBUG_GPS_RAW
5757
#define DEBUG_ACC
5858
#define DEBUG_BAT
59+
#define DEBUG_FRSKY_SENSOR_REQUEST
5960

6061

6162
// ******************************************

0 commit comments

Comments
 (0)