?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Update README.md
[FrskyTelemetry.git] / SCRIPTS / TELEMETRY / yaapu9.lua
blob69013b1f475551b0323fca68cfb48ddec5105680
1 --
2 -- An FRSKY S.Port <passthrough protocol> based Telemetry script for Taranis X9D+
3 --
4 -- Copyright (C) 2018. Alessandro Apostoli
5 -- https://github.com/yaapu
6 --
7 -- This program is free software; you can redistribute it and/or modify
8 -- it under the terms of the GNU General Public License as published by
9 -- the Free Software Foundation; either version 3 of the License, or
10 -- (at your option) any later version.
11 --
12 -- This program is distributed in the hope that it will be useful,
13 -- but WITHOUT ANY WARRANTY, without even the implied warranty of
14 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 -- GNU General Public License for more details.
16 --
17 -- You should have received a copy of the GNU General Public License
18 -- along with this program; if not, see <http://www.gnu.org/licenses>.
19 --
20 -- Passthrough protocol reference:
21 -- https://cdn.rawgit.com/ArduPilot/ardupilot_wiki/33cd0c2c/images/FrSky_Passthrough_protocol.xlsx
22 --
23 -- Borrowed some code from the LI-xx BATTCHECK v3.30 script
24 -- http://frskytaranis.forumactif.org/t2800-lua-download-un-testeur-de-batterie-sur-la-radio
25
26
27 local cellfull, cellempty = 4.2, 3.00
28 local cell = {0, 0, 0, 0, 0 ,0}
29 local cellsumfull, cellsumempty, cellsumtype, cellsum = 0, 0, 0, 0
30
31 local i, cellmin, cellresult = 0, cellfull, 0, 0
32 local thrOut = 0
33 local voltage = 0
34
35 --[[
36 MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. | */
37 MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */
38 MAV_TYPE_QUADROTOR=2, /* Quadrotor | */
39 MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */
40 MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */
41 MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */
42 MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */
43 MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */
44 MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */
45 MAV_TYPE_ROCKET=9, /* Rocket | */
46 MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */
47 MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */
48 MAV_TYPE_SUBMARINE=12, /* Submarine | */
49 MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */
50 MAV_TYPE_OCTOROTOR=14, /* Octorotor | */
51 MAV_TYPE_TRICOPTER=15, /* Tricopter | */
52 MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */
53 MAV_TYPE_KITE=17, /* Kite | */
54 MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */
55 MAV_TYPE_VTOL_DUOROTOR=19, /* Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. | */
56 MAV_TYPE_VTOL_QUADROTOR=20, /* Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. | */
57 MAV_TYPE_VTOL_TILTROTOR=21, /* Tiltrotor VTOL | */
58 MAV_TYPE_VTOL_RESERVED2=22, /* VTOL reserved 2 | */
59 MAV_TYPE_VTOL_RESERVED3=23, /* VTOL reserved 3 | */
60 MAV_TYPE_VTOL_RESERVED4=24, /* VTOL reserved 4 | */
61 MAV_TYPE_VTOL_RESERVED5=25, /* VTOL reserved 5 | */
62 MAV_TYPE_GIMBAL=26, /* Onboard gimbal | */
63 MAV_TYPE_ADSB=27, /* Onboard ADSB peripheral | */
64 MAV_TYPE_PARAFOIL=28, /* Steerable, nonrigid airfoil | */
65 MAV_TYPE_DODECAROTOR=29, /* Dodecarotor | */
66 ]]--
67
68 local frameTypes = {}
69 frameTypes[0] = "copter"
70 frameTypes[1] = "plane"
71 frameTypes[2] = "copter"
72 frameTypes[3] = "copter"
73 frameTypes[4] = "copter"
74 frameTypes[5] = ""
75 frameTypes[6] = ""
76 frameTypes[7] = ""
77 frameTypes[8] = ""
78 frameTypes[9] = ""
79 frameTypes[10] = "rover"
80 frameTypes[11] = "boat"
81 frameTypes[12] = ""
82 frameTypes[13] = "copter"
83 frameTypes[14] = "copter"
84 frameTypes[15] = "copter"
85 frameTypes[16] = "plane"
86 frameTypes[17] = ""
87 frameTypes[18] = ""
88 frameTypes[19] = "plane"
89 frameTypes[20] = "plane"
90 frameTypes[21] = "plane"
91 frameTypes[22] = "plane"
92 frameTypes[23] = "plane"
93 frameTypes[24] = "plane"
94 frameTypes[25] = "plane"
95 frameTypes[26] = ""
96 frameTypes[27] = ""
97 frameTypes[28] = "plane"
98 frameTypes[29] = "copter"
99 frameTypes[30] = ""
100
101 local flightModes = {}
102 flightModes["copter"] = {}
103 flightModes["plane"] = {}
104 flightModes["rover"] = {}
105 -- copter flight modes
106 flightModes["copter"][0]=""
107 flightModes["copter"][1]="Stabilize"
108 flightModes["copter"][2]="Acro"
109 flightModes["copter"][3]="AltHold"
110 flightModes["copter"][4]="Auto"
111 flightModes["copter"][5]="Guided"
112 flightModes["copter"][6]="Loiter"
113 flightModes["copter"][7]="RTL"
114 flightModes["copter"][8]="Circle"
115 flightModes["copter"][9]=""
116 flightModes["copter"][10]="Land"
117 flightModes["copter"][11]=""
118 flightModes["copter"][12]="Drift"
119 flightModes["copter"][13]=""
120 flightModes["copter"][14]="Sport"
121 flightModes["copter"][15]="Flip"
122 flightModes["copter"][16]="AutoTune"
123 flightModes["copter"][17]="PosHold"
124 flightModes["copter"][18]="Brake"
125 flightModes["copter"][19]="Throw"
126 flightModes["copter"][20]="Avoid ADSB"
127 flightModes["copter"][21]="Guided NO GPS"
128 -- plane flight modes
129 flightModes["plane"][0]="Manual"
130 flightModes["plane"][1]="Circle"
131 flightModes["plane"][2]="Stabilize"
132 flightModes["plane"][3]="Training"
133 flightModes["plane"][4]="Acro"
134 flightModes["plane"][5]="FlyByWireA"
135 flightModes["plane"][6]="FlyByWireB"
136 flightModes["plane"][7]="Cruise"
137 flightModes["plane"][8]="Autotune"
138 flightModes["plane"][9]=""
139 flightModes["plane"][10]="Auto"
140 flightModes["plane"][11]="RTL"
141 flightModes["plane"][12]="Loiter"
142 flightModes["plane"][13]=""
143 flightModes["plane"][14]="Avoid ADSB"
144 flightModes["plane"][15]="Guided"
145 flightModes["plane"][16]="Initializing"
146 flightModes["plane"][17]="QStabilize"
147 flightModes["plane"][18]="QHover"
148 flightModes["plane"][19]="QLoiter"
149 flightModes["plane"][20]="Qland"
150 flightModes["plane"][21]="QRTL"
151 -- rover flight modes
152 flightModes["rover"][0]="Manual"
153 flightModes["rover"][1]="Acro"
154 flightModes["rover"][2]=""
155 flightModes["rover"][3]="Steering"
156 flightModes["rover"][4]="Hold"
157 flightModes["rover"][5]=""
158 flightModes["rover"][6]=""
159 flightModes["rover"][7]=""
160 flightModes["rover"][8]=""
161 flightModes["rover"][9]=""
162 flightModes["rover"][10]="Auto"
163 flightModes["rover"][11]="RTL"
164 flightModes["rover"][12]="SmartRTL"
165 flightModes["rover"][13]=""
166 flightModes["rover"][14]=""
167 flightModes["rover"][15]="Guided"
168 flightModes["rover"][16]="Initializing"
169 flightModes["rover"][17]=""
170 flightModes["rover"][18]=""
171 flightModes["rover"][19]=""
172 flightModes["rover"][20]=""
173 flightModes["rover"][21]=""
174
175 local soundFileBasePath = "/SOUNDS/yaapu0/en"
176 local soundFiles = {}
177 -- battery
178 soundFiles["bat5"] = "bat5.wav"
179 soundFiles["bat10"] = "bat10.wav"
180 soundFiles["bat15"] = "bat15.wav"
181 soundFiles["bat20"] = "bat20.wav"
182 soundFiles["bat25"] = "bat25.wav"
183 soundFiles["bat30"] = "bat30.wav"
184 soundFiles["bat40"] = "bat40.wav"
185 soundFiles["bat50"] = "bat50.wav"
186 soundFiles["bat60"] = "bat60.wav"
187 soundFiles["bat70"] = "bat70.wav"
188 soundFiles["bat80"] = "bat80.wav"
189 soundFiles["bat90"] = "bat90.wav"
190 -- gps
191 soundFiles["gpsfix"] = "gpsfix.wav"
192 soundFiles["gpsnofix"] = "gpsnofix.wav"
193 -- failsafe
194 soundFiles["lowbat"] = "lowbat.wav"
195 soundFiles["ekf"] = "ekf.wav"
196 -- events
197 soundFiles["yaapu"] = "yaapu.wav"
198 soundFiles["landing"] = "landing.wav"
199 soundFiles["armed"] = "armed.wav"
200 soundFiles["disarmed"] = "disarmed.wav"
201
202 local soundFilesByFrameTypeAndFlightMode = {}
203 soundFilesByFrameTypeAndFlightMode["copter"] = {}
204 soundFilesByFrameTypeAndFlightMode["plane"] = {}
205 soundFilesByFrameTypeAndFlightMode["rover"] = {}
206 -- Copter
207 soundFilesByFrameTypeAndFlightMode["copter"][0]=""
208 soundFilesByFrameTypeAndFlightMode["copter"][1]="stabilize.wav"
209 soundFilesByFrameTypeAndFlightMode["copter"][2]="acro.wav"
210 soundFilesByFrameTypeAndFlightMode["copter"][3]="althold.wav"
211 soundFilesByFrameTypeAndFlightMode["copter"][4]="auto.wav"
212 soundFilesByFrameTypeAndFlightMode["copter"][5]="guided.wav"
213 soundFilesByFrameTypeAndFlightMode["copter"][6]="loiter.wav"
214 soundFilesByFrameTypeAndFlightMode["copter"][7]="rtl.wav"
215 soundFilesByFrameTypeAndFlightMode["copter"][8]="circle.wav"
216 soundFilesByFrameTypeAndFlightMode["copter"][9]=""
217 soundFilesByFrameTypeAndFlightMode["copter"][10]="land.wav"
218 soundFilesByFrameTypeAndFlightMode["copter"][11]=""
219 soundFilesByFrameTypeAndFlightMode["copter"][12]="drift.wav"
220 soundFilesByFrameTypeAndFlightMode["copter"][13]=""
221 soundFilesByFrameTypeAndFlightMode["copter"][14]="sport.wav"
222 soundFilesByFrameTypeAndFlightMode["copter"][15]="flip.wav"
223 soundFilesByFrameTypeAndFlightMode["copter"][16]="loiter.wav"
224 soundFilesByFrameTypeAndFlightMode["copter"][17]="poshold.wav"
225 soundFilesByFrameTypeAndFlightMode["copter"][18]="brake"
226 soundFilesByFrameTypeAndFlightMode["copter"][19]="throw.wav"
227 soundFilesByFrameTypeAndFlightMode["copter"][20]="avoidadbs"
228 soundFilesByFrameTypeAndFlightMode["copter"][21]="guidednogps"
229 -- Plane
230 soundFilesByFrameTypeAndFlightMode["plane"][0]="manual.wav"
231 soundFilesByFrameTypeAndFlightMode["plane"][1]="circle.wav"
232 soundFilesByFrameTypeAndFlightMode["plane"][2]="stabilize.wav"
233 soundFilesByFrameTypeAndFlightMode["plane"][3]="training.wav"
234 soundFilesByFrameTypeAndFlightMode["plane"][4]="acro.wav"
235 soundFilesByFrameTypeAndFlightMode["plane"][5]="flybywirea.wav"
236 soundFilesByFrameTypeAndFlightMode["plane"][6]="flybywireb.wav"
237 soundFilesByFrameTypeAndFlightMode["plane"][7]="cruise.wav"
238 soundFilesByFrameTypeAndFlightMode["plane"][8]="autotune.wav"
239 soundFilesByFrameTypeAndFlightMode["plane"][9]=""
240 soundFilesByFrameTypeAndFlightMode["plane"][10]="auto.wav"
241 soundFilesByFrameTypeAndFlightMode["plane"][11]="rtl.wav"
242 soundFilesByFrameTypeAndFlightMode["plane"][12]="loiter.wav"
243 soundFilesByFrameTypeAndFlightMode["plane"][13]=""
244 soundFilesByFrameTypeAndFlightMode["plane"][14]="avoidadbs"
245 soundFilesByFrameTypeAndFlightMode["plane"][15]="guided.wav"
246 soundFilesByFrameTypeAndFlightMode["plane"][16]="initializing.wav"
247 soundFilesByFrameTypeAndFlightMode["plane"][17]="qstabilize.wav"
248 soundFilesByFrameTypeAndFlightMode["plane"][18]="qhover.wav"
249 soundFilesByFrameTypeAndFlightMode["plane"][19]="qloiter.wav"
250 soundFilesByFrameTypeAndFlightMode["plane"][20]="qland.wav"
251 soundFilesByFrameTypeAndFlightMode["plane"][21]="qrtl.wav"
252 -- Rover
253 soundFilesByFrameTypeAndFlightMode["rover"][0]="manual_r.wav"
254 soundFilesByFrameTypeAndFlightMode["rover"][1]="acro_r.wav"
255 soundFilesByFrameTypeAndFlightMode["rover"][2]=""
256 soundFilesByFrameTypeAndFlightMode["rover"][3]="steering_r.wav"
257 soundFilesByFrameTypeAndFlightMode["rover"][4]="hold_r.wav"
258 soundFilesByFrameTypeAndFlightMode["rover"][5]=""
259 soundFilesByFrameTypeAndFlightMode["rover"][6]=""
260 soundFilesByFrameTypeAndFlightMode["rover"][7]=""
261 soundFilesByFrameTypeAndFlightMode["rover"][8]=""
262 soundFilesByFrameTypeAndFlightMode["rover"][9]=""
263 soundFilesByFrameTypeAndFlightMode["rover"][10]="auto_r.wav"
264 soundFilesByFrameTypeAndFlightMode["rover"][11]="rtl_r.wav"
265 soundFilesByFrameTypeAndFlightMode["rover"][12]="smartrtl_r.wav"
266 soundFilesByFrameTypeAndFlightMode["rover"][13]=""
267 soundFilesByFrameTypeAndFlightMode["rover"][14]=""
268 soundFilesByFrameTypeAndFlightMode["rover"][15]="guided_r.wav"
269 soundFilesByFrameTypeAndFlightMode["rover"][16]="initializing.wav"
270 soundFilesByFrameTypeAndFlightMode["rover"][17]=""
271 soundFilesByFrameTypeAndFlightMode["rover"][18]=""
272 soundFilesByFrameTypeAndFlightMode["rover"][19]=""
273 soundFilesByFrameTypeAndFlightMode["rover"][20]=""
274 soundFilesByFrameTypeAndFlightMode["rover"][21]=""
275
276
277 local gpsStatuses = {}
278 gpsStatuses[0]="NoGPS"
279 gpsStatuses[1]="NoLock"
280 gpsStatuses[2]="2DFIX"
281 gpsStatuses[3]="3DFix"
282
283 local mavSeverity = {}
284 mavSeverity[0]="EMR"
285 mavSeverity[1]="ALR"
286 mavSeverity[2]="CRT"
287 mavSeverity[3]="ERR"
288 mavSeverity[4]="WRN"
289 mavSeverity[5]="NOT"
290 mavSeverity[6]="INF"
291 mavSeverity[7]="DBG"
292
293 -- STATUS
294 local flightMode = 0
295 local simpleMode = 0
296 local landComplete = 0
297 local statusArmed = 0
298 local batteryFailsafe = 0
299 local lastBatteryFailsafe = 0
300 local ekfFailsafe = 0
301 local lastEkfFailsafe = 0
302 -- GPS
303 local numSats = 0
304 local gpsStatus = 0
305 local gpsHdopC = 100
306 local gpsAlt = 0
307 -- BATT
308 local battVolt = 0
309 local battCurrent = 0
310 local battMah = 0
311 -- BATT2
312 local battVolt2 = 0
313 local battCurrent2 = 0
314 local battMah2 = 0
315 -- HOME
316 local homeDist = 0
317 local homeAlt = 0
318 local homeAngle = -1
319 -- MESSAGES
320 local messageBuffer = ""
321 local messageHistory = {}
322 local severity = 0
323 local messageIdx = 1
324 local messageDuplicate = 1
325 local lastMessage = ""
326 local lastMessageValue = 0
327 local lastMessageTime = 0
328 -- VELANDYAW
329 local vSpeed = 0
330 local hSpeed = 0
331 local yaw = 0
332 -- ROLLPITCH
333 local roll = 0
334 local pitch = 0
335 -- TELEMETRY
336 local SENSOR_ID,FRAME_ID,DATA_ID,VALUE
337 local mult = 0
338 local c1,c2,c3,c4
339 -- PARAMS
340 local paramId,paramValue
341 local frameType = 2
342 local battFailsafeVoltage = 0
343 local battFailsafeCapacity = 0
344 local battCapacity = 0
345 --
346 local minX = 0
347 local maxX = 76
348 local minY = 9
349 local maxY = 55
350 --
351 local noTelemetryData = 1
352
353 local function playSound(soundFile)
354 playFile(soundFileBasePath .. "/" .. soundFiles[soundFile])
355 end
356
357 local function playSoundByFrameTypeAndFlightMode(frameType,flightMode)
358 playFile(soundFileBasePath .. "/" .. soundFilesByFrameTypeAndFlightMode[frameTypes[frameType]][flightMode])
359 end
360
361 local function getValueOrDefault(value)
362 local tmp = getValue(value)
363 if tmp == nil then
364 return 0
365 end
366 return tmp
367 end
368
369 local function pushMessage(severity, msg)
370 if ( severity < 4) then
371 playTone(400,300,0)
372 else
373 playTone(600,300,0)
374 end
375 local mm = msg
376 if msg == lastMessage then
377 messageDuplicate = messageDuplicate + 1
378 if messageDuplicate > 1 then
379 if string.len(mm) > 33 then
380 mm = string.sub(mm,1,33)
381 messageHistory[messageIdx - 1] = string.format("[%02d:%s] %-33s (x%d)", messageIdx - 1, mavSeverity[severity], mm, messageDuplicate)
382 else
383 messageHistory[messageIdx - 1] = string.format("[%02d:%s] %s (x%d)", messageIdx - 1, mavSeverity[severity], msg, messageDuplicate)
384 end
385 end
386 else
387 messageHistory[messageIdx] = string.format("[%02d:%s] %s", messageIdx, mavSeverity[severity], msg)
388 messageIdx = messageIdx + 1
389 lastMessage = msg
390 messageDuplicate = 1
391 end
392 lastMessageTime = getTime() -- valore in secondi
393 end
394
395 local function logTelemetryToFile(S_ID,F_ID,D_ID,VA)
396 local lc1 = 0
397 local lc2 = 0
398 local lc3 = 0
399 local lc4 = 0
400 if (S_ID) then
401 lc1 = S_ID
402 end
403 if (F_ID) then
404 lc2 = F_ID
405 end
406 if (D_ID) then
407 lc3 = D_ID
408 end
409 if (VA) then
410 lc4 = VA
411 end
412 local logFile = io.open("yaapu.log","a")
413 io.write(logFile,string.format("%d,%#04x,%#04x,%#04x,%#04x", getTime(), lc1, lc2, lc3, lc4),"\r\n")
414 io.close(logFile)
415 end
416
417 local seconds = 0
418 local lastTimerStart = 0
419 --
420 local function startTimer()
421 lastTimerStart = getTime()/100
422 end
423
424 local function stopTimer()
425 seconds = seconds + getTime()/100 - lastTimerStart
426 lastTimerStart = 0
427 end
428
429 local function symTimer()
430 thrOut = getValue("thr")
431 if (thrOut > -500 ) then
432 landComplete = 1
433 else
434 landComplete = 0
435 end
436 end
437
438 local function symGPS()
439 thrOut = getValue("thr")
440 if (thrOut > 0 ) then
441 numSats = 9
442 gpsStatus = 3
443 gpsHdopC = 11
444 ekfFailsafe = 0
445 batteryFailsafe = 0
446 noTelemetryData = 0
447 statusArmed = 1
448 elseif thrOut > -500 then
449 numSats = 6
450 gpsStatus = 3
451 gpsHdopC = 17
452 ekfFailsafe = 1
453 batteryFailsafe = 1
454 noTelemetryData = 0
455 statusArmed = 0
456 else
457 numSats = 0
458 gpsStatus = 0
459 gpsHdopC = 100
460 ekfFailsafe = 0
461 batteryFailsafe = 0
462 noTelemetryData = 1
463 statusArmed = 0
464 end
465 end
466
467 local function symFrameType()
468 local ch11 = getValue("ch11")
469 if (ch11 < -300) then
470 frameType = 2
471 elseif ch11 < 300 then
472 frameType = 1
473 else
474 frameType = 10
475 end
476 end
477
478 local function symBatt()
479 thrOut = getValue("thr")
480 if (thrOut > 0 ) then
481 LIPObatt = 1350 + ((thrOut)*0.01 * 30)
482 LIPOcelm = LIPObatt/4
483 battCurrent = 100 + ((thrOut)*0.01 * 30)
484 battVolt = LIPObatt*0.1
485 battCapacity = 5200
486 battMah = math.abs(1000*(thrOut/200))
487 flightMode = math.floor(20 * math.abs(thrOut)*0.001)
488 end
489 end
490
491 -- simulates attitude by using channel 1 for roll, channel 2 for pitch and channel 4 for yaw
492 local function symAttitude()
493 local rollCh = 0
494 local pitchCh = 0
495 local yawCh = 0
496 -- roll [-1024,1024] ==> [-180,180]
497 rollCh = getValue("ch1") * 0.175
498 -- pitch [1024,-1024] ==> [-90,90]
499 pitchCh = getValue("ch2") * 0.0878
500 -- yaw [-1024,1024] ==> [0,360]
501 yawCh = getValue("ch10")
502 if ( yawCh >= 0) then
503 yawCh = yawCh * 0.175
504 else
505 yawCh = 360 + (yawCh * 0.175)
506 end
507 roll = rollCh/3
508 pitch = pitchCh/2
509 yaw = yawCh
510 end
511
512 local function symHome()
513 local yawCh = 0
514 local S2Ch = 0
515 -- home angle in deg [0-360]
516 S2Ch = getValue("ch12")
517 yawCh = getValue("ch4")
518 homeAlt = yawCh
519 vSpeed = yawCh * 0.1
520 if ( yawCh >= 0) then
521 yawCh = yawCh * 0.175
522 else
523 yawCh = 360 + (yawCh * 0.175)
524 end
525 if ( S2Ch >= 0) then
526 S2Ch = S2Ch * 0.175
527 else
528 S2Ch = 360 + (S2Ch * 0.175)
529 end
530 if (thrOut > 0 ) then
531 homeAngle = S2Ch
532 else
533 homeAngle = -1
534 end
535 yaw = yawCh
536 end
537
538 local function processTelemetry()
539 SENSOR_ID,FRAME_ID,DATA_ID,VALUE = sportTelemetryPop()
540 --
541 --FRAME_ID = 0x10
542 --DATA_ID = 0x5002
543 --VALUE = 0xf4006b8
544 --
545 --FRAME_ID = 0x10
546 --DATA_ID = 0x5004
547 --VALUE = 0x168000
548 --
549 if ( FRAME_ID == 0x10) then
550 -- >> DEBUG
551 -- logTelemetryToFile(SENSOR_ID,FRAME_ID,DATA_ID,VALUE)
552 -- << DEBUG
553 noTelemetryData = 0
554 if ( DATA_ID == 0x5006) then -- ROLLPITCH
555 -- roll [0,1800] ==> [-180,180]
556 roll = (bit32.extract(VALUE,0,11) - 900) * 0.2
557 -- pitch [0,900] ==> [-90,90]
558 pitch = (bit32.extract(VALUE,11,10) - 450) * 0.2
559 elseif ( DATA_ID == 0x5005) then -- VELANDYAW
560 vSpeed = bit32.extract(VALUE,1,7) * (10^bit32.extract(VALUE,0,1))
561 if (bit32.extract(VALUE,8,1) == 1) then
562 vSpeed = -vSpeed
563 end
564 hSpeed = bit32.extract(VALUE,10,7) * (10^bit32.extract(VALUE,9,1))
565 yaw = bit32.extract(VALUE,17,11) * 0.2
566 elseif ( DATA_ID == 0x5001) then -- AP STATUS
567 flightMode = bit32.extract(VALUE,0,5)
568 simpleMode = bit32.extract(VALUE,5,2)
569 landComplete = bit32.extract(VALUE,7,1)
570 statusArmed = bit32.extract(VALUE,8,1)
571 batteryFailsafe = bit32.extract(VALUE,9,1)
572 ekfFailsafe = bit32.extract(VALUE,10,2)
573 elseif ( DATA_ID == 0x5002) then -- GPS STATUS
574 numSats = bit32.extract(VALUE,0,4)
575 gpsStatus = bit32.extract(VALUE,4,2)
576 gpsHdopC = bit32.extract(VALUE,7,7) * (10^bit32.extract(VALUE,6,1)) -- dm
577 gpsAlt = bit32.extract(VALUE,24,7) * (10^bit32.extract(VALUE,22,2)) -- dm
578 if (bit32.extract(VALUE,31,1) == 1) then
579 gpsAlt = gpsAlt * -1
580 end
581 elseif ( DATA_ID == 0x5003) then -- BATT
582 battVolt = bit32.extract(VALUE,0,9)
583 battCurrent = bit32.extract(VALUE,10,7) * (10^bit32.extract(VALUE,9,1))
584 battMah = bit32.extract(VALUE,17,15)
585 elseif ( DATA_ID == 0x5008) then -- BATT2
586 battVolt2 = bit32.extract(VALUE,0,9)
587 battCurrent2 = bit32.extract(VALUE,10,7) * (10^bit32.extract(VALUE,9,1))
588 battMah2 = bit32.extract(VALUE,17,15)
589 elseif ( DATA_ID == 0x5004) then -- HOME
590 homeDist = bit32.extract(VALUE,2,10) * (10^bit32.extract(VALUE,0,2))
591 homeAlt = bit32.extract(VALUE,14,10) * (10^bit32.extract(VALUE,12,2)) * 0.1
592 if (bit32.extract(VALUE,24,1) == 1) then
593 homeAlt = homeAlt * -1
594 end
595 homeAngle = bit32.extract(VALUE, 25, 7) * 3
596 elseif ( DATA_ID == 0x5000) then -- MESSAGES
597 if (VALUE ~= lastMessageValue) then
598 lastMessageValue = VALUE
599 c1 = bit32.extract(VALUE,0,7)
600 c2 = bit32.extract(VALUE,8,7)
601 c3 = bit32.extract(VALUE,16,7)
602 c4 = bit32.extract(VALUE,24,7)
603 messageBuffer = messageBuffer .. string.char(c4)
604 messageBuffer = messageBuffer .. string.char(c3)
605 messageBuffer = messageBuffer .. string.char(c2)
606 messageBuffer = messageBuffer .. string.char(c1)
607 if (c1 == 0 or c2 == 0 or c3 == 0 or c4 == 0) then
608 severity = (bit32.extract(VALUE,15,1) * 4) + (bit32.extract(VALUE,23,1) * 2) + (bit32.extract(VALUE,30,1) * 1)
609 pushMessage( severity, messageBuffer)
610 messageBuffer = ""
611 end
612 end
613 elseif ( DATA_ID == 0x5007) then -- PARAMS
614 paramId = bit32.extract(VALUE,24,4)
615 paramValue = bit32.extract(VALUE,0,24)
616 if paramId == 1 then
617 frameType = paramValue
618 elseif paramId == 2 then
619 battFailsafeVoltage = paramValue
620 elseif paramId == 3 then
621 battFailsafeCapacity = paramValue
622 elseif paramId == 4 then
623 battCapacity = paramValue
624 end
625 end
626 end
627 end
628
629 local function telemetryEnabled()
630 if getValue("RxBt") == 0 then
631 noTelemetryData = 1
632 end
633 return noTelemetryData == 0
634 --return true
635 end
636
637 local battSource = "na"
638
639 local function calcBattery()
640 local battA2 = 0
641 local cellCount = 3;
642 --
643 cellmin = cellfull
644 cellResult = getValue("Cels")
645
646 if type(cellResult) == "table" then
647 battSource="vs"
648 cellsum = 0
649 for i = 1, #cell do cell[i] = 0 end
650 cellsumtype = #cellResult
651 for i, v in pairs(cellResult) do
652 cellsum = cellsum + v
653 cell[i] = v
654 if cellmin > v then
655 cellmin = v
656 end
657 end -- end for
658 else
659 -- cels is not defined let's check if A2 is defined
660 cellmin = 0
661 battA2 = getValue("A2")
662 --
663 if battA2 > 0 then
664 battSource="a2"
665 if battA2 > 21 then
666 cellCount = 6
667 elseif battA2 > 17 then
668 cellCount = 5
669 elseif battA2 > 13 then
670 cellCount = 4
671 else
672 cellCount = 3
673 end
674 --
675 cellmin = battA2/cellCount
676 cellsum = battA2
677 else
678 -- A2 is not defined, last chance is battVolt
679 if battVolt > 0 then
680 battSource="fc"
681 cellsum = battVolt*0.1
682 if cellsum > 21 then
683 cellCount = 6
684 elseif cellsum > 17 then
685 cellCount = 5
686 elseif cellsum > 13 then
687 cellCount = 4
688 else
689 cellCount = 3
690 end
691 --
692 cellmin = cellsum/cellCount
693 end
694 end
695 end -- end if
696 --
697 LIPOcelm = cellmin*100
698 LIPObatt = cellsum*100
699 end
700
701 local function drawBattery()
702 if (battCapacity > 0) then
703 LIPOperc = (1 - (battMah/battCapacity))*100
704 else
705 LIPOperc = 0
706 end
707 -- display battery voltage
708 lcd.drawNumber(maxX + 3, 10, LIPObatt, DBLSIZE+PREC2)
709
710 local xx = lcd.getLastRightPos()
711 lcd.drawText(xx, 10, "V", 0)
712 lcd.drawText(xx, 20, battSource, SMLSIZE)
713
714 -- display lowest cell voltage
715 if LIPOcelm < 350 then
716 lcd.drawNumber(maxX + 57, 10, LIPOcelm, DBLSIZE+BLINK+PREC2)
717 lcd.drawText(lcd.getLastRightPos(), 10, "Vm", SMLSIZE+BLINK)
718 else
719 lcd.drawNumber(maxX + 57, 10, LIPOcelm, DBLSIZE+PREC2)
720 lcd.drawText(lcd.getLastRightPos(), 10, "Vm", SMLSIZE)
721 end
722
723 lcd.drawNumber(maxX + 101, 10, battCurrent, DBLSIZE+PREC1)
724 lcd.drawText(lcd.getLastRightPos(), 10, "A", SMLSIZE)
725
726 local barOffset = maxX + 24
727
728 lcd.drawText(barOffset - 1, 28, "%", SMLSIZE+RIGHT)
729 lcd.drawNumber(lcd.getLastLeftPos(), 28, LIPOperc, RIGHT)
730
731 -- display capacity bar %
732 lcd.drawGauge(barOffset, 28, 55, 7, LIPOperc, 100)
733
734 lcd.drawText(212, 28, "Ah", SMLSIZE+RIGHT)
735 lcd.drawNumber(lcd.getLastLeftPos(), 28, battCapacity/100, SMLSIZE+PREC1+RIGHT)
736 lcd.drawText(lcd.getLastLeftPos(), 28, "/", SMLSIZE+RIGHT)
737 lcd.drawNumber(lcd.getLastLeftPos(), 28, battMah/100, SMLSIZE+PREC1+RIGHT)
738
739 lcd.drawPoint(barOffset + 13, 28)
740 lcd.drawPoint(barOffset + 13, 34)
741
742 lcd.drawPoint(barOffset + 27, 28)
743 lcd.drawPoint(barOffset + 27, 34)
744
745 lcd.drawPoint(barOffset + 41, 28)
746 lcd.drawPoint(barOffset + 41, 34)
747 end
748
749 local function drawFlightMode()
750 lcd.drawFilledRectangle(0,0, 212, 9, SOLID)
751 lcd.drawRectangle(0, 0, 212, 9, SOLID)
752
753 if (not telemetryEnabled()) then
754 lcd.drawFilledRectangle((212-150)/2,18, 150, 30, SOLID)
755 lcd.drawText(60, 29, "no telemetry data", INVERS)
756 return
757 end
758
759 local strMode = flightModes[frameTypes[frameType]][flightMode]
760 --
761 lcd.drawText(1, 1, strMode, SMLSIZE+INVERS)
762
763 if ( simpleMode == 1) then
764 lcd.drawText(lcd.getLastRightPos(), 1, "(S)", SMLSIZE+INVERS)
765 end
766
767 if (statusArmed == 1) then
768 lcd.drawText(maxX/2 - 12, 47, "ARMED", SMLSIZE+INVERS)
769 else
770 lcd.drawText(maxX/2 - 18, 47, "DISARMED", SMLSIZE+INVERS+BLINK)
771 end
772 end
773
774 local function drawHome()
775 local xx = maxX + 76
776 local yy = 39
777 local ax = xx - 43
778 local ay = 42
779 local alen = 10
780 --
781 lcd.drawLine(ax ,ay,ax + alen,ay, SOLID, 0)
782 -- left arrow
783 lcd.drawLine(ax+1,ay-1,ax + 2,ay-2, SOLID, 0)
784 lcd.drawLine(ax+1,ay+1,ax + 2,ay+2, SOLID, 0)
785 -- right arrow
786 lcd.drawLine(ax + alen - 1,ay-1,ax + alen - 2,ay-2, SOLID, 0)
787 lcd.drawLine(ax + alen - 1,ay+1,ax + alen - 2,ay+2, SOLID, 0)
788
789 if homeAngle == -1 then
790 lcd.drawText(xx, yy, "m",SMLSIZE+RIGHT+BLINK)
791 lcd.drawNumber(lcd.getLastLeftPos(), yy, homeDist, SMLSIZE+RIGHT+BLINK)
792 lcd.drawNumber(xx, yy + 8, 0, SMLSIZE+RIGHT+BLINK)
793 else
794 lcd.drawText(xx, yy, "m",SMLSIZE+RIGHT)
795 lcd.drawNumber(lcd.getLastLeftPos(), yy, homeDist, SMLSIZE+RIGHT)
796 lcd.drawNumber(xx, yy + 8, homeAngle, SMLSIZE+RIGHT)
797 end
798 lcd.drawText(ax, yy + 8, "Angle:",SMLSIZE)
799 end
800
801 local function drawMessage()
802 lcd.drawFilledRectangle(0,55, 212, 9, SOLID)
803 lcd.drawRectangle(0, 55, 212, 9, SOLID)
804 local now = getTime()
805 if (now - lastMessageTime ) > 300 then
806 lcd.drawText(1, 56, messageHistory[messageIdx-1],SMLSIZE+INVERS)
807 else
808 lcd.drawText(1, 56, messageHistory[messageIdx-1],SMLSIZE+INVERS+BLINK)
809 end
810 end
811
812 local function drawAllMessages()
813 local idx = 1
814 if (messageIdx <= 6) then
815 for i = 1, messageIdx - 1 do
816 lcd.drawText(1, 1+10*(idx - 1), messageHistory[i],SMLSIZE)
817 idx = idx+1
818 end
819 else
820 for i = messageIdx - 6,messageIdx - 1 do
821 lcd.drawText(1, 1+10*(idx-1), messageHistory[i],SMLSIZE)
822 idx = idx+1
823 end
824 end
825 end
826
827 local function drawGPSStatus()
828 local xx = 173
829 local yy = 38
830 lcd.drawRectangle(xx,yy,50,18,SOLID)
831
832 --
833 local strStatus = gpsStatuses[gpsStatus]
834 --
835 local flags = BLINK
836 if gpsStatus > 2 then
837 lcd.drawFilledRectangle(xx,yy,50,18,SOLID)
838 if homeAngle ~= -1 then
839 flags = 0
840 end
841 lcd.drawText(xx+2, yy+2, strStatus, SMLSIZE+INVERS)
842 lcd.drawNumber(212, yy+2, numSats, SMLSIZE+INVERS+RIGHT)
843 lcd.drawText(xx+2, yy+10, "Hdop ", SMLSIZE+INVERS)
844 --
845 if gpsHdopC > 100 then
846 lcd.drawNumber(212, yy+10, gpsHdopC , SMLSIZE+INVERS+RIGHT+PREC1+flags)
847 else
848 lcd.drawNumber(212, yy+10, gpsHdopC , SMLSIZE+INVERS+RIGHT+PREC1+flags)
849 end
850 lcd.drawText(maxX + 30, 40, "AltAsl", SMLSIZE+RIGHT)
851 lcd.drawText(maxX + 30, 47, "m", SMLSIZE+RIGHT)
852 lcd.drawNumber(lcd.getLastLeftPos(), 47, gpsAlt/10, SMLSIZE+RIGHT)
853 else
854 lcd.drawText(xx+5, yy+5, strStatus, INVERS+BLINK)
855 lcd.drawText(maxX + 30, 40, "AltAsl", SMLSIZE+RIGHT)
856 lcd.drawText(maxX + 30, 47, "m", SMLSIZE+RIGHT+BLINK)
857 lcd.drawNumber(lcd.getLastLeftPos(), 47, 0, SMLSIZE+RIGHT+BLINK)
858 end
859 end
860
861 local function drawGrid()
862 lcd.drawLine(maxX, 0, maxX, 63, SOLID, 0)
863 lcd.drawRectangle(maxX,38,31,18,SOLID)
864 end
865
866 local timerRunning = 0
867
868 local function checkLandingStatus()
869 if ( timerRunning == 0 and landComplete == 1 and lastTimerStart == 0) then
870 startTimer()
871 end
872 if (timerRunning == 1 and landComplete == 0 and lastTimerStart ~= 0) then
873 stopTimer()
874 playSound("landing")
875 end
876 timerRunning = landComplete
877 end
878 local flightTime = 0
879
880 local function calcFlightTime()
881 local elapsed = 0
882 if ( lastTimerStart ~= 0) then
883 elapsed = getTime()/100 - lastTimerStart
884 end
885 flightTime = elapsed + seconds
886 end
887
888 -- draws a line centered at ox,oy with given angle and length W/O CROPPING
889 local function drawLine(ox,oy,angle,len,style,maxX,maxY)
890 local xx = math.cos(math.rad(angle)) * len * 0.5
891 local yy = math.sin(math.rad(angle)) * len * 0.5
892
893 local x1 = ox - xx
894 local x2 = ox + xx
895 local y1 = oy - yy
896 local y2 = oy + yy
897 --
898 lcd.drawLine(x1,y1,x2,y2, style,0)
899 end
900
901 -- draws a line centered at ox,oy with given angle and length WITH CROPPING
902 local function drawCroppedLine(ox,oy,angle,len,style,maxX,maxY)
903 --
904 local xx = math.cos(math.rad(angle)) * len * 0.5
905 local yy = math.sin(math.rad(angle)) * len * 0.5
906 --
907 local x1 = ox - xx
908 local x2 = ox + xx
909 local y1 = oy - yy
910 local y2 = oy + yy
911 --
912 if (x1 >= maxX and x2 >= maxX) then
913 return
914 end
915 if (x1 >= maxX) then
916 y1 = y1 - math.tan(math.rad(angle)) * (maxX - x1)
917 x1 = maxX - 1
918 end
919 if (x2 >= maxX) then
920 y2 = y2 + math.tan(math.rad(angle)) * (maxX - x2)
921 x2 = maxX - 1
922 end
923 lcd.drawLine(x1,y1,x2,y2, style,0)
924 end
925
926 local function drawFailsafe()
927 if ekfFailsafe > 0 then
928 if lastEkfFailsafe == 0 then
929 playSound("ekf")
930 end
931 lcd.drawText(maxX/2 - 28, 47, "EKF FAILSAFE", SMLSIZE+INVERS+BLINK)
932 end
933 if batteryFailsafe > 0 then
934 if lastBatteryFailsafe == 0 then
935 playSound("lowbat")
936 end
937 lcd.drawText(maxX/2 - 30, 47, "BATT FAILSAFE", SMLSIZE+INVERS+BLINK)
938 end
939 lastEkfFailsafe = ekfFailsafe
940 lastBatteryFailsafe = batteryFailsafe
941 end
942
943 local function drawPitch()
944 local y = 0
945 local p = pitch
946 -- horizon min max +/- 30°
947 if ( pitch > 0) then
948 if (pitch > 30) then
949 p = 30
950 end
951 else
952 if (pitch < -30) then
953 p = -30
954 end
955 end
956 -- y normalized at 32 +/-20 (0.75 = 20/32)
957 y = 32 + 0.75*p
958 -- horizon, lower half of HUD filled in grey
959 --lcd.drawFilledRectangle(minX,y,maxX-minX,maxY-y + 1,GREY_DEFAULT)
960 --
961 -- center indicators for vSpeed and alt
962 local width = 17
963 lcd.drawLine(minX,32 - 5,minX + width,32 - 5, SOLID, 0)
964 lcd.drawLine(minX,32 + 4,minX + width,32 + 4, SOLID, 0)
965 lcd.drawLine(minX + width + 1,32 + 4,minX + width + 5,32, SOLID, 0)
966 lcd.drawLine(minX + width + 1,32 - 4,minX + width + 5,32, SOLID, 0)
967 lcd.drawPoint(minX + width + 5,32)
968 --
969 lcd.drawLine(maxX - width - 1,32 - 5,maxX - 1,32 - 5,SOLID,0)
970 lcd.drawLine(maxX - width - 1,32 + 4,maxX - 1,32 + 4,SOLID,0)
971 lcd.drawLine(maxX - width - 2,32 + 4,maxX - width - 6,32, SOLID, 0)
972 lcd.drawLine(maxX - width - 2,32 - 4,maxX - width - 6,32, SOLID, 0)
973 lcd.drawPoint(maxX - width - 6,32)
974 --
975 local xx = maxX - width - 2
976 if homeAlt > 0 then
977 if homeAlt < 10 then -- 2 digits with decimal
978 lcd.drawNumber(maxX,32 - 3,homeAlt * 10,SMLSIZE+PREC1+RIGHT)
979 else -- 3 digits
980 lcd.drawNumber(maxX,32 - 3,homeAlt,SMLSIZE+RIGHT)
981 end
982 else
983 if homeAlt > -10 then -- 1 digit with sign
984 lcd.drawNumber(maxX,32 - 3,homeAlt * 10,SMLSIZE+PREC1+RIGHT)
985 else -- 3 digits with sign
986 lcd.drawNumber(maxX,32 - 3,homeAlt,SMLSIZE+RIGHT)
987 end
988 end
989 --
990 if (vSpeed > 999) then
991 lcd.drawNumber(minX + 1,32 - 3,vSpeed*0.1,SMLSIZE)
992 elseif (vSpeed < -99) then
993 lcd.drawNumber(minX + 1,32 - 3,vSpeed * 0.1,SMLSIZE)
994 else
995 lcd.drawNumber(minX + 1,32 - 3,vSpeed,SMLSIZE+PREC1)
996 end
997 -- up pointing center arrow
998 local x = math.floor(maxX/2)
999 lcd.drawLine(x-10,34 + 5,x ,34 ,SOLID,0)
1000 lcd.drawLine(x+1 ,34 ,x + 10, 34 + 5,SOLID,0)
1001 end
1002
1003 local function drawRoll()
1004 local r = -roll
1005 local r2 = 10 --vertical distance between roll horiz segments
1006 local cx,cy,dx,dy,ccx,ccy,cccx,cccy
1007 -- no roll ==> segments are vertical, offsets are multiples of r2
1008 if ( roll == 0) then
1009 dx=0
1010 dy=pitch
1011 cx=0
1012 cy=r2
1013 ccx=0
1014 ccy=2*r2
1015 cccx=0
1016 cccy=3*r2
1017 else
1018 -- center line offsets
1019 dx = math.cos(math.rad(90 - r)) * -pitch
1020 dy = math.sin(math.rad(90 - r)) * pitch
1021 -- 1st line offsets
1022 cx = math.cos(math.rad(90 - r)) * r2
1023 cy = math.sin(math.rad(90 - r)) * r2
1024 -- 2nd line offsets
1025 ccx = math.cos(math.rad(90 - r)) * 2 * r2
1026 ccy = math.sin(math.rad(90 - r)) * 2 * r2
1027 -- 3rd line offsets
1028 cccx = math.cos(math.rad(90 - r)) * 3 * r2
1029 cccy = math.sin(math.rad(90 - r)) * 3 * r2
1030 end
1031 local x = math.floor(maxX/2)
1032 drawCroppedLine(dx + x - cccx,dy + 32 + cccy,r,5,DOTTED,maxX,maxY)
1033 drawCroppedLine(dx + x - ccx,dy + 32 + ccy,r,7,DOTTED,maxX,maxY)
1034 drawCroppedLine(dx + x - cx,dy + 32 + cy,r,10,DOTTED,maxX,maxY)
1035 drawCroppedLine(dx + x,dy + 32,r,28,SOLID,maxX,maxY)
1036 drawCroppedLine(dx + x + cx,dy + 32 - cy,r,10,DOTTED,maxX,maxY)
1037 drawCroppedLine(dx + x + ccx,dy + 32 - ccy,r,7,DOTTED,maxX,maxY)
1038 drawCroppedLine(dx + x + cccx,dy + 32 - cccy,r,5,DOTTED,maxX,maxY)
1039 end
1040
1041 local function roundTo(val,int)
1042 return math.floor(val/int) * int
1043 end
1044
1045 local function drawYaw()
1046 local northX = math.floor(maxX/2) - 2
1047 local offset = northX - 4
1048 local yawRounded = math.floor(yaw)
1049 local yawWindow = roundTo(yaw,10)
1050 local maxXHalf = math.floor(maxX/2)
1051 local yawWindowOn = false
1052 local flags = SMLSIZE
1053 if (not(yawWindow == 0 or yawWindow == 90 or yawWindow == 180 or yawWindow == 270 or yawWindow == 360)) then
1054 yawWindowOn = true
1055 end
1056 --
1057 if (yawRounded == 0 or yawRounded == 360) then
1058 lcd.drawText(northX - offset, minY+1, "W", SMLSIZE)
1059 lcd.drawText(northX, minY+1, "N", flags)
1060 lcd.drawText(northX + offset, minY+1, "E", SMLSIZE)
1061 elseif (yawRounded == 90) then
1062 lcd.drawText(northX - offset, minY+1, "N", SMLSIZE)
1063 lcd.drawText(northX, minY+1, "E", flags)
1064 lcd.drawText(northX + offset, minY+1, "S", SMLSIZE)
1065 elseif (yawRounded == 180) then
1066 lcd.drawText(northX - offset, minY+1, "W", SMLSIZE)
1067 lcd.drawText(northX, minY+1, "S", flags)
1068 lcd.drawText(northX + offset, minY+1, "E", SMLSIZE)
1069 elseif (yawRounded == 270) then
1070 lcd.drawText(northX - offset, minY+1, "S", SMLSIZE)
1071 lcd.drawText(northX, minY+1, "W", flags)
1072 lcd.drawText(northX + offset, minY+1, "N", SMLSIZE)
1073 elseif ( yaw > 0 and yaw < 90) then
1074 northX = (maxXHalf - 2) - 0.3*yaw
1075 lcd.drawText(northX, minY+1, "N", flags)
1076 lcd.drawText(northX + offset, minY+1, "E", SMLSIZE)
1077 if (yaw > 75) then
1078 lcd.drawText(northX + offset + offset, minY+1, "S", SMLSIZE)
1079 end
1080 elseif ( yaw > 90 and yaw < 180) then
1081 northX = (maxXHalf - 2) - 0.3*(yaw - 180)
1082 lcd.drawText(northX - offset, minY+1, "E", SMLSIZE)
1083 lcd.drawText(northX, minY+1, "S", flags)
1084 if (yaw > 170) then
1085 lcd.drawText(northX + offset, minY+1, "W", SMLSIZE)
1086 end
1087 elseif ( yaw > 180 and yaw < 270) then
1088 northX = (maxXHalf - 2) - 0.3*(yaw - 270)
1089 lcd.drawText(northX, minY+1, "W", SMLSIZE)
1090 lcd.drawText(northX - offset, minY+1, "S", flags)
1091 if (yaw < 190) then
1092 lcd.drawText(northX - offset - offset, minY+1, "E", SMLSIZE)
1093 end
1094 elseif ( yaw > 270 and yaw < 360) then
1095 northX = (maxXHalf - 2) - 0.3*(yaw - 360)
1096 lcd.drawText(northX, minY+1, "N", SMLSIZE)
1097 lcd.drawText(northX - offset, minY+1, "W", flags)
1098 if (yaw < 290) then
1099 lcd.drawText(northX - offset - offset, minY+1, "S", SMLSIZE)
1100 end
1101 if (yaw > 345) then
1102 lcd.drawText(northX + offset, minY+1, "E", SMLSIZE)
1103 end
1104 end
1105 lcd.drawLine(minX, minY +7, maxX, minY+7, SOLID, 0)
1106 --
1107 local xx = 0
1108 if ( yaw < 10) then
1109 xx = 3
1110 elseif (yaw < 100) then
1111 xx = 0
1112 else
1113 xx = -3
1114 end
1115 lcd.drawNumber(minX + offset + xx, minY, yaw, MIDSIZE+INVERS)
1116 end
1117
1118 local function drawHud()
1119 drawPitch()
1120 drawRoll()
1121 drawYaw()
1122 end
1123
1124 local function drawHomeDirection()
1125 local ox = 163
1126 local oy = 46
1127 local angle = math.floor(yaw - homeAngle)
1128 if ( math.abs(angle) > 45 and math.abs(angle) < 315) then
1129 oy = 44
1130 end
1131 local r1 = 10
1132 local r2 = 5
1133 local x1 = ox + r1 * math.cos(math.rad(angle - 90))
1134 local y1 = oy + r1 * math.sin(math.rad(angle - 90))
1135 local x2 = ox + r2 * math.cos(math.rad(angle - 90 + 120))
1136 local y2 = oy + r2 * math.sin(math.rad(angle - 90 + 120))
1137 local x3 = ox + r2 * math.cos(math.rad(angle - 90 - 120))
1138 local y3 = oy + r2 * math.sin(math.rad(angle - 90 - 120))
1139 --
1140 lcd.drawLine(x1,y1,x2,y2,SOLID,1)
1141 lcd.drawLine(x1,y1,x3,y3,SOLID,1)
1142 lcd.drawLine(ox,oy,x2,y2,SOLID,1)
1143 lcd.drawLine(ox,oy,x3,y3,SOLID,1)
1144 end
1145
1146 local function drawFlightTime()
1147 lcd.drawText(maxX + 1, 1, "T:", SMLSIZE+INVERS)
1148 lcd.drawTimer(lcd.getLastRightPos()+1, 1, flightTime, SMLSIZE+INVERS)
1149 end
1150
1151 local function drawRSSI()
1152 local rssi = getRSSI()
1153 lcd.drawText(212, 1, rssi, SMLSIZE+INVERS+RIGHT)
1154 lcd.drawText(lcd.getLastLeftPos(), 1, "Rssi:", SMLSIZE+INVERS+RIGHT)
1155 end
1156
1157 local function drawTxVoltage()
1158 txVId=getFieldInfo("tx-voltage").id
1159 txV = getValue(txVId)*10
1160
1161 lcd.drawText(150, 1, "Tx:", SMLSIZE+INVERS)
1162 lcd.drawNumber(lcd.getLastRightPos(), 1, txV, SMLSIZE+INVERS+PREC1)
1163 lcd.drawText(lcd.getLastRightPos(), 1, "v", SMLSIZE+INVERS)
1164 end
1165
1166 local function drawCircle(x0,y0,radius)
1167 local x = radius-1
1168 local y = 0
1169 local dx = 1
1170 local dy = 1
1171 local err = dx - bit32.lshift(radius,1)--(radius << 1)
1172
1173 while (x >= y) do
1174 lcd.drawPoint(x0 + x, y0 + y);
1175 lcd.drawPoint(x0 + y, y0 + x);
1176 lcd.drawPoint(x0 - y, y0 + x);
1177 lcd.drawPoint(x0 - x, y0 + y);
1178 lcd.drawPoint(x0 - x, y0 - y);
1179 lcd.drawPoint(x0 - y, y0 - x);
1180 lcd.drawPoint(x0 + y, y0 - x);
1181 lcd.drawPoint(x0 + x, y0 - y);
1182
1183 if err <= 0 then
1184 y=y+1
1185 err = err + dy
1186 dy = dy + 2
1187 end
1188 if err > 0 then
1189
1190 x=x-1
1191 dx = dx + 2
1192 err = err + dx - bit32.lshift(radius,1)--(radius << 1);
1193 end
1194 end
1195 end
1196
1197
1198 local lastStatusArmed = 0
1199 local lastGpsStatus = 0
1200 local lastFlightMode = 0
1201 local lastBattLevel = 0
1202 local batLevel = 99
1203 local batLevels = {}
1204 batLevels[12]=0
1205 batLevels[11]=5
1206 batLevels[10]=10
1207 batLevels[9]=15
1208 batLevels[8]=20
1209 batLevels[7]=25
1210 batLevels[6]=30
1211 batLevels[5]=40
1212 batLevels[4]=50
1213 batLevels[3]=60
1214 batLevels[2]=70
1215 batLevels[1]=80
1216 batLevels[0]=90
1217
1218 local function checkSoundEvents()
1219 if (battCapacity > 0) then
1220 batLevel = (1 - (battMah/battCapacity))*100
1221 else
1222 batLevel = 99
1223 end
1224
1225 if batLevel < (batLevels[lastBattLevel] + 1) and lastBattLevel <= 11 then
1226 playSound("bat"..batLevels[lastBattLevel])
1227 lastBattLevel = lastBattLevel + 1
1228 end
1229
1230 if statusArmed == 1 and lastStatusArmed == 0 then
1231 lastStatusArmed = statusArmed
1232 playSound("armed")
1233 elseif statusArmed == 0 and lastStatusArmed == 1 then
1234 lastStatusArmed = statusArmed
1235 playSound("disarmed")
1236 end
1237
1238 if gpsStatus > 2 and lastGpsStatus <= 2 then
1239 lastGpsStatus = gpsStatus
1240 playSound("gpsfix")
1241 elseif gpsStatus <= 2 and lastGpsStatus > 2 then
1242 lastGpsStatus = gpsStatus
1243 playSound("gpsnofix")
1244 end
1245
1246 if flightMode ~= lastFlightMode then
1247 lastFlightMode = flightMode
1248 playSoundByFrameTypeAndFlightMode(frameType,flightMode)
1249 end
1250 end
1251 --------------------------------------------------------------------------------
1252 -- loop FUNCTIONS
1253 --------------------------------------------------------------------------------
1254 local showMessages = false
1255 --
1256 local function background()
1257 processTelemetry()
1258 end
1259 --
1260 local clock = 0
1261 --
1262 local function symMode()
1263 symAttitude()
1264 symTimer()
1265 symHome()
1266 symGPS()
1267 symBatt()
1268 symFrameType()
1269 end
1270 --
1271 local function run(event)
1272 processTelemetry()
1273 if event == EVT_PLUS_FIRST or event == EVT_MINUS_FIRST then
1274 showMessages = not showMessages
1275 end
1276 --
1277 if (clock % 8 == 0) then
1278 calcBattery()
1279 calcFlightTime()
1280 checkSoundEvents()
1281 clock = 0
1282 end
1283 lcd.clear()
1284 if showMessages then
1285 processTelemetry()
1286 drawAllMessages()
1287 --drawCircle(100,32,30)
1288 else
1289 for r=1,3
1290 do
1291 processTelemetry()
1292 lcd.clear()
1293 --symMode()
1294 drawHud()
1295 drawGrid()
1296 drawBattery()
1297 drawGPSStatus()
1298 checkLandingStatus()
1299 drawMessage()
1300 drawHomeDirection()
1301 drawHome()
1302 drawFlightMode()
1303 drawFlightTime()
1304 drawTxVoltage()
1305 drawRSSI()
1306 drawFailsafe()
1307 end
1308 end
1309 clock = clock + 1
1310 end
1311
1312 local function init()
1313 pushMessage(6,"Yaapu X9D+ telemetry script v1.1")
1314 playSound("yaapu")
1315 end
1316
1317 --------------------------------------------------------------------------------
1318 -- SCRIPT END
1319 --------------------------------------------------------------------------------
1320 return {run=run, background=background, init=init}