3 #include "rxtx_devLua.h"
5 #include "devServoOutput.h"
8 #define RX_HAS_SERIAL1 (GPIO_PIN_SERIAL1_TX != UNDEF_PIN || OPT_HAS_SERVO_OUTPUT)
10 extern void reconfigureSerial();
11 #if defined(PLATFORM_ESP32)
12 extern void reconfigureSerial1();
14 extern bool BindingModeRequest
;
16 static char modelString
[] = "000";
17 static char pwmModes
[] = "50Hz;60Hz;100Hz;160Hz;333Hz;400Hz;10kHzDuty;On/Off;DShot;Serial RX;Serial TX;I2C SCL;I2C SDA;Serial2 RX;Serial2 TX";
19 static struct luaItem_selection luaSerialProtocol
= {
20 {"Protocol", CRSF_TEXT_SELECTION
},
22 "CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;MAVLink;DisplayPort",
26 #if defined(PLATFORM_ESP32)
27 static struct luaItem_selection luaSerial1Protocol
= {
28 {"Protocol2", CRSF_TEXT_SELECTION
},
30 "Off;CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;Tramp;SmartAudio;DisplayPort",
35 static struct luaItem_selection luaSBUSFailsafeMode
= {
36 {"SBUS failsafe", CRSF_TEXT_SELECTION
},
42 static struct luaItem_int8 luaTargetSysId
= {
43 {"Target SysID", CRSF_UINT8
},
46 (uint8_t)1, // value - default to 1
53 static struct luaItem_int8 luaSourceSysId
= {
54 {"Source SysID", CRSF_UINT8
},
57 (uint8_t)255, // value - default to 255
65 static struct luaItem_selection luaTlmPower
= {
66 {"Tlm Power", CRSF_TEXT_SELECTION
},
72 static struct luaItem_selection luaAntennaMode
= {
73 {"Ant. Mode", CRSF_TEXT_SELECTION
},
75 "Antenna A;Antenna B;Diversity",
79 static struct luaItem_folder luaTeamraceFolder
= {
80 {"Team Race", CRSF_FOLDER
},
83 static struct luaItem_selection luaTeamraceChannel
= {
84 {"Channel", CRSF_TEXT_SELECTION
},
86 "AUX2;AUX3;AUX4;AUX5;AUX6;AUX7;AUX8;AUX9;AUX10;AUX11;AUX12",
90 static struct luaItem_selection luaTeamracePosition
= {
91 {"Position", CRSF_TEXT_SELECTION
},
93 "Disabled;1/Low;2;3;Mid;4;5;6/High",
97 //----------------------------Info-----------------------------------
99 static struct luaItem_string luaModelNumber
= {
100 {"Model Id", CRSF_INFO
},
104 static struct luaItem_string luaELRSversion
= {
105 {version
, CRSF_INFO
},
109 //----------------------------Info-----------------------------------
111 //---------------------------- WiFi -----------------------------
114 //---------------------------- WiFi -----------------------------
116 //---------------------------- Output Mapping -----------------------------
118 static struct luaItem_folder luaMappingFolder
= {
119 {"Output Mapping", CRSF_FOLDER
},
122 static struct luaItem_int8 luaMappingChannelOut
= {
123 {"Output Ch", CRSF_UINT8
},
126 (uint8_t)5, // value - start on AUX1, value is 1-16, not zero-based
128 PWM_MAX_CHANNELS
, // max
134 static struct luaItem_int8 luaMappingChannelIn
= {
135 {"Input Ch", CRSF_UINT8
},
140 CRSF_NUM_CHANNELS
, // max
146 static struct luaItem_selection luaMappingOutputMode
= {
147 {"Output Mode", CRSF_TEXT_SELECTION
},
153 static struct luaItem_selection luaMappingInverted
= {
154 {"Invert", CRSF_TEXT_SELECTION
},
160 static struct luaItem_command luaSetFailsafe
= {
161 {"Set Failsafe Pos", CRSF_COMMAND
},
166 //---------------------------- Output Mapping -----------------------------
168 static struct luaItem_selection luaBindStorage
= {
169 {"Bind Storage", CRSF_TEXT_SELECTION
},
171 "Persistent;Volatile;Returnable",
175 static struct luaItem_command luaBindMode
= {
176 {STR_EMPTYSPACE
, CRSF_COMMAND
},
181 static void luaparamMappingChannelOut(struct luaPropertiesCommon
*item
, uint8_t arg
)
183 bool sclAssigned
= false;
184 bool sdaAssigned
= false;
185 #if defined(PLATFORM_ESP32)
186 bool serial1rxAssigned
= false;
187 bool serial1txAssigned
= false;
190 const char *no1Option
= ";";
191 const char *no2Options
= ";;";
192 const char *dshot
= ";DShot";
193 const char *serial_RX
= ";Serial RX";
194 const char *serial_TX
= ";Serial TX";
195 const char *i2c_SCL
= ";I2C SCL;";
196 const char *i2c_SDA
= ";;I2C SDA";
197 const char *i2c_BOTH
= ";I2C SCL;I2C SDA";
198 #if defined(PLATFORM_ESP32)
199 const char *serial1_RX
= ";Serial2 RX;";
200 const char *serial1_TX
= ";;Serial2 TX";
201 const char *serial1_BOTH
= ";Serial2 RX;Serial2 TX";
204 const char *pModeString
;
207 // find out if use once only modes have already been assigned
208 for (uint8_t ch
= 0; ch
< GPIO_PIN_PWM_OUTPUTS_COUNT
; ch
++)
213 eServoOutputMode mode
= (eServoOutputMode
)config
.GetPwmChannel(ch
)->val
.mode
;
221 #if defined(PLATFORM_ESP32)
222 if (mode
== somSerial1RX
)
223 serial1rxAssigned
= true;
225 if (mode
== somSerial1TX
)
226 serial1txAssigned
= true;
230 setLuaUint8Value(&luaMappingChannelOut
, arg
);
232 // When the selected output channel changes, update the available PWM modes for that pin
233 // Truncate the select options before the ; following On/Off
236 #if defined(PLATFORM_ESP32)
237 // DShot output (1 option)
239 // ESP8266 enum skips this, so it is never present
240 if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] != 0) // DShot doesn't work with GPIO0, exclude it
247 pModeString
= no1Option
;
249 strcat(pwmModes
, pModeString
);
251 // SerialIO outputs (1 option)
252 // ;[Serial RX] | [Serial TX]
253 if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == U0RXD_GPIO_NUM
)
255 pModeString
= serial_RX
;
257 else if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == U0TXD_GPIO_NUM
)
259 pModeString
= serial_TX
;
263 pModeString
= no1Option
;
265 strcat(pwmModes
, pModeString
);
267 // I2C pins (2 options)
268 // ;[I2C SCL] ;[I2C SDA]
269 if (GPIO_PIN_SCL
!= UNDEF_PIN
|| GPIO_PIN_SDA
!= UNDEF_PIN
)
271 // If the target defines SCL/SDA then those pins MUST be used
272 if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == GPIO_PIN_SCL
)
274 pModeString
= i2c_SCL
;
276 else if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == GPIO_PIN_SDA
)
278 pModeString
= i2c_SDA
;
282 pModeString
= no2Options
;
287 // otherwise allow any pin to be either SCL or SDA but only once
288 if (sclAssigned
&& !sdaAssigned
)
290 pModeString
= i2c_SDA
;
292 else if (sdaAssigned
&& !sclAssigned
)
294 pModeString
= i2c_SCL
;
296 else if (!sclAssigned
&& !sdaAssigned
)
298 pModeString
= i2c_BOTH
;
302 pModeString
= no2Options
;
305 strcat(pwmModes
, pModeString
);
307 // nothing to do for unsupported somPwm mode
308 strcat(pwmModes
, no1Option
);
310 #if defined(PLATFORM_ESP32)
311 // secondary Serial pins (2 options)
312 // ;[SERIAL2 RX] ;[SERIAL2_TX]
313 if (GPIO_PIN_SERIAL1_RX
!= UNDEF_PIN
|| GPIO_PIN_SERIAL1_TX
!= UNDEF_PIN
)
315 // If the target defines Serial2 RX/TX then those pins MUST be used
316 if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == GPIO_PIN_SERIAL1_RX
)
318 pModeString
= serial1_RX
;
320 else if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == GPIO_PIN_SERIAL1_TX
)
322 pModeString
= serial1_TX
;
326 pModeString
= no2Options
;
330 { // otherwise allow any pin to be either RX or TX but only once
331 if (serial1txAssigned
&& !serial1rxAssigned
)
333 pModeString
= serial1_RX
;
335 else if (serial1rxAssigned
&& !serial1txAssigned
)
337 pModeString
= serial1_TX
;
340 else if (!serial1rxAssigned
&& !serial1txAssigned
)
342 pModeString
= serial1_BOTH
;
346 pModeString
= no2Options
;
349 strcat(pwmModes
, pModeString
);
352 // trim off trailing semicolons (assumes pwmModes has at least 1 non-semicolon)
353 for (auto lastPos
= strlen(pwmModes
)-1; pwmModes
[lastPos
] == ';'; lastPos
--)
355 pwmModes
[lastPos
] = '\0';
358 // Trigger an event to update the related fields to represent the selected channel
359 devicesTriggerEvent();
362 static void luaparamMappingChannelIn(struct luaPropertiesCommon
*item
, uint8_t arg
)
364 const uint8_t ch
= luaMappingChannelOut
.properties
.u
.value
- 1;
365 rx_config_pwm_t newPwmCh
;
366 newPwmCh
.raw
= config
.GetPwmChannel(ch
)->raw
;
367 newPwmCh
.val
.inputChannel
= arg
- 1; // convert 1-16 -> 0-15
369 config
.SetPwmChannelRaw(ch
, newPwmCh
.raw
);
372 static void configureSerialPin(uint8_t sibling
, uint8_t oldMode
, uint8_t newMode
)
374 for (int ch
=0 ; ch
<GPIO_PIN_PWM_OUTPUTS_COUNT
; ch
++)
376 if (GPIO_PIN_PWM_OUTPUTS
[ch
] == sibling
)
378 // Retain as much of the sibling's current config as possible
379 rx_config_pwm_t siblingPinConfig
;
380 siblingPinConfig
.raw
= config
.GetPwmChannel(ch
)->raw
;
382 // If the new mode is serial, the sibling is also forced to serial
383 if (newMode
== somSerial
)
385 siblingPinConfig
.val
.mode
= somSerial
;
387 // If the new mode is not serial, and the sibling is serial, set the sibling to PWM (50Hz)
388 else if (siblingPinConfig
.val
.mode
== somSerial
)
390 siblingPinConfig
.val
.mode
= som50Hz
;
393 config
.SetPwmChannelRaw(ch
, siblingPinConfig
.raw
);
398 if (oldMode
!= newMode
)
400 deferExecutionMillis(100, [](){
406 static void luaparamMappingOutputMode(struct luaPropertiesCommon
*item
, uint8_t arg
)
409 const uint8_t ch
= luaMappingChannelOut
.properties
.u
.value
- 1;
410 rx_config_pwm_t newPwmCh
;
411 newPwmCh
.raw
= config
.GetPwmChannel(ch
)->raw
;
412 uint8_t oldMode
= newPwmCh
.val
.mode
;
413 newPwmCh
.val
.mode
= arg
;
415 // Check if pin == 1/3 and do other pin adjustment accordingly
416 if (GPIO_PIN_PWM_OUTPUTS
[ch
] == 1)
418 configureSerialPin(3, oldMode
, newPwmCh
.val
.mode
);
420 else if (GPIO_PIN_PWM_OUTPUTS
[ch
] == 3)
422 configureSerialPin(1, oldMode
, newPwmCh
.val
.mode
);
424 config
.SetPwmChannelRaw(ch
, newPwmCh
.raw
);
427 static void luaparamMappingInverted(struct luaPropertiesCommon
*item
, uint8_t arg
)
430 const uint8_t ch
= luaMappingChannelOut
.properties
.u
.value
- 1;
431 rx_config_pwm_t newPwmCh
;
432 newPwmCh
.raw
= config
.GetPwmChannel(ch
)->raw
;
433 newPwmCh
.val
.inverted
= arg
;
435 config
.SetPwmChannelRaw(ch
, newPwmCh
.raw
);
438 static void luaparamSetFailsafe(struct luaPropertiesCommon
*item
, uint8_t arg
)
440 luaCmdStep_e newStep
;
444 newStep
= lcsAskConfirm
;
445 msg
= "Set failsafe to curr?";
447 else if (arg
== lcsConfirmed
)
449 // This is generally not seen by the user, since we'll disconnect to commit config
450 // and the handset will send another lcdQuery that will overwrite it with idle
451 newStep
= lcsExecuting
;
452 msg
= "Setting failsafe";
454 for (int ch
=0; ch
<GPIO_PIN_PWM_OUTPUTS_COUNT
; ++ch
)
456 rx_config_pwm_t newPwmCh
;
457 // The value must fit into the 10 bit range of the failsafe
458 newPwmCh
.raw
= config
.GetPwmChannel(ch
)->raw
;
459 newPwmCh
.val
.failsafe
= CRSF_to_UINT10(constrain(ChannelData
[config
.GetPwmChannel(ch
)->val
.inputChannel
], CRSF_CHANNEL_VALUE_MIN
, CRSF_CHANNEL_VALUE_MAX
));
460 //DBGLN("FSCH(%u) crsf=%u us=%u", ch, ChannelData[ch], newPwmCh.val.failsafe+988U);
461 config
.SetPwmChannelRaw(ch
, newPwmCh
.raw
);
467 msg
= STR_EMPTYSPACE
;
470 sendLuaCommandResponse((struct luaItem_command
*)item
, newStep
, msg
);
473 static void luaparamSetPower(struct luaPropertiesCommon
* item
, uint8_t arg
)
476 uint8_t newPower
= arg
+ POWERMGNT::getMinPower();
477 if (newPower
> POWERMGNT::getMaxPower())
479 newPower
= PWR_MATCH_TX
;
482 config
.SetPower(newPower
);
483 // POWERMGNT::setPower() will be called in updatePower() in the main loop
486 static void registerLuaParameters()
488 registerLUAParameter(&luaSerialProtocol
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
489 config
.SetSerialProtocol((eSerialProtocol
)arg
);
490 if (config
.IsModified()) {
491 deferExecutionMillis(100, [](){
497 #if defined(PLATFORM_ESP32)
500 registerLUAParameter(&luaSerial1Protocol
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
501 config
.SetSerial1Protocol((eSerial1Protocol
)arg
);
502 if (config
.IsModified()) {
503 deferExecutionMillis(100, [](){
504 reconfigureSerial1();
511 registerLUAParameter(&luaSBUSFailsafeMode
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
512 config
.SetFailsafeMode((eFailsafeMode
)arg
);
515 registerLUAParameter(&luaTargetSysId
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
516 config
.SetTargetSysId((uint8_t)arg
);
518 registerLUAParameter(&luaSourceSysId
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
519 config
.SetSourceSysId((uint8_t)arg
);
522 if (GPIO_PIN_ANT_CTRL
!= UNDEF_PIN
)
524 registerLUAParameter(&luaAntennaMode
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
525 config
.SetAntennaMode(arg
);
529 if (MinPower
!= MaxPower
)
531 luadevGeneratePowerOpts(&luaTlmPower
);
532 registerLUAParameter(&luaTlmPower
, &luaparamSetPower
);
536 registerLUAParameter(&luaTeamraceFolder
);
537 registerLUAParameter(&luaTeamraceChannel
, [](struct luaPropertiesCommon
* item
, uint8_t arg
) {
538 config
.SetTeamraceChannel(arg
+ AUX2
);
539 }, luaTeamraceFolder
.common
.id
);
540 registerLUAParameter(&luaTeamracePosition
, [](struct luaPropertiesCommon
* item
, uint8_t arg
) {
541 config
.SetTeamracePosition(arg
);
542 }, luaTeamraceFolder
.common
.id
);
544 if (OPT_HAS_SERVO_OUTPUT
)
546 luaparamMappingChannelOut(&luaMappingOutputMode
.common
, luaMappingChannelOut
.properties
.u
.value
);
547 registerLUAParameter(&luaMappingFolder
);
548 registerLUAParameter(&luaMappingChannelOut
, &luaparamMappingChannelOut
, luaMappingFolder
.common
.id
);
549 registerLUAParameter(&luaMappingChannelIn
, &luaparamMappingChannelIn
, luaMappingFolder
.common
.id
);
550 registerLUAParameter(&luaMappingOutputMode
, &luaparamMappingOutputMode
, luaMappingFolder
.common
.id
);
551 registerLUAParameter(&luaMappingInverted
, &luaparamMappingInverted
, luaMappingFolder
.common
.id
);
552 registerLUAParameter(&luaSetFailsafe
, &luaparamSetFailsafe
);
555 registerLUAParameter(&luaBindStorage
, [](struct luaPropertiesCommon
* item
, uint8_t arg
) {
556 config
.SetBindStorage((rx_config_bindstorage_t
)arg
);
558 registerLUAParameter(&luaBindMode
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
559 // Complete when TX polls for status i.e. going back to idle, because we're going to lose connection
560 if (arg
== lcsQuery
) {
561 deferExecutionMillis(200, EnterBindingModeSafely
);
563 sendLuaCommandResponse(&luaBindMode
, arg
< 5 ? lcsExecuting
: lcsIdle
, arg
< 5 ? "Entering..." : "");
566 registerLUAParameter(&luaModelNumber
);
567 registerLUAParameter(&luaELRSversion
);
568 registerLUAParameter(nullptr);
571 static void updateBindModeLabel()
573 if (config
.IsOnLoan())
574 luaBindMode
.common
.name
= "Return Model";
576 luaBindMode
.common
.name
= "Enter Bind Mode";
581 setLuaTextSelectionValue(&luaSerialProtocol
, config
.GetSerialProtocol());
582 #if defined(PLATFORM_ESP32)
585 setLuaTextSelectionValue(&luaSerial1Protocol
, config
.GetSerial1Protocol());
589 setLuaTextSelectionValue(&luaSBUSFailsafeMode
, config
.GetFailsafeMode());
591 if (GPIO_PIN_ANT_CTRL
!= UNDEF_PIN
)
593 setLuaTextSelectionValue(&luaAntennaMode
, config
.GetAntennaMode());
596 if (MinPower
!= MaxPower
)
598 // The last item (for MatchTX) will be MaxPower - MinPower + 1
599 uint8_t luaPwrVal
= (config
.GetPower() == PWR_MATCH_TX
) ? POWERMGNT::getMaxPower() + 1 : config
.GetPower();
600 setLuaTextSelectionValue(&luaTlmPower
, luaPwrVal
- POWERMGNT::getMinPower());
604 setLuaTextSelectionValue(&luaTeamraceChannel
, config
.GetTeamraceChannel() - AUX2
);
605 setLuaTextSelectionValue(&luaTeamracePosition
, config
.GetTeamracePosition());
607 if (OPT_HAS_SERVO_OUTPUT
)
609 const rx_config_pwm_t
*pwmCh
= config
.GetPwmChannel(luaMappingChannelOut
.properties
.u
.value
- 1);
610 setLuaUint8Value(&luaMappingChannelIn
, pwmCh
->val
.inputChannel
+ 1);
611 setLuaTextSelectionValue(&luaMappingOutputMode
, pwmCh
->val
.mode
);
612 setLuaTextSelectionValue(&luaMappingInverted
, pwmCh
->val
.inverted
);
615 if (config
.GetModelId() == 255)
617 setLuaStringValue(&luaModelNumber
, "Off");
621 itoa(config
.GetModelId(), modelString
, 10);
622 setLuaStringValue(&luaModelNumber
, modelString
);
624 setLuaTextSelectionValue(&luaBindStorage
, config
.GetBindStorage());
625 updateBindModeLabel();
627 if (config
.GetSerialProtocol() == PROTOCOL_MAVLINK
)
629 setLuaUint8Value(&luaSourceSysId
, config
.GetSourceSysId() == 0 ? 255 : config
.GetSourceSysId()); //display Source sysID if 0 display 255 to mimic logic in SerialMavlink.cpp
630 setLuaUint8Value(&luaTargetSysId
, config
.GetTargetSysId() == 0 ? 1 : config
.GetTargetSysId()); //display Target sysID if 0 display 1 to mimic logic in SerialMavlink.cpp
631 LUA_FIELD_SHOW(luaSourceSysId
)
632 LUA_FIELD_SHOW(luaTargetSysId
)
636 LUA_FIELD_HIDE(luaSourceSysId
)
637 LUA_FIELD_HIDE(luaTargetSysId
)
640 return DURATION_IMMEDIATELY
;
645 luaHandleUpdateParameter();
646 // Receivers can only `UpdateParamReq == true` every 4th packet due to the transmitter cadence in 1:2
647 // Channels, Downlink Telemetry Slot, Uplink Telemetry (the write command), Downlink Telemetry Slot...
648 // (interval * 4 / 1000) or 1 second if not connected
649 return (connectionState
== connected
) ? ExpressLRS_currAirRate_Modparams
->interval
/ 250 : 1000;
654 registerLuaParameters();
656 return DURATION_IMMEDIATELY
;
659 device_t LUA_device
= {
660 .initialize
= nullptr,