diff --git a/Makefile b/Makefile
index 5d4c39945f..27c5cb5e64 100644
--- a/Makefile
+++ b/Makefile
@@ -711,6 +711,7 @@ COMMON_SRC = \
io/serial.c \
io/statusindicator.c \
io/transponder_ir.c \
+ io/rcsplit.c \
msp/msp_serial.c \
scheduler/scheduler.c \
sensors/battery.c \
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index b3794a5b79..9082e0dde7 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -124,6 +124,7 @@
#include "flight/pid.h"
#include "flight/servos.h"
+#include "io/rcsplit.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
@@ -636,5 +637,10 @@ void init(void)
#else
fcTasksInit();
#endif
+
+#ifdef USE_RCSPLIT
+ rcSplitInit();
+#endif // USE_RCSPLIT
+
systemState |= SYSTEM_STATE_READY;
}
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 28f6ea4258..ce8a74520d 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -153,6 +153,9 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
+ { BOXCAMERA1, "CAMERA CONTROL 1", 32},
+ { BOXCAMERA2, "CAMERA CONTROL 2", 33},
+ { BOXCAMERA3, "CAMERA CONTROL 3", 34 },
};
// mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -420,6 +423,12 @@ void initActiveBoxIds(void)
}
#endif
+#ifdef USE_RCSPLIT
+ BME(BOXCAMERA1);
+ BME(BOXCAMERA2);
+ BME(BOXCAMERA3);
+#endif
+
#undef BME
// check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++)
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 4bfd4ec84e..0131892928 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -84,6 +84,7 @@
#include "telemetry/telemetry.h"
#include "io/osd_slave.h"
+#include "io/rcsplit.h"
#ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs);
@@ -594,5 +595,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
+
+#ifdef USE_RCSPLIT
+ [TASK_RCSPLIT] = {
+ .taskName = "RCSPLIT",
+ .taskFunc = rcSplitProcess,
+ .desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz, 100ms
+ .staticPriority = TASK_PRIORITY_MEDIUM,
+ },
+#endif
#endif
};
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 2b0d223786..17c35f7e22 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -54,6 +54,9 @@ typedef enum {
BOX3DDISABLESWITCH,
BOXFPVANGLEMIX,
BOXBLACKBOXERASE,
+ BOXCAMERA1,
+ BOXCAMERA2,
+ BOXCAMERA3,
CHECKBOX_ITEM_COUNT
} boxId_e;
diff --git a/src/main/io/rcsplit.c b/src/main/io/rcsplit.c
new file mode 100644
index 0000000000..e9533bed6d
--- /dev/null
+++ b/src/main/io/rcsplit.c
@@ -0,0 +1,163 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+#include
+#include
+#include
+
+#include
+
+#include "common/utils.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "fc/rc_controls.h"
+
+#include "io/beeper.h"
+#include "io/serial.h"
+
+#include "scheduler/scheduler.h"
+
+#include "drivers/serial.h"
+
+#include "io/rcsplit.h"
+
+// communicate with camera device variables
+serialPort_t *rcSplitSerialPort = NULL;
+rcsplit_switch_state_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
+rcsplit_state_e cameraState = RCSPLIT_STATE_UNKNOWN;
+
+static uint8_t crc_high_first(uint8_t *ptr, uint8_t len)
+{
+ uint8_t i;
+ uint8_t crc=0x00;
+ while (len--) {
+ crc ^= *ptr++;
+ for (i=8; i>0; --i) {
+ if (crc & 0x80)
+ crc = (crc << 1) ^ 0x31;
+ else
+ crc = (crc << 1);
+ }
+ }
+ return (crc);
+}
+
+static void sendCtrlCommand(rcsplit_ctrl_argument_e argument)
+{
+ if (!rcSplitSerialPort)
+ return ;
+
+ uint8_t uart_buffer[5] = {0};
+ uint8_t crc = 0;
+
+ uart_buffer[0] = RCSPLIT_PACKET_HEADER;
+ uart_buffer[1] = RCSPLIT_PACKET_CMD_CTRL;
+ uart_buffer[2] = argument;
+ uart_buffer[3] = RCSPLIT_PACKET_TAIL;
+ crc = crc_high_first(uart_buffer, 4);
+
+ // build up a full request [header]+[command]+[argument]+[crc]+[tail]
+ uart_buffer[3] = crc;
+ uart_buffer[4] = RCSPLIT_PACKET_TAIL;
+
+ // write to device
+ serialWriteBuf(rcSplitSerialPort, uart_buffer, 5);
+}
+
+static void rcSplitProcessMode()
+{
+ // if the device not ready, do not handle any mode change event
+ if (RCSPLIT_STATE_IS_READY != cameraState)
+ return ;
+
+ for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
+ uint8_t switchIndex = i - BOXCAMERA1;
+ if (IS_RC_MODE_ACTIVE(i)) {
+ // check last state of this mode, if it's true, then ignore it.
+ // Here is a logic to make a toggle control for this mode
+ if (switchStates[switchIndex].isActivated) {
+ continue;
+ }
+
+ uint8_t argument = RCSPLIT_CTRL_ARGU_INVALID;
+ switch (i) {
+ case BOXCAMERA1:
+ argument = RCSPLIT_CTRL_ARGU_WIFI_BTN;
+ break;
+ case BOXCAMERA2:
+ argument = RCSPLIT_CTRL_ARGU_POWER_BTN;
+ break;
+ case BOXCAMERA3:
+ argument = RCSPLIT_CTRL_ARGU_CHANGE_MODE;
+ break;
+ default:
+ argument = RCSPLIT_CTRL_ARGU_INVALID;
+ break;
+ }
+
+ if (argument != RCSPLIT_CTRL_ARGU_INVALID) {
+ sendCtrlCommand(argument);
+ switchStates[switchIndex].isActivated = true;
+ }
+ } else {
+ switchStates[switchIndex].isActivated = false;
+ }
+ }
+}
+
+bool rcSplitInit(void)
+{
+ // found the port config with FUNCTION_RUNCAM_SPLIT_CONTROL
+ // User must set some UART inteface with RunCam Split at peripherals column in Ports tab
+ serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RCSPLIT);
+ if (portConfig) {
+ rcSplitSerialPort = openSerialPort(portConfig->identifier, FUNCTION_RCSPLIT, NULL, 115200, MODE_RXTX, 0);
+ }
+
+ if (!rcSplitSerialPort) {
+ return false;
+ }
+
+ // set init value to true, to avoid the action auto run when the flight board start and the switch is on.
+ for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
+ uint8_t switchIndex = i - BOXCAMERA1;
+ switchStates[switchIndex].boxId = 1 << i;
+ switchStates[switchIndex].isActivated = true;
+ }
+
+ cameraState = RCSPLIT_STATE_IS_READY;
+
+#ifdef USE_RCSPLIT
+ setTaskEnabled(TASK_RCSPLIT, true);
+#endif
+
+ return true;
+}
+
+void rcSplitProcess(timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+
+ if (rcSplitSerialPort == NULL)
+ return ;
+
+ // process rcsplit custom mode if has any changed
+ rcSplitProcessMode();
+}
diff --git a/src/main/io/rcsplit.h b/src/main/io/rcsplit.h
new file mode 100644
index 0000000000..6900700eee
--- /dev/null
+++ b/src/main/io/rcsplit.h
@@ -0,0 +1,56 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#include
+#include "common/time.h"
+#include "fc/fc_msp.h"
+
+typedef struct {
+ uint8_t boxId;
+ bool isActivated;
+} rcsplit_switch_state_t;
+
+typedef enum {
+ RCSPLIT_STATE_UNKNOWN = 0,
+ RCSPLIT_STATE_INITIALIZING,
+ RCSPLIT_STATE_IS_READY,
+} rcsplit_state_e;
+
+// packet header and tail
+#define RCSPLIT_PACKET_HEADER 0x55
+#define RCSPLIT_PACKET_CMD_CTRL 0x01
+#define RCSPLIT_PACKET_TAIL 0xaa
+
+
+// the commands of RunCam Split serial protocol
+typedef enum {
+ RCSPLIT_CTRL_ARGU_INVALID = 0x0,
+ RCSPLIT_CTRL_ARGU_WIFI_BTN = 0x1,
+ RCSPLIT_CTRL_ARGU_POWER_BTN = 0x2,
+ RCSPLIT_CTRL_ARGU_CHANGE_MODE = 0x3,
+ RCSPLIT_CTRL_ARGU_WHO_ARE_YOU = 0xFF,
+} rcsplit_ctrl_argument_e;
+
+bool rcSplitInit(void);
+void rcSplitProcess(timeUs_t currentTimeUs);
+
+// only for unit test
+extern rcsplit_state_e cameraState;
+extern serialPort_t *rcSplitSerialPort;
+extern rcsplit_switch_state_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index e7fb4d4c63..5d0009d1b3 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -44,6 +44,7 @@ typedef enum {
FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048
FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096
FUNCTION_VTX_TRAMP = (1 << 13), // 8192
+ FUNCTION_RCSPLIT = (1 << 14), // 16384
} serialPortFunction_e;
typedef enum {
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 7f819b6df6..f943d950cb 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -111,6 +111,10 @@ typedef enum {
TASK_VTXCTRL,
#endif
+#ifdef USE_RCSPLIT
+ TASK_RCSPLIT,
+#endif
+
/* Count of real tasks */
TASK_COUNT,
diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h
index 9db23d8599..534e788507 100644
--- a/src/main/target/common_fc_pre.h
+++ b/src/main/target/common_fc_pre.h
@@ -130,3 +130,5 @@
#define USE_UNCOMMON_MIXERS
#endif
+
+#define USE_RCSPLIT
diff --git a/src/test/Makefile b/src/test/Makefile
index 6a02179fdd..c97ed18723 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -185,6 +185,15 @@ type_conversion_unittest_SRC := \
ws2811_unittest_SRC := \
$(USER_DIR)/drivers/light_ws2811strip.c
+rcsplit_unittest_SRC := \
+ $(USER_DIR)/common/bitarray.c \
+ $(USER_DIR)/fc/rc_modes.c \
+ $(USER_DIR)/io/rcsplit.c
+
+rcsplit_unitest_DEFINES := \
+ USE_UART3 \
+ USE_RCSPLIT \
+
# Please tweak the following variable definitions as needed by your
# project, except GTEST_HEADERS, which you can use in your own targets
# but shouldn't modify.
diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc
index 415f9deb1c..5cf1c57e6b 100644
--- a/src/test/unit/rc_controls_unittest.cc
+++ b/src/test/unit/rc_controls_unittest.cc
@@ -25,6 +25,7 @@ extern "C" {
#include "common/maths.h"
#include "common/axis.h"
+ #include "common/bitarray.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
@@ -156,14 +157,14 @@ TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXV
rcData[AUX7] = 950; // value equal to range step upper boundary should not activate the mode
// and
- uint32_t expectedMask = 0;
- expectedMask |= (1 << 0);
- expectedMask |= (1 << 1);
- expectedMask |= (1 << 2);
- expectedMask |= (1 << 3);
- expectedMask |= (1 << 4);
- expectedMask |= (1 << 5);
- expectedMask |= (0 << 6);
+ boxBitmask_t activeBoxIds;
+ memset(&activeBoxIds, 0, sizeof(boxBitmask_t));
+ bitArraySet(&activeBoxIds, 0);
+ bitArraySet(&activeBoxIds, 1);
+ bitArraySet(&activeBoxIds, 2);
+ bitArraySet(&activeBoxIds, 3);
+ bitArraySet(&activeBoxIds, 4);
+ bitArraySet(&activeBoxIds, 5);
// when
updateActivatedModes();
@@ -171,9 +172,9 @@ TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXV
// then
for (int index = 0; index < CHECKBOX_ITEM_COUNT; index++) {
#ifdef DEBUG_RC_CONTROLS
- printf("iteration: %d\n", index);
+ printf("iteration: %d, %d\n", index, (bool)(bitArrayGet(&activeBoxIds, index)));
#endif
- EXPECT_EQ((bool)(expectedMask & (1 << index)), IS_RC_MODE_ACTIVE((boxId_e)index));
+ EXPECT_EQ((bool)(bitArrayGet(&activeBoxIds, index)), IS_RC_MODE_ACTIVE((boxId_e)index));
}
}
diff --git a/src/test/unit/rcsplit_unittest.cc b/src/test/unit/rcsplit_unittest.cc
new file mode 100644
index 0000000000..67be33ae78
--- /dev/null
+++ b/src/test/unit/rcsplit_unittest.cc
@@ -0,0 +1,431 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+
+#include "gtest/gtest.h"
+
+extern "C" {
+ #include
+ #include
+ #include
+
+ #include "platform.h"
+
+ #include "common/utils.h"
+ #include "common/maths.h"
+ #include "common/bitarray.h"
+
+ #include "config/parameter_group.h"
+ #include "config/parameter_group_ids.h"
+
+ #include "fc/rc_controls.h"
+ #include "fc/rc_modes.h"
+
+
+ #include "io/beeper.h"
+ #include "io/serial.h"
+
+ #include "scheduler/scheduler.h"
+ #include "drivers/serial.h"
+ #include "io/rcsplit.h"
+
+ #include "rx/rx.h"
+
+ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
+
+ rcsplit_state_e unitTestRCsplitState()
+ {
+ return cameraState;
+ }
+
+ bool unitTestIsSwitchActivited(boxId_e boxId)
+ {
+ uint8_t adjustBoxID = boxId - BOXCAMERA1;
+ rcsplit_switch_state_t switchState = switchStates[adjustBoxID];
+ return switchState.isActivated;
+ }
+
+ void unitTestResetRCSplit()
+ {
+ rcSplitSerialPort = NULL;
+ cameraState = RCSPLIT_STATE_UNKNOWN;
+ }
+}
+
+typedef struct testData_s {
+ bool isRunCamSplitPortConfigurated;
+ bool isRunCamSplitOpenPortSupported;
+ int8_t maxTimesOfRespDataAvailable;
+ bool isAllowBufferReadWrite;
+} testData_t;
+
+static testData_t testData;
+
+TEST(RCSplitTest, TestRCSplitInitWithoutPortConfigurated)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ bool result = rcSplitInit();
+ EXPECT_EQ(false, result);
+ EXPECT_EQ(RCSPLIT_STATE_UNKNOWN, unitTestRCsplitState());
+}
+
+TEST(RCSplitTest, TestRCSplitInitWithoutOpenPortConfigurated)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = false;
+ testData.isRunCamSplitPortConfigurated = true;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(false, result);
+ EXPECT_EQ(RCSPLIT_STATE_UNKNOWN, unitTestRCsplitState());
+}
+
+TEST(RCSplitTest, TestRCSplitInit)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = true;
+ testData.isRunCamSplitPortConfigurated = true;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(true, result);
+ EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState());
+}
+
+TEST(RCSplitTest, TestRecvWhoAreYouResponse)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = true;
+ testData.isRunCamSplitPortConfigurated = true;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(true, result);
+
+ // here will generate a number in [6-255], it's make the serialRxBytesWaiting() and serialRead() run at least 5 times,
+ // so the "who are you response" will full received, and cause the state change to RCSPLIT_STATE_IS_READY;
+ int8_t randNum = rand() % 127 + 6;
+ testData.maxTimesOfRespDataAvailable = randNum;
+ rcSplitProcess((timeUs_t)0);
+
+ EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState());
+}
+
+TEST(RCSplitTest, TestWifiModeChangeWithDeviceUnready)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = true;
+ testData.isRunCamSplitPortConfigurated = true;
+ testData.maxTimesOfRespDataAvailable = 0;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(true, result);
+
+ // bind aux1, aux2, aux3 channel to wifi button, power button and change mode
+ for (uint8_t i = 0; i <= (BOXCAMERA3 - BOXCAMERA1); i++) {
+ memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t));
+ }
+
+ // bind aux1 to wifi button with range [900,1600]
+ modeActivationConditionsMutable(0)->auxChannelIndex = 0;
+ modeActivationConditionsMutable(0)->modeId = BOXCAMERA1;
+ modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
+ modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
+
+ // bind aux2 to power button with range [1900, 2100]
+ modeActivationConditionsMutable(1)->auxChannelIndex = 1;
+ modeActivationConditionsMutable(1)->modeId = BOXCAMERA2;
+ modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
+ modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
+
+ // bind aux3 to change mode with range [1300, 1600]
+ modeActivationConditionsMutable(2)->auxChannelIndex = 2;
+ modeActivationConditionsMutable(2)->modeId = BOXCAMERA3;
+ modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1300);
+ modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
+
+ // make the binded mode inactive
+ rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1800;
+ rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 900;
+ rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 900;
+
+ updateActivatedModes();
+
+ // runn process loop
+ rcSplitProcess(0);
+
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA2));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
+}
+
+TEST(RCSplitTest, TestWifiModeChangeWithDeviceReady)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = true;
+ testData.isRunCamSplitPortConfigurated = true;
+ testData.maxTimesOfRespDataAvailable = 0;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(true, result);
+
+ // bind aux1, aux2, aux3 channel to wifi button, power button and change mode
+ for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) {
+ memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t));
+ }
+
+
+ // bind aux1 to wifi button with range [900,1600]
+ modeActivationConditionsMutable(0)->auxChannelIndex = 0;
+ modeActivationConditionsMutable(0)->modeId = BOXCAMERA1;
+ modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
+ modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
+
+ // bind aux2 to power button with range [1900, 2100]
+ modeActivationConditionsMutable(1)->auxChannelIndex = 1;
+ modeActivationConditionsMutable(1)->modeId = BOXCAMERA2;
+ modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
+ modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
+
+ // bind aux3 to change mode with range [1300, 1600]
+ modeActivationConditionsMutable(2)->auxChannelIndex = 2;
+ modeActivationConditionsMutable(2)->modeId = BOXCAMERA3;
+ modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
+ modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
+
+ rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
+ rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000;
+ rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
+
+ updateActivatedModes();
+
+ // runn process loop
+ int8_t randNum = rand() % 127 + 6;
+ testData.maxTimesOfRespDataAvailable = randNum;
+ rcSplitProcess((timeUs_t)0);
+
+ EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState());
+
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1));
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
+}
+
+TEST(RCSplitTest, TestWifiModeChangeCombine)
+{
+ memset(&testData, 0, sizeof(testData));
+ unitTestResetRCSplit();
+ testData.isRunCamSplitOpenPortSupported = true;
+ testData.isRunCamSplitPortConfigurated = true;
+ testData.maxTimesOfRespDataAvailable = 0;
+
+ bool result = rcSplitInit();
+ EXPECT_EQ(true, result);
+
+ // bind aux1, aux2, aux3 channel to wifi button, power button and change mode
+ for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) {
+ memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t));
+ }
+
+
+ // bind aux1 to wifi button with range [900,1600]
+ modeActivationConditionsMutable(0)->auxChannelIndex = 0;
+ modeActivationConditionsMutable(0)->modeId = BOXCAMERA1;
+ modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
+ modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
+
+ // bind aux2 to power button with range [1900, 2100]
+ modeActivationConditionsMutable(1)->auxChannelIndex = 1;
+ modeActivationConditionsMutable(1)->modeId = BOXCAMERA2;
+ modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
+ modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
+
+ // bind aux3 to change mode with range [1300, 1600]
+ modeActivationConditionsMutable(2)->auxChannelIndex = 2;
+ modeActivationConditionsMutable(2)->modeId = BOXCAMERA3;
+ modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
+ modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
+
+ // // make the binded mode inactive
+ rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
+ rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000;
+ rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
+ updateActivatedModes();
+
+ // runn process loop
+ int8_t randNum = rand() % 127 + 6;
+ testData.maxTimesOfRespDataAvailable = randNum;
+ rcSplitProcess((timeUs_t)0);
+
+ EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState());
+
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1));
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
+
+
+ // // make the binded mode inactive
+ rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1500;
+ rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1300;
+ rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1900;
+ updateActivatedModes();
+ rcSplitProcess((timeUs_t)0);
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA1));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA2));
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA3));
+
+
+ rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1899;
+ updateActivatedModes();
+ rcSplitProcess((timeUs_t)0);
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
+
+ rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2001;
+ updateActivatedModes();
+ rcSplitProcess((timeUs_t)0);
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA1));
+ EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2));
+ EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
+}
+
+extern "C" {
+ serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options)
+ {
+ UNUSED(identifier);
+ UNUSED(functionMask);
+ UNUSED(baudRate);
+ UNUSED(callback);
+ UNUSED(mode);
+ UNUSED(options);
+
+ if (testData.isRunCamSplitOpenPortSupported) {
+ static serialPort_t s;
+ s.vTable = NULL;
+
+ // common serial initialisation code should move to serialPort::init()
+ s.rxBufferHead = s.rxBufferTail = 0;
+ s.txBufferHead = s.txBufferTail = 0;
+ s.rxBufferSize = 0;
+ s.txBufferSize = 0;
+ s.rxBuffer = s.rxBuffer;
+ s.txBuffer = s.txBuffer;
+
+ // callback works for IRQ-based RX ONLY
+ s.rxCallback = NULL;
+ s.baudRate = 0;
+
+ return (serialPort_t *)&s;
+ }
+
+ return NULL;
+ }
+
+ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function)
+ {
+ UNUSED(function);
+ if (testData.isRunCamSplitPortConfigurated) {
+ static serialPortConfig_t portConfig;
+
+ portConfig.identifier = SERIAL_PORT_USART3;
+ portConfig.msp_baudrateIndex = BAUD_115200;
+ portConfig.gps_baudrateIndex = BAUD_57600;
+ portConfig.telemetry_baudrateIndex = BAUD_AUTO;
+ portConfig.blackbox_baudrateIndex = BAUD_115200;
+ portConfig.functionMask = FUNCTION_MSP;
+
+ return &portConfig;
+ }
+
+ return NULL;
+ }
+
+ uint32_t serialRxBytesWaiting(const serialPort_t *instance)
+ {
+ UNUSED(instance);
+
+ testData.maxTimesOfRespDataAvailable--;
+ if (testData.maxTimesOfRespDataAvailable > 0) {
+ return 1;
+ }
+
+ return 0;
+ }
+
+ uint8_t serialRead(serialPort_t *instance)
+ {
+ UNUSED(instance);
+
+ if (testData.maxTimesOfRespDataAvailable > 0) {
+ static uint8_t i = 0;
+ static uint8_t buffer[] = { 0x55, 0x01, 0xFF, 0xad, 0xaa };
+
+ if (i >= 5) {
+ i = 0;
+ }
+
+ return buffer[i++];
+ }
+
+ return 0;
+ }
+
+ void sbufWriteString(sbuf_t *dst, const char *string)
+ {
+ UNUSED(dst); UNUSED(string);
+
+ if (testData.isAllowBufferReadWrite) {
+ sbufWriteData(dst, string, strlen(string));
+ }
+ }
+ void sbufWriteU8(sbuf_t *dst, uint8_t val)
+ {
+ UNUSED(dst); UNUSED(val);
+
+ if (testData.isAllowBufferReadWrite) {
+ *dst->ptr++ = val;
+ }
+ }
+
+ void sbufWriteData(sbuf_t *dst, const void *data, int len)
+ {
+ UNUSED(dst); UNUSED(data); UNUSED(len);
+
+ if (testData.isAllowBufferReadWrite) {
+ memcpy(dst->ptr, data, len);
+ dst->ptr += len;
+
+ }
+ }
+
+ // modifies streambuf so that written data are prepared for reading
+ void sbufSwitchToReader(sbuf_t *buf, uint8_t *base)
+ {
+ UNUSED(buf); UNUSED(base);
+
+ if (testData.isAllowBufferReadWrite) {
+ buf->end = buf->ptr;
+ buf->ptr = base;
+ }
+ }
+
+ bool feature(uint32_t) { return false;}
+ void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { UNUSED(instance); UNUSED(data); UNUSED(count); }
+}
\ No newline at end of file