11local  INTERVAL           =  50          --  in 1/100th seconds
2- local  MSP_SET_RTC         =   246 
2+ local  MSP_API_VERSION    =   1 
33local  MSP_TX_INFO        =  186 
4+ local  MSP_SET_RTC        =  246 
45
56local  lastRunTS 
67local  sensorId  =  - 1 
8+ local  apiVersionReceived  =  false 
79local  timeIsSet  =  false 
8- local  mspMsgQueued  =  false 
910
1011local  function  getSensorValue ()
1112    if  sensorId  ==  - 1  then 
@@ -25,87 +26,87 @@ local function init()
2526    lastRunTS  =  0 
2627end 
2728
29+ local  function  processMspReply (cmd ,rx_buf )
30+     if  cmd  ==  nil  or  rx_buf  ==  nil  then 
31+         return 
32+     end 
33+     if  cmd  ==  MSP_API_VERSION  and  # (rx_buf ) >=  3  then 
34+         apiVersion  =  rx_buf [2 ] +  rx_buf [3 ] /  1000 
35+ 
36+         apiVersionReceived  =  true 
37+     end 
38+ end 
39+ 
2840local  function  run_bg ()
2941    --  run in intervals
30- 
3142    if  lastRunTS  ==  0  or  lastRunTS  +  INTERVAL  <  getTime () then 
32-         mspMsgQueued  =  false 
33-         --  ------------------------------------
34-         --  SYNC DATE AND TIME
35-         --  ------------------------------------
3643        local  sensorValue  =  getSensorValue ()
37- 
38-         if  not  timeIsSet  and  modelActive (sensorValue ) then 
39-             --  Send datetime when the telemetry connection is available
44+         if  modelActive (sensorValue ) then 
45+             --  Send data when the telemetry connection is available
4046            --  assuming when sensor value higher than 0 there is an telemetry connection
41-             --  only send datetime one time after telemetry connection became available
42-             --  or when connection is restored after e.g. lipo refresh
47+             if  not  apiVersionReceived  then 
48+                 protocol .mspRead (MSP_API_VERSION )
49+ 
50+                 processMspReply (mspPollReply ())
51+             elseif  apiVersionReceived  and  not  timeIsSet  then 
52+                 --  only send datetime one time after telemetry connection became available
53+                 --  or when connection is restored after e.g. lipo refresh
54+ 
55+                 if  apiVersion  >=  1.041  then 
56+                     --  format: seconds after the epoch (32) / milliseconds (16)
57+                     local  now  =  getRtcTime ()
58+ 
59+                     values  =  {}
60+ 
61+                     for  i  =  1 , 4  do 
62+                         values [i ] =  bit32.band (now , 0xFF )
63+                         now  =  bit32.rshift (now , 8 )
64+                     end 
65+ 
66+                     values [5 ] =  0  --  we don't have milliseconds
67+                     values [6 ] =  0 
68+                 else 
69+                     --  format: year (16) / month (8) / day (8) / hour (8) / min (8) / sec (8)
70+                     local  now  =  getDateTime ()
71+                     local  year  =  now .year ;
72+ 
73+                     values  =  {}
74+                     values [1 ] =  bit32.band (year , 0xFF )
75+                     year  =  bit32.rshift (year , 8 )
76+                     values [2 ] =  bit32.band (year , 0xFF )
77+                     values [3 ] =  now .mon 
78+                     values [4 ] =  now .day 
79+                     values [5 ] =  now .hour 
80+                     values [6 ] =  now .min 
81+                     values [7 ] =  now .sec 
82+                 end 
4383
44-             if  API_VERSION  <  1.041  then 
45-                 --  format: year (16) / month (8) / day (8) / hour (8) / min (8) / sec (8)
46-                 local  now  =  getDateTime ()
47-                 local  year  =  now .year ;
84+                 protocol .mspWrite (MSP_SET_RTC , values )
4885
49-                 values  =  {}
50-                 values [1 ] =  bit32.band (year , 0xFF )
51-                 year  =  bit32.rshift (year , 8 )
52-                 values [2 ] =  bit32.band (year , 0xFF )
53-                 values [3 ] =  now .mon 
54-                 values [4 ] =  now .day 
55-                 values [5 ] =  now .hour 
56-                 values [6 ] =  now .min 
57-                 values [7 ] =  now .sec 
86+                 timeIsSet  =  true 
5887            else 
59-                 --  format: seconds after the epoch (32) / milliseconds (16)
60-                 local  now  =  getRtcTime ()
88+                 local  rssi , alarm_low , alarm_crit  =  getRSSI ()
89+                 --  Scale the [0, 85] (empirical) RSSI values to [0, 255]
90+                 rssi  =  rssi  *  3 
91+                 if  rssi  >  255  then 
92+                     rssi  =  255 
93+                 end 
6194
6295                values  =  {}
96+                 values [1 ] =  rssi 
6397
64-                 for  i  =  1 , 4  do 
65-                     values [i ] =  bit32.band (now , 0xFF )
66-                     now  =  bit32.rshift (now , 8 )
67-                 end 
68- 
69-                 values [5 ] =  0  --  we don't have milliseconds
70-                 values [6 ] =  0 
98+                 protocol .mspWrite (MSP_TX_INFO , values )
7199            end 
72- 
73-             --  send msp message
74-             protocol .mspWrite (MSP_SET_RTC , values )
75-             mspMsgQueued  =  true 
76- 
77-             timeIsSet  =  true 
78-         elseif  not  modelActive (sensorValue ) then 
100+         else 
101+             apiVersionReceived  =  false 
79102            timeIsSet  =  false 
80103        end 
81104
82- 
83-         --  ------------------------------------
84-         --  SEND RSSI VALUE
85-         --  ------------------------------------
86- 
87-         if  mspMsgQueued  ==  false  then 
88-             local  rssi , alarm_low , alarm_crit  =  getRSSI ()
89-             --  Scale the [0, 85] (empirical) RSSI values to [0, 255]
90-             rssi  =  rssi  *  3 
91-             if  rssi  >  255  then 
92-                 rssi  =  255 
93-             end 
94- 
95-             values  =  {}
96-             values [1 ] =  rssi 
97- 
98-             --  send msp message
99-             protocol .mspWrite (MSP_TX_INFO , values )
100-             mspMsgQueued  =  true 
101-         end 
102- 
103105        lastRunTS  =  getTime ()
104106    end 
105107
106108    --  process queue
107109    mspProcessTxQ ()
108- 
109110end 
110111
111112return  { init = init , run_bg = run_bg  }
0 commit comments