SCRIPTS/TELEMETRY/yaapu7.lua

   1 --
   2 -- An FRSKY S.Port <passthrough protocol> based Telemetry script for Taranis X7
   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 local flightModes = {}
  36   flightModes[0]=""
  37   flightModes[1]="Stabilize"
  38   flightModes[2]="Acro"
  39   flightModes[3]="AltHold"
  40   flightModes[4]="Auto"
  41   flightModes[5]="Guided"
  42   flightModes[6]="Loiter"
  43   flightModes[7]="RTL"
  44   flightModes[8]="Circle"
  45   flightModes[9]=""
  46   flightModes[10]="Land"
  47   flightModes[11]=""
  48   flightModes[12]="Drift"
  49   flightModes[13]=""
  50   flightModes[14]="Sport"
  51   flightModes[15]="Flip"
  52   flightModes[16]="AutoTune"
  53   flightModes[17]="PosHold"
  54   flightModes[18]="Brake"
  55   flightModes[19]="Throw"
  56   flightModes[20]="Avoid"
  57   flightModes[21]="GuiNGPS"
  58
  59 local gpsStatuses = {}
  60   gpsStatuses[0]="NoGPS"
  61   gpsStatuses[1]="NoLock"
  62   gpsStatuses[2]="2DFIX"
  63   gpsStatuses[3]="3DFix"
  64
  65 local mavSeverity = {}
  66   mavSeverity[0]="EMR"
  67   mavSeverity[1]="ALR"
  68   mavSeverity[2]="CRT"
  69   mavSeverity[3]="ERR"
  70   mavSeverity[4]="WRN"
  71   mavSeverity[5]="NOT"
  72   mavSeverity[6]="INF"
  73   mavSeverity[7]="DBG"
  74
  75 local soundFileBasePath = "/SOUNDS/yaapu0/en"
  76 local soundFiles = {}
  77         -- battery
  78         soundFiles["bat5"] = "bat5.wav"
  79         soundFiles["bat10"] = "bat10.wav"
  80         soundFiles["bat15"] = "bat15.wav"
  81         soundFiles["bat20"] = "bat20.wav"
  82         soundFiles["bat25"] = "bat25.wav"
  83         soundFiles["bat30"] = "bat30.wav"
  84         soundFiles["bat40"] = "bat40.wav"
  85         soundFiles["bat50"] = "bat50.wav"
  86         soundFiles["bat60"] = "bat60.wav"
  87         soundFiles["bat75"] = "bat75.wav"
  88         -- gps
  89         soundFiles["gpsfix"] = "gpsfix.wav"
  90         soundFiles["gpsnofix"] = "gpsnofix.wav"
  91         -- failsafe
  92         soundFiles["lowbat"] = "lowbat.wav"
  93         soundFiles["ekf"] = "ekf.wav"
  94         -- events
  95         soundFiles["yaapu"] = "yaapu.wav"
  96         soundFiles["landing"] = "landing.wav"
  97         soundFiles["armed"] = "armed.wav"
  98         soundFiles["disarmed"] = "disarmed.wav"
  99         -- events
 100         soundFiles["land"] = "land.wav"
 101         soundFiles["auto"] = "auto.wav"
 102         soundFiles["stabilize"] = "stabilize.wav"
 103         soundFiles["althold"] = "althold.wav"
 104         soundFiles["poshold"] = "poshold.wav"
 105         soundFiles["loiter"] = "loiter.wav"
 106         soundFiles["autotune"] = "autotune.wav"
 107         soundFiles["rtl"] = "rtl.wav"
 108         soundFiles["autotune"] = "autotune.wav"
 109
 110   -- STATUS
 111 local flightMode = 0
 112 local simpleMode = 0
 113 local landComplete = 0
 114 local statusArmed = 0
 115 local batteryFailsafe = 0
 116 local lastBatteryFailsafe = 0
 117 local ekfFailsafe = 0
 118 local lastEkfFailsafe = 0
 119 -- GPS
 120 local numSats = 0
 121 local gpsStatus = 0
 122 local gpsHdopC = 100
 123 local gpsHdopM = 0
 124 local gpsAlt = 0
 125 -- BATT
 126 local battVolt = 0
 127 local battCurrent = 0
 128 local battMah = 0
 129 -- BATT2
 130 local battVolt2 = 0
 131 local battCurrent2 = 0
 132 local battMah2 = 0
 133 -- HOME
 134 local homeDist = 0
 135 local homeAlt = 0
 136 local homeAngle = -1
 137 -- MESSAGES max 10 lines
 138 local messageBuffer = ""
 139 local messageHistory = {}
 140 local severity = 0
 141 local messageIdx = 1
 142 local messageDuplicate = 1
 143 local lastMessage = ""
 144 local lastMessageValue = 0
 145 local lastMessageTime = 0
 146 -- VELANDYAW
 147 local vSpeed = 0
 148 local hSpeed = 0
 149 local yaw = 0
 150 -- ROLLPITCH
 151 local roll = 0
 152 local pitch = 0
 153 -- TELEMETRY
 154 local SENSOR_ID,FRAME_ID,DATA_ID,VALUE
 155 local mult = 0
 156 local c1,c2,c3,c4
 157 -- PARAMS
 158 local paramId,paramValue
 159 local frameType
 160 local battFailsafeVoltage = 0
 161 local battFailsafeCapacity = 0
 162 local battCapacity = 0
 163 --
 164 local minX = 0
 165 local maxX = 60
 166 local minY = 9
 167 local maxY = 55
 168 --
 169 local noTelemetryData = 1
 170
 171 local function playSound(soundFile)
 172         playFile(soundFileBasePath .. "/" .. soundFiles[soundFile])
 173 end
 174
 175 local function getValueOrDefault(value)
 176         local tmp = getValue(value)
 177         if tmp == nil then
 178                 return 0
 179         end
 180         return tmp
 181 end
 182
 183 local function pushMessage(severity, msg)
 184         if ( severity < 4) then
 185                 playTone(400,300,0)
 186         else
 187                 playTone(600,300,0)
 188         end
 189         local mm = msg
 190         if msg == lastMessage then
 191                 messageDuplicate = messageDuplicate + 1
 192                 if messageDuplicate > 1 then
 193                         if string.len(mm) > 19 then
 194                                 mm = string.sub(mm,1,19)
 195                                 messageHistory[messageIdx - 1] = string.format("%d.%s %-20s (x%d)", messageIdx - 1, mavSeverity[severity], mm, messageDuplicate)
 196                         else
 197                                 messageHistory[messageIdx - 1] = string.format("%d.%s %s (x%d)", messageIdx - 1, mavSeverity[severity], msg, messageDuplicate)
 198                         end
 199                 end
 200         else
 201                 messageHistory[messageIdx] = string.format("%d.%s %s", messageIdx, mavSeverity[severity], msg)
 202                 messageIdx = messageIdx + 1
 203                 lastMessage = msg
 204                 messageDuplicate = 1
 205         end
 206         lastMessageTime = getTime() -- valore in secondi
 207 end
 208
 209 local seconds = 0
 210 local lastTimerStart = 0
 211 --
 212 local function startTimer()
 213         lastTimerStart = getTime()/100
 214 end
 215
 216 local function stopTimer()
 217         seconds = seconds + getTime()/100 - lastTimerStart
 218         lastTimerStart = 0
 219 end
 220
 221 local function symTimer()
 222         thrOut = getValue("thr")
 223         if (thrOut > -500 ) then
 224                 landComplete = 1
 225         else
 226                 landComplete = 0
 227         end
 228 end
 229
 230 local function symGPS()
 231         thrOut = getValue("thr")
 232         if (thrOut > 0 ) then
 233                 numSats = 9
 234                 gpsStatus = 3
 235                 gpsHdopC = 11
 236                 ekfFailsafe = 0
 237                 batteryFailsafe = 0
 238                 noTelemetryData = 0
 239         elseif thrOut > -500  then
 240                 numSats = 6
 241                 gpsStatus = 3
 242                 gpsHdopC = 25
 243                 ekfFailsafe = 1
 244                 batteryFailsafe = 1
 245                 noTelemetryData = 0
 246         else
 247                 numSats = 0
 248                 gpsStatus = 0
 249                 gpsHdopC = 100
 250                 ekfFailsafe = 0
 251                 batteryFailsafe = 0
 252                 noTelemetryData = 1
 253         end
 254 end
 255
 256 local function symBatt()
 257         thrOut = getValue("thr")
 258         if (thrOut > 0 ) then
 259                 LIPObatt = 1350 + ((thrOut)*0.01 * 30)
 260                 LIPOcelm = LIPObatt/4
 261                 battCurrent = 100 +  ((thrOut)*0.01 * 30)
 262                 battVolt = LIPObatt*0.1
 263                 battCapacity = 10500
 264                 battMah = 5200
 265                 flightMode = 1
 266                 statusArmed = 1
 267                 simpleMode = 1
 268                 homeDist = math.abs(thrOut)*2
 269         end
 270 end
 271
 272 -- simulates attitude by using channel 1 for roll, channel 2 for pitch and channel 4 for yaw
 273 local function symAttitude()
 274         local rollCh = 0
 275         local pitchCh = 0
 276         local yawCh = 0
 277         -- roll [-1024,1024] ==> [-180,180]
 278         rollCh = getValue("ch1") * 0.175
 279         -- pitch [1024,-1024] ==> [-90,90]
 280         pitchCh = getValue("ch2") * 0.0878
 281         -- yaw [-1024,1024] ==> [0,360]
 282         yawCh = getValue("ch10")
 283         if ( yawCh >= 0) then
 284                 yawCh = yawCh * 0.175
 285         else
 286                 yawCh = 360 + (yawCh * 0.175)
 287         end
 288         roll = rollCh/3
 289         pitch = pitchCh/2
 290         yaw = yawCh
 291 end
 292
 293 local function symHome()
 294         local yawCh = 0
 295         local S2Ch = 0
 296         -- home angle in deg [0-360]
 297         S2Ch = getValue("ch12")
 298         yawCh = getValue("ch4")
 299         homeAlt = yawCh
 300         vSpeed = yawCh * 0.1
 301         if ( yawCh >= 0) then
 302                 yawCh = yawCh * 0.175
 303         else
 304                 yawCh = 360 + (yawCh * 0.175)
 305         end
 306         if ( S2Ch >= 0) then
 307                 S2Ch = S2Ch * 0.175
 308         else
 309                 S2Ch = 360 + (S2Ch * 0.175)
 310         end
 311         if (thrOut > 0 ) then
 312                 homeAngle = S2Ch
 313         else
 314                 homeAngle = -1
 315         end
 316         yaw = yawCh
 317 end
 318
 319 local function processTelemetry()
 320   SENSOR_ID,FRAME_ID,DATA_ID,VALUE = sportTelemetryPop()
 321         --
 322         --FRAME_ID = 0x10
 323         --DATA_ID = 0x5002
 324         --VALUE = 0xf4006b8
 325         --
 326         --FRAME_ID = 0x10
 327         --DATA_ID = 0x5004
 328         --VALUE = 0x168000
 329         --
 330         if ( FRAME_ID == 0x10) then
 331                 noTelemetryData = 0
 332                 if ( DATA_ID == 0x5006) then -- ROLLPITCH
 333                         -- roll [0,1800] ==> [-180,180]
 334                         roll = (bit32.extract(VALUE,0,11) - 900) * 0.2
 335                         -- pitch [0,900] ==> [-90,90]
 336                         pitch = (bit32.extract(VALUE,11,10) - 450) * 0.2
 337                 elseif ( DATA_ID == 0x5005) then -- VELANDYAW
 338                         vSpeed = bit32.extract(VALUE,1,7) * (10^bit32.extract(VALUE,0,1))
 339                         if (bit32.extract(VALUE,8,1) == 1) then
 340                                 vSpeed = -vSpeed
 341                         end
 342                         hSpeed = bit32.extract(VALUE,10,7) * (10^bit32.extract(VALUE,9,1))
 343                         yaw = bit32.extract(VALUE,17,11) * 0.2
 344                 elseif ( DATA_ID == 0x5001) then -- AP STATUS
 345                         flightMode = bit32.extract(VALUE,0,5)
 346                         simpleMode = bit32.extract(VALUE,5,2)
 347                         landComplete = bit32.extract(VALUE,7,1)
 348                         statusArmed = bit32.extract(VALUE,8,1)
 349                         batteryFailsafe = bit32.extract(VALUE,9,1)
 350                         ekfFailsafe = bit32.extract(VALUE,10,2)
 351                 elseif ( DATA_ID == 0x5002) then -- GPS STATUS
 352                         numSats = bit32.extract(VALUE,0,4)
 353                         gpsStatus = bit32.extract(VALUE,4,2)
 354                         gpsHdopC = bit32.extract(VALUE,7,7) * (10^bit32.extract(VALUE,6,1)) -- dm
 355                         gpsAlt = bit32.extract(VALUE,24,7) * (10^bit32.extract(VALUE,22,2)) -- dm
 356                         if (bit32.extract(VALUE,31,1) == 1) then
 357                                 gpsAlt = gpsAlt * -1
 358                         end
 359                 elseif ( DATA_ID == 0x5003) then -- BATT
 360                         battVolt = bit32.extract(VALUE,0,9)
 361                         battCurrent = bit32.extract(VALUE,10,7) * (10^bit32.extract(VALUE,9,1))
 362                         battMah = bit32.extract(VALUE,17,15)
 363                 elseif ( DATA_ID == 0x5008) then -- BATT2
 364                         battVolt2 = bit32.extract(VALUE,0,9)
 365                         battCurrent2 = bit32.extract(VALUE,10,7) * (10^bit32.extract(VALUE,9,1))
 366                         battMah2 = bit32.extract(VALUE,17,15)
 367                 elseif ( DATA_ID == 0x5004) then -- HOME
 368                         homeDist = bit32.extract(VALUE,2,10) * (10^bit32.extract(VALUE,0,2))
 369                         homeAlt = bit32.extract(VALUE,14,10) * (10^bit32.extract(VALUE,12,2)) * 0.1
 370                         if (bit32.extract(VALUE,24,1) == 1) then
 371                                 homeAlt = homeAlt * -1
 372                         end
 373                         homeAngle = bit32.extract(VALUE, 25,  7) * 3
 374                 elseif ( DATA_ID == 0x5000) then -- MESSAGES
 375                         if (VALUE ~= lastMessageValue) then
 376                                 lastMessageValue = VALUE
 377                                 c1 = bit32.extract(VALUE,0,7)
 378                                 c2 = bit32.extract(VALUE,8,7)
 379                                 c3 = bit32.extract(VALUE,16,7)
 380                                 c4 = bit32.extract(VALUE,24,7)
 381                                 messageBuffer = messageBuffer .. string.char(c4)
 382                                 messageBuffer = messageBuffer .. string.char(c3)
 383                                 messageBuffer = messageBuffer .. string.char(c2)
 384                                 messageBuffer = messageBuffer .. string.char(c1)
 385                                 if (c1 == 0 or c2 == 0 or c3 == 0 or c4 == 0) then
 386                                         severity = (bit32.extract(VALUE,15,1) * 4) + (bit32.extract(VALUE,23,1) * 2) + (bit32.extract(VALUE,30,1) * 1)
 387                                         pushMessage( severity, messageBuffer)
 388                                         messageBuffer = ""
 389                                 end
 390                         end
 391                 elseif ( DATA_ID == 0x5007) then -- PARAMS
 392                         paramId = bit32.extract(VALUE,24,4)
 393                         paramValue = bit32.extract(VALUE,0,24)
 394                         if paramId == 1 then
 395                                 frameType = paramValue
 396                         elseif paramId == 2 then
 397                                 battFailsafeVoltage = paramValue
 398                         elseif paramId == 3 then
 399                                 battFailsafeCapacity = paramValue
 400                         elseif paramId == 4 then
 401                                 battCapacity = paramValue
 402                         end
 403                 end
 404         end
 405 end
 406
 407 local function telemetryEnabled()
 408         if getValue("RxBt") == 0 then
 409                 noTelemetryData = 1
 410         end
 411         --return true
 412         return noTelemetryData == 0
 413 end
 414
 415 local battSource = "na"
 416
 417 local function calcBattery()
 418         local battA2 = 0
 419         local cellCount = 3;
 420         --
 421         cellmin = cellfull
 422         cellResult = getValue("Cels")
 423
 424         if type(cellResult) == "table" then
 425                 battSource="vs"
 426                 cellsum = 0
 427                 for i = 1, #cell do cell[i] = 0 end
 428                         cellsumtype = #cellResult
 429                 for i, v in pairs(cellResult) do
 430                         cellsum = cellsum + v
 431                         cell[i] = v
 432                         if cellmin > v then
 433                                 cellmin = v
 434                         end
 435                 end -- end for
 436         else
 437                 -- cels is not defined let's check if A2 is defined
 438                 cellmin = 0
 439                 battA2 = getValue("A2")
 440                 --
 441                 if battA2 > 0 then
 442                         battSource="a2"
 443                                 if battA2 > 21 then
 444                                         cellCount = 6
 445                                 elseif battA2 > 17 then
 446                                         cellCount = 5
 447                                 elseif battA2 > 13 then
 448                                         cellCount = 4
 449                                 else
 450                                         cellCount = 3
 451                                 end
 452                         --
 453                         cellmin = battA2/cellCount
 454                         cellsum = battA2
 455                 else
 456                         -- A2 is not defined, last chance is battVolt
 457                         if battVolt > 0 then
 458                                 battSource="fc"
 459                                 cellsum = battVolt*0.1
 460                                 if cellsum > 21 then
 461                                         cellCount = 6
 462                                 elseif cellsum > 17 then
 463                                         cellCount = 5
 464                                 elseif cellsum > 13 then
 465                                         cellCount = 4
 466                                 else
 467                                         cellCount = 3
 468                                 end
 469                                 --
 470                                 cellmin = cellsum/cellCount
 471                         end
 472                 end
 473         end -- end if
 474         --
 475         LIPOcelm = cellmin*100
 476         LIPObatt = cellsum*100
 477 end
 478
 479 local function drawBattery()
 480         if (battCapacity > 0) then
 481                 LIPOperc = (1 - (battMah/battCapacity))*100
 482         else
 483                 LIPOperc = 0
 484         end
 485         lcd.drawRectangle(maxX+1,17,28,16,SOLID)
 486         lcd.drawFilledRectangle(maxX+1,17,28,16,SOLID)
 487         -- display battery voltage
 488         lcd.drawText(maxX + 28, 19, "v", SMLSIZE+RIGHT+INVERS)
 489         lcd.drawNumber(lcd.getLastLeftPos(), 18, LIPObatt, PREC2+RIGHT+INVERS)
 490         -- display battery current
 491         lcd.drawText(maxX + 28, 26, "A", SMLSIZE+RIGHT+INVERS)
 492         lcd.drawNumber(lcd.getLastLeftPos(), 26, battCurrent, SMLSIZE+PREC1+RIGHT+INVERS)
 493
 494         -- display lowest cell voltage
 495         if LIPOcelm < 350 then
 496                 lcd.drawNumber(maxX + 30, 16, LIPOcelm, DBLSIZE+BLINK+PREC2)
 497         else
 498                 lcd.drawNumber(maxX + 30, 16, LIPOcelm, DBLSIZE+PREC2)
 499         end
 500         lcd.drawText(lcd.getLastRightPos(), 17, "v", SMLSIZE)
 501         lcd.drawText(128, 25, battSource, SMLSIZE+RIGHT)
 502
 503         -- display capacity %
 504         local yy = 48
 505         lcd.drawText(128, yy, "Ah", SMLSIZE+RIGHT)
 506         lcd.drawNumber(lcd.getLastLeftPos(), yy, battCapacity/100, SMLSIZE+PREC1+RIGHT)
 507         lcd.drawText(lcd.getLastLeftPos(), yy, "/", SMLSIZE+RIGHT)
 508         lcd.drawNumber(lcd.getLastLeftPos(), yy, battMah/100, SMLSIZE+PREC1+RIGHT)
 509         --
 510         lcd.drawRectangle(maxX +1,yy-2,17,12,SOLID)
 511         lcd.drawFilledRectangle(maxX +1,yy-2,17,12,SOLID)
 512         --
 513         lcd.drawText(maxX+17, yy, "%", SMLSIZE+INVERS+RIGHT)
 514         lcd.drawNumber(lcd.getLastLeftPos(), yy-1, LIPOperc, INVERS+RIGHT)
 515 end
 516
 517 local function drawFlightMode()
 518         lcd.drawFilledRectangle(0,0, 128, 9, SOLID)
 519         lcd.drawRectangle(0, 0, 128, 9, SOLID)
 520
 521         if (not telemetryEnabled()) then
 522                 lcd.drawFilledRectangle((128-100)/2,18, 100, 30, SOLID)
 523                 lcd.drawText(17, 29, "no telemetry data", INVERS)
 524                 return
 525         end
 526
 527         local strMode = flightModes[flightMode]
 528         --
 529         lcd.drawText(1, 1, strMode, SMLSIZE+INVERS)
 530
 531         if ( simpleMode == 1) then
 532                 lcd.drawText(lcd.getLastRightPos(), 1, "(S)", SMLSIZE+INVERS)
 533         end
 534
 535         if (statusArmed == 1) then
 536                 lcd.drawText(18, 47, "ARMED", SMLSIZE+INVERS)
 537         else
 538                 lcd.drawText(18, 47, "DISARMED", SMLSIZE+INVERS+BLINK)
 539         end
 540 end
 541
 542 local function drawHome()
 543         local xx = maxX + 2
 544         local yy = 10
 545         --
 546         lcd.drawLine(xx,13,xx + 8,13, SOLID, 0)
 547         -- left arrow
 548         lcd.drawLine(xx+1,12,xx + 2,11, SOLID, 0)
 549         lcd.drawLine(xx+1,14,xx + 2,15, SOLID, 0)
 550         -- right arrow
 551         lcd.drawLine(xx+7,12,xx + 6,11, SOLID, 0)
 552         lcd.drawLine(xx+7,14,xx + 6,15, SOLID, 0)
 553
 554         lcd.drawNumber(xx + 11, yy, homeDist, SMLSIZE)
 555         lcd.drawText(lcd.getLastRightPos(), yy, "m",SMLSIZE)
 556 end
 557
 558 local function drawMessage()
 559         lcd.drawFilledRectangle(0,55, 212, 9, SOLID)
 560         lcd.drawRectangle(0, 55, 212, 9, SOLID)
 561         local now = getTime()
 562         if (now - lastMessageTime ) > 300 then
 563                 lcd.drawText(1, 56, messageHistory[messageIdx-1],SMLSIZE+INVERS)
 564         else
 565                 lcd.drawText(1, 56, messageHistory[messageIdx-1],SMLSIZE+INVERS+BLINK)
 566         end
 567 end
 568
 569 local function drawAllMessages()
 570         local idx = 1
 571         if (messageIdx <= 6) then
 572                 for i = 1, messageIdx - 1 do
 573                         lcd.drawText(1, 1+10*(idx - 1), messageHistory[i],SMLSIZE)
 574                         idx = idx+1
 575                 end
 576         else
 577                 for i = messageIdx - 6,messageIdx - 1 do
 578                         lcd.drawText(1, 1+10*(idx-1), messageHistory[i],SMLSIZE)
 579                         idx = idx+1
 580                 end
 581         end
 582 end
 583
 584 local function drawGPSStatus()
 585         local xx = 89
 586         local yy = 33
 587         --
 588         lcd.drawRectangle(xx,yy - 1,50,15,SOLID)
 589         --
 590         local strStatus = gpsStatuses[gpsStatus]
 591         --
 592         local flags = BLINK
 593         if gpsStatus  > 2 then
 594                 lcd.drawFilledRectangle(xx,yy - 1,50,15,SOLID)
 595                 if homeAngle ~= -1 then
 596                         flags = 0
 597                 end
 598
 599                 lcd.drawText(xx + 2, yy, strStatus, SMLSIZE+INVERS)
 600                 lcd.drawNumber(128, yy, numSats, SMLSIZE+INVERS+RIGHT)
 601                 lcd.drawText(xx + 2, yy + 7, "Hdop ", SMLSIZE+INVERS)
 602                 --
 603                 if gpsHdopC > 100 then
 604                         lcd.drawText(128, yy + 7, "10+", SMLSIZE+INVERS+RIGHT+flags)
 605                 else
 606                         lcd.drawNumber(128, yy + 7, gpsHdopC , SMLSIZE+INVERS+RIGHT+PREC1+flags)
 607                 end
 608         else
 609                 lcd.drawText(xx + 5, yy + 3, strStatus, BLINK)
 610         end
 611 end
 612
 613 local function drawGrid()
 614         lcd.drawLine(maxX, 0, maxX, 63, SOLID, 0)
 615 end
 616
 617 local timerRunning = 0
 618
 619 local function checkLandingStatus()
 620         if ( timerRunning == 0 and landComplete == 1 and lastTimerStart == 0) then
 621                 startTimer()
 622         end
 623         if (timerRunning == 1 and landComplete == 0 and lastTimerStart ~= 0) then
 624                 stopTimer()
 625                 playSound("landing")
 626         end
 627         timerRunning = landComplete
 628 end
 629 local flightTime = 0
 630
 631 local function calcFlightTime()
 632         local elapsed = 0
 633         if ( lastTimerStart ~= 0) then
 634                 elapsed = getTime()/100 - lastTimerStart
 635         end
 636         flightTime = elapsed + seconds
 637 end
 638
 639 -- draws a line centered at ox,oy with given angle and length W/O CROPPING
 640 local function drawLine(ox,oy,angle,len,style,maxX,maxY)
 641         local xx = math.cos(math.rad(angle)) * len * 0.5
 642         local yy = math.sin(math.rad(angle)) * len * 0.5
 643
 644         local x1 = ox - xx
 645         local x2 = ox + xx
 646         local y1 = oy - yy
 647         local y2 = oy + yy
 648         --
 649         lcd.drawLine(x1,y1,x2,y2, style,0)
 650 end
 651
 652 -- draws a line centered at ox,oy with given angle and length WITH CROPPING
 653 local function drawCroppedLine(ox,oy,angle,len,style,maxX,maxY)
 654         --
 655         local xx = math.cos(math.rad(angle)) * len * 0.5
 656         local yy = math.sin(math.rad(angle)) * len * 0.5
 657         --
 658         local x1 = ox - xx
 659         local x2 = ox + xx
 660         local y1 = oy - yy
 661         local y2 = oy + yy
 662         --
 663         if (x1 >= maxX and x2 >= maxX) then
 664                 return
 665         end
 666         if (x1 >= maxX) then
 667                 y1 = y1 - math.tan(math.rad(angle)) * (maxX - x1)
 668                 x1 = maxX - 1
 669         end
 670         if (x2 >= maxX) then
 671                 y2 = y2 + math.tan(math.rad(angle)) * (maxX - x2)
 672                 x2 = maxX - 1
 673         end
 674         lcd.drawLine(x1,y1,x2,y2, style,0)
 675 end
 676
 677 local function drawFailsafe()
 678         if ekfFailsafe > 0 then
 679                 if lastEkfFailsafe == 0 then
 680                         playSound("ekf")
 681                 end
 682                 lcd.drawText(maxX/2 - 28, 47, "EKF FAILSAFE", SMLSIZE+INVERS+BLINK)
 683         end
 684         if batteryFailsafe > 0 then
 685                 if lastBatteryFailsafe == 0 then
 686                         playSound("lowbat")
 687                 end
 688                 lcd.drawText(maxX/2 - 28, 47, "BAT FAILSAFE", SMLSIZE+INVERS+BLINK)
 689         end
 690         lastEkfFailsafe = ekfFailsafe
 691         lastBatteryFailsafe = batteryFailsafe
 692 end
 693
 694 local function drawPitch()
 695         local y = 0
 696         local p = pitch
 697         -- horizon min max +/- 30°
 698         if ( pitch > 0) then
 699                 if (pitch > 30) then
 700                         p = 30
 701                 end
 702         else
 703                 if (pitch < -30) then
 704                         p = -30
 705                 end
 706         end
 707         -- y normalized at 32 +/-20  (0.75 = 20/32)
 708         y = 32 + 0.75*p
 709         -- horizon, lower half of HUD filled in grey
 710         --lcd.drawFilledRectangle(minX,y,maxX-minX,maxY-y + 1,GREY_DEFAULT)
 711         --
 712         -- center indicators for vSpeed and alt
 713         local width = 17
 714         lcd.drawLine(minX,32 - 5,minX + width,32 - 5, SOLID, 0)
 715         lcd.drawLine(minX,32 + 4,minX + width,32 + 4, SOLID, 0)
 716         lcd.drawLine(minX + width + 1,32 + 4,minX + width + 5,32, SOLID, 0)
 717         lcd.drawLine(minX + width + 1,32 - 4,minX + width + 5,32, SOLID, 0)
 718         lcd.drawPoint(minX + width + 5,32)
 719         --
 720         lcd.drawLine(maxX - width - 1,32 - 5,maxX - 1,32 - 5,SOLID,0)
 721         lcd.drawLine(maxX - width - 1,32 + 4,maxX - 1,32 + 4,SOLID,0)
 722         lcd.drawLine(maxX - width - 2,32 + 4,maxX - width - 6,32, SOLID, 0)
 723         lcd.drawLine(maxX - width - 2,32 - 4,maxX - width - 6,32, SOLID, 0)
 724         lcd.drawPoint(maxX - width - 6,32)
 725         -- center value based on char count
 726         if homeAlt > 0 then
 727                 if homeAlt < 10 then -- 2 digits with decimal
 728                         lcd.drawNumber(maxX - 1,32 - 3,homeAlt * 10,SMLSIZE+PREC1+RIGHT)
 729                 else -- 3 digits
 730                         lcd.drawNumber(maxX - 1 + 2,32 - 3,homeAlt,SMLSIZE+RIGHT)
 731                 end
 732         else
 733                 if homeAlt > -10 then -- 1 digit with sign
 734                         lcd.drawNumber(maxX - 1,32 - 3,homeAlt * 10,SMLSIZE+PREC1+RIGHT)
 735                 else -- 3 digits with sign
 736                         lcd.drawNumber(maxX - 1,32 - 3,homeAlt,SMLSIZE+RIGHT)
 737                 end
 738         end
 739         --
 740         if (vSpeed > 999) then
 741                 lcd.drawNumber(minX + 1,32 - 3,vSpeed*0.1,SMLSIZE)
 742         elseif (vSpeed < -99) then
 743                 lcd.drawNumber(minX + 1,32 - 3,vSpeed * 0.1,SMLSIZE)
 744         else
 745                 lcd.drawNumber(minX + 1,32 - 3,vSpeed,SMLSIZE+PREC1)
 746         end
 747         -- up pointing center arrow
 748         local x = math.floor(maxX/2)
 749         lcd.drawLine(x-10,34 + 5,x ,34 ,SOLID,0)
 750         lcd.drawLine(x+1 ,34 ,x + 10, 34 + 5,SOLID,0)
 751 end
 752
 753 local function drawRoll()
 754         local r = -roll
 755         local r2 = 10 --vertical distance between roll horiz segments
 756         local cx,cy,dx,dy,ccx,ccy,cccx,cccy
 757         -- no roll ==> segments are vertical, offsets are multiples of r2
 758         if ( roll == 0) then
 759                 dx=0
 760                 dy=pitch
 761                 cx=0
 762                 cy=r2
 763                 ccx=0
 764                 ccy=2*r2
 765                 cccx=0
 766                 cccy=3*r2
 767         else
 768                 -- center line offsets
 769                 dx = math.cos(math.rad(90 - r)) * -pitch
 770                 dy = math.sin(math.rad(90 - r)) * pitch
 771                 -- 1st line offsets
 772                 cx = math.cos(math.rad(90 - r)) * r2
 773                 cy = math.sin(math.rad(90 - r)) * r2
 774                 -- 2nd line offsets
 775                 ccx = math.cos(math.rad(90 - r)) * 2 * r2
 776                 ccy = math.sin(math.rad(90 - r)) * 2 * r2
 777                 -- 3rd line offsets
 778                 cccx = math.cos(math.rad(90 - r)) * 3 * r2
 779                 cccy = math.sin(math.rad(90 - r)) * 3 * r2
 780         end
 781         local x = math.floor(maxX/2)
 782         drawCroppedLine(dx + x - cccx,dy + 32 + cccy,r,4,DOTTED,maxX,maxY)
 783         drawCroppedLine(dx + x - ccx,dy + 32 + ccy,r,6,DOTTED,maxX,maxY)
 784         drawCroppedLine(dx + x - cx,dy + 32 + cy,r,9,DOTTED,maxX,maxY)
 785         drawCroppedLine(dx + x,dy + 32,r,16,SOLID,maxX,maxY)
 786         drawCroppedLine(dx + x + cx,dy + 32 - cy,r,9,DOTTED,maxX,maxY)
 787         drawCroppedLine(dx + x + ccx,dy + 32 - ccy,r,6,DOTTED,maxX,maxY)
 788         drawCroppedLine(dx + x + cccx,dy + 32 - cccy,r,4,DOTTED,maxX,maxY)
 789 end
 790
 791 local function roundTo(val,int)
 792         return math.floor(val/int) * int
 793 end
 794
 795 local function drawYaw()
 796         local northX = math.floor(maxX/2) - 2
 797         local offset = northX - 4
 798         local yawRounded =  math.floor(yaw)
 799         local yawWindow = roundTo(yaw,10)
 800         local maxXHalf = math.floor(maxX/2)
 801         local yawWindowOn = false
 802         local flags = SMLSIZE
 803         if (not(yawWindow == 0 or yawWindow == 90 or yawWindow == 180 or yawWindow == 270 or yawWindow == 360)) then
 804                 yawWindowOn = true
 805         end
 806         --
 807         if (yawRounded == 0 or yawRounded == 360) then
 808                 lcd.drawText(northX - offset, minY+1, "W", SMLSIZE)
 809                 lcd.drawText(northX, minY+1, "N", flags)
 810                 lcd.drawText(northX + offset, minY+1, "E", SMLSIZE)
 811         elseif (yawRounded == 90) then
 812                 lcd.drawText(northX - offset, minY+1, "N", SMLSIZE)
 813                 lcd.drawText(northX, minY+1, "E", flags)
 814                 lcd.drawText(northX + offset, minY+1, "S", SMLSIZE)
 815         elseif (yawRounded == 180) then
 816                 lcd.drawText(northX - offset, minY+1, "W", SMLSIZE)
 817                 lcd.drawText(northX, minY+1, "S", flags)
 818                 lcd.drawText(northX + offset, minY+1, "E", SMLSIZE)
 819         elseif (yawRounded == 270) then
 820                 lcd.drawText(northX - offset, minY+1, "S", SMLSIZE)
 821                 lcd.drawText(northX, minY+1, "W", flags)
 822                 lcd.drawText(northX + offset, minY+1, "N", SMLSIZE)
 823         elseif ( yaw > 0 and yaw < 90) then
 824                 northX = (maxXHalf - 2) - 0.3*yaw
 825                 lcd.drawText(northX, minY+1, "N", flags)
 826                 lcd.drawText(northX + offset, minY+1, "E", SMLSIZE)
 827                 if (yaw > 75) then
 828                         lcd.drawText(northX + offset + offset, minY+1, "S", SMLSIZE)
 829                 end
 830         elseif ( yaw > 90 and yaw < 180) then
 831                 northX = (maxXHalf - 2) - 0.3*(yaw - 180)
 832                 lcd.drawText(northX - offset, minY+1, "E", SMLSIZE)
 833                 lcd.drawText(northX, minY+1, "S", flags)
 834                 if (yaw > 170) then
 835                         lcd.drawText(northX + offset, minY+1, "W", SMLSIZE)
 836                 end
 837         elseif ( yaw > 180 and yaw < 270) then
 838                 northX = (maxXHalf - 2) - 0.3*(yaw - 270)
 839                 lcd.drawText(northX, minY+1, "W", SMLSIZE)
 840                 lcd.drawText(northX - offset, minY+1, "S", flags)
 841                 if (yaw < 190) then
 842                         lcd.drawText(northX - offset - offset, minY+1, "E", SMLSIZE)
 843                 end
 844         elseif ( yaw > 270 and yaw < 360) then
 845                 northX = (maxXHalf - 2) - 0.3*(yaw - 360)
 846                 lcd.drawText(northX, minY+1, "N", SMLSIZE)
 847                 lcd.drawText(northX - offset, minY+1, "W", flags)
 848                 if (yaw < 290) then
 849                         lcd.drawText(northX - offset - offset, minY+1, "S", SMLSIZE)
 850                 end
 851                 if (yaw > 345) then
 852                         lcd.drawText(northX + offset, minY+1, "E", SMLSIZE)
 853                 end
 854         end
 855         lcd.drawLine(minX, minY +7, maxX, minY+7, SOLID, 0)
 856         --
 857         local xx = 0
 858         if ( yaw < 10) then
 859                 xx = 3
 860         elseif (yaw < 100) then
 861                 xx = 0
 862         else
 863                 xx = -3
 864         end
 865         lcd.drawNumber(minX + offset + xx, minY, yaw, MIDSIZE+INVERS)
 866 end
 867
 868 local function drawHud()
 869         drawPitch()
 870         drawRoll()
 871         drawYaw()
 872 end
 873
 874 local function drawHomeDirection()
 875         local ox = maxX + 14
 876         local oy = 41
 877         local angle = math.floor( yaw - homeAngle)
 878         if ( math.abs(angle) > 45 and math.abs(angle) < 315) then
 879                 oy = 39
 880         end
 881
 882         local r1 = 8
 883         local r2 = 5
 884         local x1 = ox + r1 * math.cos(math.rad(angle - 90))
 885         local y1 = oy + r1 * math.sin(math.rad(angle - 90))
 886         local x2 = ox + r2 * math.cos(math.rad(angle - 90 + 120))
 887         local y2 = oy + r2 * math.sin(math.rad(angle - 90 + 120))
 888         local x3 = ox + r2 * math.cos(math.rad(angle - 90 - 120))
 889         local y3 = oy + r2 * math.sin(math.rad(angle - 90 - 120))
 890         --
 891         lcd.drawLine(x1,y1,x2,y2,SOLID,1)
 892         lcd.drawLine(x1,y1,x3,y3,SOLID,1)
 893         lcd.drawLine(ox,oy,x2,y2,SOLID,1)
 894         lcd.drawLine(ox,oy,x3,y3,SOLID,1)
 895 end
 896
 897 local function drawFlightTime()
 898         lcd.drawText(maxX, 1, "T:", SMLSIZE+INVERS)
 899         lcd.drawTimer(lcd.getLastRightPos(), 1, flightTime, SMLSIZE+INVERS)
 900 end
 901
 902 local function drawRSSI()
 903         local rssi = getRSSI()
 904         lcd.drawText(128, 1, rssi, SMLSIZE+INVERS+RIGHT)
 905         lcd.drawText(lcd.getLastLeftPos(), 1, "Rssi:", SMLSIZE+INVERS+RIGHT)
 906 end
 907
 908 local function drawTxVoltage()
 909         txVId=getFieldInfo("tx-voltage").id
 910         txV = getValue(txVId)*10
 911
 912         lcd.drawText(128, 10, "v", SMLSIZE+RIGHT)
 913         lcd.drawNumber(lcd.getLastLeftPos(), 10, txV, SMLSIZE+PREC1+RIGHT)
 914         lcd.drawText(lcd.getLastLeftPos(), 10, "Tx:", SMLSIZE+RIGHT)
 915 end
 916 --
 917
 918 local lastStatusArmed = 0
 919 local lastGpsStatus = 0
 920 local lastFlightMode = 0
 921 local lastBattLevel = 0
 922 local batLevel = 99
 923 local batLevels = {}
 924         batLevels[10]=0
 925         batLevels[9]=5
 926         batLevels[8]=10
 927         batLevels[7]=15
 928         batLevels[6]=20
 929         batLevels[5]=25
 930         batLevels[4]=30
 931         batLevels[3]=40
 932         batLevels[2]=50
 933         batLevels[1]=60
 934         batLevels[0]=75
 935
 936 local function checkSoundEvents()
 937         if (battCapacity > 0) then
 938                 batLevel = (1 - (battMah/battCapacity))*100
 939         else
 940                 batLevel = 99
 941         end
 942
 943         if batLevel < (batLevels[lastBattLevel] + 1) and lastBattLevel <= 9 then
 944                 playSound("bat"..batLevels[lastBattLevel])
 945                 lastBattLevel = lastBattLevel + 1
 946         end
 947
 948         if statusArmed == 1 and lastStatusArmed == 0 then
 949                 lastStatusArmed = statusArmed
 950                 playSound("armed")
 951         elseif statusArmed == 0 and lastStatusArmed == 1 then
 952                 lastStatusArmed = statusArmed
 953                 playSound("disarmed")
 954         end
 955
 956         if gpsStatus > 2 and lastGpsStatus <= 2 then
 957                 lastGpsStatus = gpsStatus
 958                 playSound("gpsfix")
 959         elseif gpsStatus <= 2 and lastGpsStatus > 2 then
 960                 lastGpsStatus = gpsStatus
 961                 playSound("gpsnofix")
 962         end
 963
 964         if flightMode ~= lastFlightMode then
 965                 lastFlightMode = flightMode
 966                 if flightMode == 1 then
 967                         playSound("stabilize")
 968                 elseif flightMode == 3 then
 969                         playSound("althold")
 970                 elseif flightMode == 4 then
 971                         playSound("auto")
 972                 elseif flightMode == 6 then
 973                         playSound("loiter")
 974                 elseif flightMode == 7 then
 975                         playSound("rtl")
 976                 elseif flightMode == 10 then
 977                         playSound("land")
 978                 elseif flightMode == 16 then
 979                         playSound("autotune")
 980                 elseif flightMode == 17 then
 981                         playSound("poshold")
 982                 end
 983         end
 984 end
 985 --------------------------------------------------------------------------------
 986 -- loop FUNCTIONS
 987 --------------------------------------------------------------------------------
 988 local showMessages = false
 989 local showConfig = false
 990 --
 991 local function background()
 992         processTelemetry()
 993 end
 994 --
 995 local clock = 0
 996 --
 997 local function symMode()
 998         symAttitude()
 999         symTimer()
1000         symHome()
1001         symGPS()
1002         symBatt()
1003 end
1004 --
1005 local function run(event)
1006         processTelemetry()
1007         if event == EVT_ENTER_BREAK  then
1008                 showMessages = not showMessages
1009         end
1010         --
1011         if (clock % 8 == 0) then
1012                 calcBattery()
1013                 calcFlightTime()
1014                 checkSoundEvents()
1015                 clock = 0
1016         end
1017         lcd.clear()
1018         if showMessages then
1019                 processTelemetry()
1020                 drawAllMessages()
1021         else
1022                 for r=1,3
1023                 do
1024                         processTelemetry()
1025                         lcd.clear()
1026                         --symMode()
1027                         drawHud()
1028                         drawGrid()
1029                         drawBattery()
1030                         drawGPSStatus()
1031                         checkLandingStatus()
1032                         drawMessage()
1033                         drawHomeDirection()
1034                         drawHome()
1035                         drawFlightMode()
1036                         drawFailsafe()
1037                         drawFlightTime()
1038                         drawTxVoltage()
1039                         drawRSSI()
1040                 end
1041         end
1042         clock = clock + 1
1043 end
1044
1045 local function init()
1046         pushMessage(6,"Yaapu X7 script v1.0")
1047 end
1048
1049 --------------------------------------------------------------------------------
1050 -- SCRIPT END
1051 --------------------------------------------------------------------------------
1052 return {run=run,  background=background, init=init}