Переглянути джерело

Added FrSky telemetry decoding

Thomas Buck 7 роки тому
джерело
коміт
4bba710380
3 змінених файлів з 239 додано та 0 видалено
  1. 3
    0
      Saitek-X52-PPM.ino
  2. 141
    0
      frsky.cpp
  3. 95
    0
      frsky.h

+ 3
- 0
Saitek-X52-PPM.ino Переглянути файл

@@ -18,6 +18,7 @@
18 18
 #include "parser.h"
19 19
 #include "x52.h"
20 20
 #include "cppm.h"
21
+#include "frsky.h"
21 22
 
22 23
 #define ENABLE_SERIAL_PORT
23 24
 #define DEBUG_OUTPUT
@@ -31,6 +32,7 @@ JoystickEventsCPPM joyCPPM;
31 32
 JoystickEventsButtons joyButtons(&x52, (JoystickEvents*)&joyCPPM);
32 33
 JoystickEventsDeadZone joyevents((JoystickEvents*)&joyButtons);
33 34
 JoystickReportParser joy(&joyevents);
35
+FrSky frsky(&Serial);
34 36
 
35 37
 void setup() {
36 38
 #ifdef ENABLE_SERIAL_PORT
@@ -63,6 +65,7 @@ void init_joystick() {
63 65
 
64 66
 void loop() {
65 67
     usb.Task();
68
+    frsky.poll();
66 69
 
67 70
     static unsigned long lastTime = 0;
68 71
     static uint8_t initialized = 0;

+ 141
- 0
frsky.cpp Переглянути файл

@@ -0,0 +1,141 @@
1
+/*
2
+ * FrSky Telemetry Protocol Host implementation.
3
+ * Copyright 2016 by Thomas Buck <xythobuz@xythobuz.de>
4
+ *
5
+ * Based on the FrSky Telemetry Protocol documentation:
6
+ * http://www.frsky-rc.com/download/down.php?id=128
7
+ *
8
+ * This program is free software; you can redistribute it and/or
9
+ * modify it under the terms of the GNU General Public License as
10
+ * published by the Free Software Foundation, version 2.
11
+ */
12
+
13
+#include "frsky.h"
14
+
15
+#define DEBUG_OUTPUT Serial
16
+
17
+FrSky::FrSky(Stream* s) : serial(s), dataHandler(0),
18
+        alarmThresholdHandler(0), userDataHandler(0), bufferIndex(0) {
19
+    for (uint8_t i = 0; i < userDataSize; i++) {
20
+        userData[i] = 0;
21
+    }
22
+
23
+    for (uint8_t i = 0; i < bufferSize; i++) {
24
+        buffer[i] = 0;
25
+    }
26
+}
27
+
28
+void FrSky::poll() {
29
+    if ((!serial) || (!serial->available())) {
30
+        return;
31
+    }
32
+
33
+    uint8_t c = serial->read();
34
+    if (c == delimiter) {
35
+        if (bufferIndex < minPacketSize) {
36
+            bufferIndex = 0;
37
+        }
38
+        if (bufferIndex >= bufferSize) {
39
+            bufferIndex = bufferSize - 1;
40
+        }
41
+        buffer[bufferIndex++] = c;
42
+        if (bufferIndex > minPacketSize) {
43
+            handleMessage();
44
+            bufferIndex = 0;
45
+        }
46
+    } else if ((bufferIndex > 0) && (bufferIndex < bufferSize)) {
47
+        buffer[bufferIndex++] = c;
48
+    }
49
+}
50
+
51
+void FrSky::pollAlarms() {
52
+    serial->write(delimiter);
53
+    writeEscaped(idGetAlarms);
54
+    for (uint8_t i = 0; i < 8; i++) {
55
+        writeEscaped(0);
56
+    }
57
+    serial->write(delimiter);
58
+}
59
+
60
+void FrSky::setAlarm(AlarmThreshold alarm) {
61
+    uint8_t id = (alarm.id == analog1_1) ? idAlarm0
62
+            : ((alarm.id == analog1_2) ? idAlarm1
63
+            : ((alarm.id == analog2_1) ? idAlarm2 : idAlarm3));
64
+    serial->write(delimiter);
65
+    writeEscaped(id);
66
+    writeEscaped(alarm.value);
67
+    writeEscaped(alarm.dir);
68
+    writeEscaped(alarm.level);
69
+    for (uint8_t i = 0; i < 5; i++) {
70
+        writeEscaped(0);
71
+    }
72
+    serial->write(delimiter);
73
+}
74
+
75
+void FrSky::writeEscaped(uint8_t v) {
76
+    if ((v == delimiter) || (v == escape)) {
77
+        v ^= key;
78
+        serial->write(escape);
79
+    }
80
+    serial->write(v);
81
+}
82
+
83
+void FrSky::handleMessage() {
84
+#ifdef DEBUG_OUTPUT
85
+    DEBUG_OUTPUT.println("FrSky::handleMessage()");
86
+    for (uint8_t i = 0; i < bufferIndex; i++) {
87
+        DEBUG_OUTPUT.print(buffer[i], HEX);
88
+    }
89
+    DEBUG_OUTPUT.println();
90
+#endif
91
+
92
+    if ((buffer[0] != delimiter) || (buffer[bufferIndex - 1] != delimiter)) {
93
+#ifdef DEBUG_OUTPUT
94
+        DEBUG_OUTPUT.println("invalid packet begin/end!");
95
+#endif
96
+        return;
97
+    }
98
+
99
+    // Fix escaped bytes
100
+    for (uint8_t i = 0; i < (bufferIndex - 1); i++) {
101
+        if (buffer[i] == escape) {
102
+            buffer[i] = buffer[i + 1] ^ key;
103
+            for (uint8_t j = i + 1; j < (bufferIndex - 1); j++) {
104
+                buffer[j] = buffer[j + 1];
105
+            }
106
+            bufferIndex--;
107
+        }
108
+    }
109
+
110
+    if (buffer[1] == idVoltageQuality) {
111
+        if (dataHandler) {
112
+            dataHandler(buffer[2], buffer[3], buffer[4], buffer[5]);
113
+        }
114
+    } else if (buffer[1] == idUserData) {
115
+        uint8_t len = buffer[2];
116
+        if (len > userDataSize) {
117
+            len = userDataSize;
118
+        }
119
+        for (uint8_t i = 0; i < len; i++) {
120
+            userData[i] = buffer[i + 4];
121
+        }
122
+        if ((len > 0) && (userDataHandler)) {
123
+            userDataHandler(userData, len);
124
+        }
125
+    } else if ((buffer[1] == idAlarm0) || (buffer[1] == idAlarm1)
126
+            || (buffer[1] == idAlarm2) || (buffer[1] == idAlarm3)) {
127
+        AnalogValue v = (buffer[1] == idAlarm0) ? analog1_1
128
+                : ((buffer[1] == idAlarm1) ? analog1_2
129
+                : ((buffer[1] == idAlarm2) ? analog2_1 : analog2_2));
130
+        if (alarmThresholdHandler) {
131
+            alarmThresholdHandler(AlarmThreshold(v, (GreaterLessThan)buffer[3],
132
+                    (AlarmLevel)buffer[4], buffer[2]));
133
+        }
134
+    } else {
135
+#ifdef DEBUG_OUTPUT
136
+        DEBUG_OUTPUT.print("Unexpected ID: ");
137
+        DEBUG_OUTPUT.println(buffer[1], HEX);
138
+#endif
139
+    }
140
+}
141
+

+ 95
- 0
frsky.h Переглянути файл

@@ -0,0 +1,95 @@
1
+/*
2
+ * FrSky Telemetry Protocol Host implementation.
3
+ * Copyright 2016 by Thomas Buck <xythobuz@xythobuz.de>
4
+ *
5
+ * Based on the FrSky Telemetry Protocol documentation:
6
+ * http://www.frsky-rc.com/download/down.php?id=128
7
+ *
8
+ * This program is free software; you can redistribute it and/or
9
+ * modify it under the terms of the GNU General Public License as
10
+ * published by the Free Software Foundation, version 2.
11
+ */
12
+
13
+#ifndef __FRSKY_H__
14
+#define __FRSKY_H__
15
+
16
+#include <Arduino.h>
17
+
18
+class FrSky {
19
+  public:
20
+    enum AnalogValue {
21
+        analog1_1 = 0,
22
+        analog1_2 = 1,
23
+        analog2_1 = 2,
24
+        analog2_2 = 3
25
+    };
26
+
27
+    enum GreaterLessThan {
28
+        less = 0,
29
+        greater = 1
30
+    };
31
+
32
+    enum AlarmLevel {
33
+        disable = 0,
34
+        yellow = 1,
35
+        orange = 2,
36
+        red = 3
37
+    };
38
+
39
+    struct AlarmThreshold {
40
+        AlarmThreshold(AnalogValue i, GreaterLessThan d, AlarmLevel l, uint8_t v)
41
+                : id(i), dir(d), level(l), value(v) { }
42
+
43
+        AnalogValue id;
44
+        GreaterLessThan dir;
45
+        AlarmLevel level;
46
+        uint8_t value;
47
+    };
48
+
49
+    typedef void (*DataHandler)(uint8_t a1, uint8_t a2, uint8_t q1, uint8_t q2);
50
+    typedef void (*AlarmThresholdHandler)(AlarmThreshold alarm);
51
+    typedef void (*UserDataHandler)(const uint8_t* buf, uint8_t len);
52
+
53
+    // -------------------------------------------------------------------------------
54
+
55
+    FrSky(Stream* s);
56
+
57
+    void poll();
58
+    void pollAlarms();
59
+    void setAlarm(AlarmThreshold alarm);
60
+
61
+    void setDataHandler(DataHandler h) { dataHandler = h; }
62
+    void setAlarmThresholdHandler(AlarmThresholdHandler h) { alarmThresholdHandler = h; }
63
+    void setUserDataHandler(UserDataHandler h) { userDataHandler = h; }
64
+
65
+  private:
66
+    void handleMessage();
67
+    void writeEscaped(uint8_t v);
68
+
69
+    const static uint8_t bufferSize = 19;
70
+    const static uint8_t userDataSize = 6;
71
+    const static uint8_t minPacketSize = 11;
72
+    const static uint8_t delimiter = 0x7E;
73
+    const static uint8_t escape = 0x7D;
74
+    const static uint8_t key = 0x20;
75
+    const static uint8_t alarms = 4;
76
+    const static uint8_t idVoltageQuality = 0xFE;
77
+    const static uint8_t idUserData = 0xFD;
78
+    const static uint8_t idAlarm0 = 0xFC;
79
+    const static uint8_t idAlarm1 = 0xFB;
80
+    const static uint8_t idAlarm2 = 0xFA;
81
+    const static uint8_t idAlarm3 = 0xF9;
82
+    const static uint8_t idGetAlarms = 0xF8;
83
+
84
+    Stream* serial;
85
+    DataHandler dataHandler;
86
+    AlarmThresholdHandler alarmThresholdHandler;
87
+    UserDataHandler userDataHandler;
88
+
89
+    uint8_t userData[userDataSize];
90
+    uint8_t buffer[bufferSize];
91
+    uint8_t bufferIndex;
92
+};
93
+
94
+#endif // __FRSKY_H__
95
+

Завантаження…
Відмінити
Зберегти