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 #if defined(GPIO_PIN_PWM_OUTPUTS)
18 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";
21 static struct luaItem_selection luaSerialProtocol
= {
22 {"Protocol", CRSF_TEXT_SELECTION
},
24 "CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;MAVLink;DisplayPort",
28 #if defined(PLATFORM_ESP32)
29 static struct luaItem_selection luaSerial1Protocol
= {
30 {"Protocol2", CRSF_TEXT_SELECTION
},
32 "Off;CRSF;Inverted CRSF;SBUS;Inverted SBUS;SUMD;DJI RS Pro;HoTT Telemetry;Tramp;SmartAudio;DisplayPort",
37 static struct luaItem_selection luaSBUSFailsafeMode
= {
38 {"SBUS failsafe", CRSF_TEXT_SELECTION
},
44 static struct luaItem_int8 luaTargetSysId
= {
45 {"Target SysID", CRSF_UINT8
},
48 (uint8_t)1, // value - default to 1
55 static struct luaItem_int8 luaSourceSysId
= {
56 {"Source SysID", CRSF_UINT8
},
59 (uint8_t)255, // value - default to 255
67 #if defined(POWER_OUTPUT_VALUES)
68 static struct luaItem_selection luaTlmPower
= {
69 {"Tlm Power", CRSF_TEXT_SELECTION
},
76 #if defined(GPIO_PIN_ANT_CTRL)
77 static struct luaItem_selection luaAntennaMode
= {
78 {"Ant. Mode", CRSF_TEXT_SELECTION
},
80 "Antenna A;Antenna B;Diversity",
85 static struct luaItem_folder luaTeamraceFolder
= {
86 {"Team Race", CRSF_FOLDER
},
89 static struct luaItem_selection luaTeamraceChannel
= {
90 {"Channel", CRSF_TEXT_SELECTION
},
92 "AUX2;AUX3;AUX4;AUX5;AUX6;AUX7;AUX8;AUX9;AUX10;AUX11;AUX12",
96 static struct luaItem_selection luaTeamracePosition
= {
97 {"Position", CRSF_TEXT_SELECTION
},
99 "Disabled;1/Low;2;3;Mid;4;5;6/High",
103 //----------------------------Info-----------------------------------
105 static struct luaItem_string luaModelNumber
= {
106 {"Model Id", CRSF_INFO
},
110 static struct luaItem_string luaELRSversion
= {
111 {version
, CRSF_INFO
},
115 //----------------------------Info-----------------------------------
117 //---------------------------- WiFi -----------------------------
120 //---------------------------- WiFi -----------------------------
122 //---------------------------- Output Mapping -----------------------------
124 #if defined(GPIO_PIN_PWM_OUTPUTS)
125 static struct luaItem_folder luaMappingFolder
= {
126 {"Output Mapping", CRSF_FOLDER
},
129 static struct luaItem_int8 luaMappingChannelOut
= {
130 {"Output Ch", CRSF_UINT8
},
133 (uint8_t)5, // value - start on AUX1, value is 1-16, not zero-based
135 PWM_MAX_CHANNELS
, // max
141 static struct luaItem_int8 luaMappingChannelIn
= {
142 {"Input Ch", CRSF_UINT8
},
147 CRSF_NUM_CHANNELS
, // max
153 static struct luaItem_selection luaMappingOutputMode
= {
154 {"Output Mode", CRSF_TEXT_SELECTION
},
160 static struct luaItem_selection luaMappingInverted
= {
161 {"Invert", CRSF_TEXT_SELECTION
},
167 static struct luaItem_command luaSetFailsafe
= {
168 {"Set Failsafe Pos", CRSF_COMMAND
},
173 #endif // GPIO_PIN_PWM_OUTPUTS
175 //---------------------------- Output Mapping -----------------------------
177 static struct luaItem_selection luaBindStorage
= {
178 {"Bind Storage", CRSF_TEXT_SELECTION
},
180 "Persistent;Volatile;Returnable",
184 static struct luaItem_command luaBindMode
= {
185 {STR_EMPTYSPACE
, CRSF_COMMAND
},
190 #if defined(GPIO_PIN_PWM_OUTPUTS)
191 static void luaparamMappingChannelOut(struct luaPropertiesCommon
*item
, uint8_t arg
)
193 bool sclAssigned
= false;
194 bool sdaAssigned
= false;
195 #if defined(PLATFORM_ESP32)
196 bool serial1rxAssigned
= false;
197 bool serial1txAssigned
= false;
200 const char *no1Option
= ";";
201 const char *no2Options
= ";;";
202 const char *dshot
= ";DShot";
203 const char *serial_RX
= ";Serial RX";
204 const char *serial_TX
= ";Serial TX";
205 const char *i2c_SCL
= ";I2C SCL;";
206 const char *i2c_SDA
= ";;I2C SDA";
207 const char *i2c_BOTH
= ";I2C SCL;I2C SDA";
208 #if defined(PLATFORM_ESP32)
209 const char *serial1_RX
= ";Serial2 RX;";
210 const char *serial1_TX
= ";;Serial2 TX";
211 const char *serial1_BOTH
= ";Serial2 RX;Serial2 TX";
214 const char *pModeString
;
217 // find out if use once only modes have already been assigned
218 for (uint8_t ch
= 0; ch
< GPIO_PIN_PWM_OUTPUTS_COUNT
; ch
++)
223 eServoOutputMode mode
= (eServoOutputMode
)config
.GetPwmChannel(ch
)->val
.mode
;
231 #if defined(PLATFORM_ESP32)
232 if (mode
== somSerial1RX
)
233 serial1rxAssigned
= true;
235 if (mode
== somSerial1TX
)
236 serial1txAssigned
= true;
240 setLuaUint8Value(&luaMappingChannelOut
, arg
);
242 // When the selected output channel changes, update the available PWM modes for that pin
243 // Truncate the select options before the ; following On/Off
246 #if defined(PLATFORM_ESP32)
247 // DShot output (1 option)
249 // ESP8266 enum skips this, so it is never present
250 if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] != 0) // DShot doesn't work with GPIO0, exclude it
257 pModeString
= no1Option
;
259 strcat(pwmModes
, pModeString
);
261 // SerialIO outputs (1 option)
262 // ;[Serial RX] | [Serial TX]
263 if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == U0RXD_GPIO_NUM
)
265 pModeString
= serial_RX
;
267 else if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == U0TXD_GPIO_NUM
)
269 pModeString
= serial_TX
;
273 pModeString
= no1Option
;
275 strcat(pwmModes
, pModeString
);
277 // I2C pins (2 options)
278 // ;[I2C SCL] ;[I2C SDA]
279 if (GPIO_PIN_SCL
!= UNDEF_PIN
|| GPIO_PIN_SDA
!= UNDEF_PIN
)
281 // If the target defines SCL/SDA then those pins MUST be used
282 if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == GPIO_PIN_SCL
)
284 pModeString
= i2c_SCL
;
286 else if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == GPIO_PIN_SDA
)
288 pModeString
= i2c_SDA
;
292 pModeString
= no2Options
;
297 // otherwise allow any pin to be either SCL or SDA but only once
298 if (sclAssigned
&& !sdaAssigned
)
300 pModeString
= i2c_SDA
;
302 else if (sdaAssigned
&& !sclAssigned
)
304 pModeString
= i2c_SCL
;
306 else if (!sclAssigned
&& !sdaAssigned
)
308 pModeString
= i2c_BOTH
;
312 pModeString
= no2Options
;
315 strcat(pwmModes
, pModeString
);
317 // nothing to do for unsupported somPwm mode
318 strcat(pwmModes
, no1Option
);
320 #if defined(PLATFORM_ESP32)
321 // secondary Serial pins (2 options)
322 // ;[SERIAL2 RX] ;[SERIAL2_TX]
323 if (GPIO_PIN_SERIAL1_RX
!= UNDEF_PIN
|| GPIO_PIN_SERIAL1_TX
!= UNDEF_PIN
)
325 // If the target defines Serial2 RX/TX then those pins MUST be used
326 if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == GPIO_PIN_SERIAL1_RX
)
328 pModeString
= serial1_RX
;
330 else if (GPIO_PIN_PWM_OUTPUTS
[arg
-1] == GPIO_PIN_SERIAL1_TX
)
332 pModeString
= serial1_TX
;
336 pModeString
= no2Options
;
340 { // otherwise allow any pin to be either RX or TX but only once
341 if (serial1txAssigned
&& !serial1rxAssigned
)
343 pModeString
= serial1_RX
;
345 else if (serial1rxAssigned
&& !serial1txAssigned
)
347 pModeString
= serial1_TX
;
350 else if (!serial1rxAssigned
&& !serial1txAssigned
)
352 pModeString
= serial1_BOTH
;
356 pModeString
= no2Options
;
359 strcat(pwmModes
, pModeString
);
362 // trim off trailing semicolons (assumes pwmModes has at least 1 non-semicolon)
363 for (auto lastPos
= strlen(pwmModes
)-1; pwmModes
[lastPos
] == ';'; lastPos
--)
365 pwmModes
[lastPos
] = '\0';
368 // Trigger an event to update the related fields to represent the selected channel
369 devicesTriggerEvent();
372 static void luaparamMappingChannelIn(struct luaPropertiesCommon
*item
, uint8_t arg
)
374 const uint8_t ch
= luaMappingChannelOut
.properties
.u
.value
- 1;
375 rx_config_pwm_t newPwmCh
;
376 newPwmCh
.raw
= config
.GetPwmChannel(ch
)->raw
;
377 newPwmCh
.val
.inputChannel
= arg
- 1; // convert 1-16 -> 0-15
379 config
.SetPwmChannelRaw(ch
, newPwmCh
.raw
);
382 static void configureSerialPin(uint8_t sibling
, uint8_t oldMode
, uint8_t newMode
)
384 for (int ch
=0 ; ch
<GPIO_PIN_PWM_OUTPUTS_COUNT
; ch
++)
386 if (GPIO_PIN_PWM_OUTPUTS
[ch
] == sibling
)
388 // Retain as much of the sibling's current config as possible
389 rx_config_pwm_t siblingPinConfig
;
390 siblingPinConfig
.raw
= config
.GetPwmChannel(ch
)->raw
;
392 // If the new mode is serial, the sibling is also forced to serial
393 if (newMode
== somSerial
)
395 siblingPinConfig
.val
.mode
= somSerial
;
397 // If the new mode is not serial, and the sibling is serial, set the sibling to PWM (50Hz)
398 else if (siblingPinConfig
.val
.mode
== somSerial
)
400 siblingPinConfig
.val
.mode
= som50Hz
;
403 config
.SetPwmChannelRaw(ch
, siblingPinConfig
.raw
);
408 if (oldMode
!= newMode
)
410 deferExecutionMillis(100, [](){
416 static void luaparamMappingOutputMode(struct luaPropertiesCommon
*item
, uint8_t arg
)
419 const uint8_t ch
= luaMappingChannelOut
.properties
.u
.value
- 1;
420 rx_config_pwm_t newPwmCh
;
421 newPwmCh
.raw
= config
.GetPwmChannel(ch
)->raw
;
422 uint8_t oldMode
= newPwmCh
.val
.mode
;
423 newPwmCh
.val
.mode
= arg
;
425 // Check if pin == 1/3 and do other pin adjustment accordingly
426 if (GPIO_PIN_PWM_OUTPUTS
[ch
] == 1)
428 configureSerialPin(3, oldMode
, newPwmCh
.val
.mode
);
430 else if (GPIO_PIN_PWM_OUTPUTS
[ch
] == 3)
432 configureSerialPin(1, oldMode
, newPwmCh
.val
.mode
);
434 config
.SetPwmChannelRaw(ch
, newPwmCh
.raw
);
437 static void luaparamMappingInverted(struct luaPropertiesCommon
*item
, uint8_t arg
)
440 const uint8_t ch
= luaMappingChannelOut
.properties
.u
.value
- 1;
441 rx_config_pwm_t newPwmCh
;
442 newPwmCh
.raw
= config
.GetPwmChannel(ch
)->raw
;
443 newPwmCh
.val
.inverted
= arg
;
445 config
.SetPwmChannelRaw(ch
, newPwmCh
.raw
);
448 static void luaparamSetFailsafe(struct luaPropertiesCommon
*item
, uint8_t arg
)
450 luaCmdStep_e newStep
;
454 newStep
= lcsAskConfirm
;
455 msg
= "Set failsafe to curr?";
457 else if (arg
== lcsConfirmed
)
459 // This is generally not seen by the user, since we'll disconnect to commit config
460 // and the handset will send another lcdQuery that will overwrite it with idle
461 newStep
= lcsExecuting
;
462 msg
= "Setting failsafe";
464 for (int ch
=0; ch
<GPIO_PIN_PWM_OUTPUTS_COUNT
; ++ch
)
466 rx_config_pwm_t newPwmCh
;
467 // The value must fit into the 10 bit range of the failsafe
468 newPwmCh
.raw
= config
.GetPwmChannel(ch
)->raw
;
469 newPwmCh
.val
.failsafe
= CRSF_to_UINT10(constrain(ChannelData
[config
.GetPwmChannel(ch
)->val
.inputChannel
], CRSF_CHANNEL_VALUE_MIN
, CRSF_CHANNEL_VALUE_MAX
));
470 //DBGLN("FSCH(%u) crsf=%u us=%u", ch, ChannelData[ch], newPwmCh.val.failsafe+988U);
471 config
.SetPwmChannelRaw(ch
, newPwmCh
.raw
);
477 msg
= STR_EMPTYSPACE
;
480 sendLuaCommandResponse((struct luaItem_command
*)item
, newStep
, msg
);
483 #endif // GPIO_PIN_PWM_OUTPUTS
485 #if defined(POWER_OUTPUT_VALUES)
487 static void luaparamSetPower(struct luaPropertiesCommon
* item
, uint8_t arg
)
490 uint8_t newPower
= arg
+ POWERMGNT::getMinPower();
491 if (newPower
> POWERMGNT::getMaxPower())
493 newPower
= PWR_MATCH_TX
;
496 config
.SetPower(newPower
);
497 // POWERMGNT::setPower() will be called in updatePower() in the main loop
500 #endif // POWER_OUTPUT_VALUES
502 static void registerLuaParameters()
504 registerLUAParameter(&luaSerialProtocol
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
505 config
.SetSerialProtocol((eSerialProtocol
)arg
);
506 if (config
.IsModified()) {
507 deferExecutionMillis(100, [](){
513 #if defined(PLATFORM_ESP32)
516 registerLUAParameter(&luaSerial1Protocol
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
517 config
.SetSerial1Protocol((eSerial1Protocol
)arg
);
518 if (config
.IsModified()) {
519 deferExecutionMillis(100, [](){
520 reconfigureSerial1();
527 registerLUAParameter(&luaSBUSFailsafeMode
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
528 config
.SetFailsafeMode((eFailsafeMode
)arg
);
531 registerLUAParameter(&luaTargetSysId
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
532 config
.SetTargetSysId((uint8_t)arg
);
534 registerLUAParameter(&luaSourceSysId
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
535 config
.SetSourceSysId((uint8_t)arg
);
538 if (GPIO_PIN_ANT_CTRL
!= UNDEF_PIN
)
540 registerLUAParameter(&luaAntennaMode
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
541 config
.SetAntennaMode(arg
);
545 #if defined(POWER_OUTPUT_VALUES)
546 luadevGeneratePowerOpts(&luaTlmPower
);
547 registerLUAParameter(&luaTlmPower
, &luaparamSetPower
);
551 registerLUAParameter(&luaTeamraceFolder
);
552 registerLUAParameter(&luaTeamraceChannel
, [](struct luaPropertiesCommon
* item
, uint8_t arg
) {
553 config
.SetTeamraceChannel(arg
+ AUX2
);
554 }, luaTeamraceFolder
.common
.id
);
555 registerLUAParameter(&luaTeamracePosition
, [](struct luaPropertiesCommon
* item
, uint8_t arg
) {
556 config
.SetTeamracePosition(arg
);
557 }, luaTeamraceFolder
.common
.id
);
559 #if defined(GPIO_PIN_PWM_OUTPUTS)
560 if (OPT_HAS_SERVO_OUTPUT
)
562 luaparamMappingChannelOut(&luaMappingOutputMode
.common
, luaMappingChannelOut
.properties
.u
.value
);
563 registerLUAParameter(&luaMappingFolder
);
564 registerLUAParameter(&luaMappingChannelOut
, &luaparamMappingChannelOut
, luaMappingFolder
.common
.id
);
565 registerLUAParameter(&luaMappingChannelIn
, &luaparamMappingChannelIn
, luaMappingFolder
.common
.id
);
566 registerLUAParameter(&luaMappingOutputMode
, &luaparamMappingOutputMode
, luaMappingFolder
.common
.id
);
567 registerLUAParameter(&luaMappingInverted
, &luaparamMappingInverted
, luaMappingFolder
.common
.id
);
568 registerLUAParameter(&luaSetFailsafe
, &luaparamSetFailsafe
);
572 registerLUAParameter(&luaBindStorage
, [](struct luaPropertiesCommon
* item
, uint8_t arg
) {
573 config
.SetBindStorage((rx_config_bindstorage_t
)arg
);
575 registerLUAParameter(&luaBindMode
, [](struct luaPropertiesCommon
* item
, uint8_t arg
){
576 // Complete when TX polls for status i.e. going back to idle, because we're going to lose connection
577 if (arg
== lcsQuery
) {
578 deferExecutionMillis(200, EnterBindingModeSafely
);
580 sendLuaCommandResponse(&luaBindMode
, arg
< 5 ? lcsExecuting
: lcsIdle
, arg
< 5 ? "Entering..." : "");
583 registerLUAParameter(&luaModelNumber
);
584 registerLUAParameter(&luaELRSversion
);
585 registerLUAParameter(nullptr);
588 static void updateBindModeLabel()
590 if (config
.IsOnLoan())
591 luaBindMode
.common
.name
= "Return Model";
593 luaBindMode
.common
.name
= "Enter Bind Mode";
598 setLuaTextSelectionValue(&luaSerialProtocol
, config
.GetSerialProtocol());
599 #if defined(PLATFORM_ESP32)
602 setLuaTextSelectionValue(&luaSerial1Protocol
, config
.GetSerial1Protocol());
606 setLuaTextSelectionValue(&luaSBUSFailsafeMode
, config
.GetFailsafeMode());
608 if (GPIO_PIN_ANT_CTRL
!= UNDEF_PIN
)
610 setLuaTextSelectionValue(&luaAntennaMode
, config
.GetAntennaMode());
613 #if defined(POWER_OUTPUT_VALUES)
614 // The last item (for MatchTX) will be MaxPower - MinPower + 1
615 uint8_t luaPwrVal
= (config
.GetPower() == PWR_MATCH_TX
) ? POWERMGNT::getMaxPower() + 1 : config
.GetPower();
616 setLuaTextSelectionValue(&luaTlmPower
, luaPwrVal
- POWERMGNT::getMinPower());
620 setLuaTextSelectionValue(&luaTeamraceChannel
, config
.GetTeamraceChannel() - AUX2
);
621 setLuaTextSelectionValue(&luaTeamracePosition
, config
.GetTeamracePosition());
623 #if defined(GPIO_PIN_PWM_OUTPUTS)
624 if (OPT_HAS_SERVO_OUTPUT
)
626 const rx_config_pwm_t
*pwmCh
= config
.GetPwmChannel(luaMappingChannelOut
.properties
.u
.value
- 1);
627 setLuaUint8Value(&luaMappingChannelIn
, pwmCh
->val
.inputChannel
+ 1);
628 setLuaTextSelectionValue(&luaMappingOutputMode
, pwmCh
->val
.mode
);
629 setLuaTextSelectionValue(&luaMappingInverted
, pwmCh
->val
.inverted
);
633 if (config
.GetModelId() == 255)
635 setLuaStringValue(&luaModelNumber
, "Off");
639 itoa(config
.GetModelId(), modelString
, 10);
640 setLuaStringValue(&luaModelNumber
, modelString
);
642 setLuaTextSelectionValue(&luaBindStorage
, config
.GetBindStorage());
643 updateBindModeLabel();
645 if (config
.GetSerialProtocol() == PROTOCOL_MAVLINK
)
647 setLuaUint8Value(&luaSourceSysId
, config
.GetSourceSysId() == 0 ? 255 : config
.GetSourceSysId()); //display Source sysID if 0 display 255 to mimic logic in SerialMavlink.cpp
648 setLuaUint8Value(&luaTargetSysId
, config
.GetTargetSysId() == 0 ? 1 : config
.GetTargetSysId()); //display Target sysID if 0 display 1 to mimic logic in SerialMavlink.cpp
649 LUA_FIELD_SHOW(luaSourceSysId
)
650 LUA_FIELD_SHOW(luaTargetSysId
)
654 LUA_FIELD_HIDE(luaSourceSysId
)
655 LUA_FIELD_HIDE(luaTargetSysId
)
658 return DURATION_IMMEDIATELY
;
663 luaHandleUpdateParameter();
664 // Receivers can only `UpdateParamReq == true` every 4th packet due to the transmitter cadence in 1:2
665 // Channels, Downlink Telemetry Slot, Uplink Telemetry (the write command), Downlink Telemetry Slot...
666 // (interval * 4 / 1000) or 1 second if not connected
667 return (connectionState
== connected
) ? ExpressLRS_currAirRate_Modparams
->interval
/ 250 : 1000;
672 registerLuaParameters();
674 return DURATION_IMMEDIATELY
;
677 device_t LUA_device
= {
678 .initialize
= nullptr,