Logic Conditions integration

make/source.mk

@@ -46,6 +46,7 @@ COMMON_SRC = \
             drivers/io.c \
             drivers/io_pca9685.c \
             drivers/io_pcf8574.c \
+            drivers/io_port_expander.c \
             drivers/irlock.c \
             drivers/light_led.c \
             drivers/osd.c \

src/main/drivers/io_pcf8574.c

@@ -29,8 +29,6 @@
 #include "drivers/time.h"
 #include "build/debug.h"
 
-#ifdef USE_PCF8574
-
 #define PCF8574_WRITE_ADDRESS 0x40
 #define PCF8574_READ_ADDRESS 0x41
 
@@ -70,8 +68,6 @@ bool pcf8574Init(void)
         return false;
     }
 
-    pcf8574Write(0x00); //Set all ports to OFF
-    delay(25);
     return true;
 }
 
@@ -85,6 +81,4 @@ uint8_t pcf8574Read(void)
     uint8_t data;
     busRead(busDev, PCF8574_READ_ADDRESS, &data);
     return data;
-}
+}
-
-#endif

src/main/drivers/io_port_expander.c

@@ -0,0 +1,67 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include "platform.h"
+#include "drivers/io_port_expander.h"
+#include "drivers/io_pcf8574.h"
+
+#ifdef USE_I2C_IO_EXPANDER
+
+static ioPortExpanderState_t ioPortExpanderState;
+
+void ioPortExpanderInit(void)
+{
+
+    ioPortExpanderState.active = pcf8574Init();
+
+    if (ioPortExpanderState.active) {
+        ioPortExpanderState.state = 0x00;
+        pcf8574Write(ioPortExpanderState.state); //Set all ports to OFF
+    }
+
+}
+
+void ioPortExpanderSet(uint8_t pin, uint8_t value)
+{
+    if (pin > 7) {
+        return;
+    }
+
+    //Cast to 0/1
+    value = (bool) value;
+
+    ioPortExpanderState.state ^= (-value ^ ioPortExpanderState.state) & (1UL << pin);
+    ioPortExpanderState.shouldSync = true;
+}
+
+void ioPortExpanderSync(void)
+{
+    if (ioPortExpanderState.active && ioPortExpanderState.shouldSync) {
+        pcf8574Write(ioPortExpanderState.state);
+        ioPortExpanderState.shouldSync = false;;
+    }
+}
+
+#endif

src/main/drivers/io_port_expander.h

@@ -0,0 +1,37 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include "stdint.h"
+
+typedef struct ioPortExpanderState_s {
+    uint8_t active;
+    uint8_t state;
+    uint8_t shouldSync;
+} ioPortExpanderState_t;
+
+void ioPortExpanderInit(void);
+void ioPortExpanderSync(void);
+void ioPortExpanderSet(uint8_t pin, uint8_t value);

src/main/fc/fc_init.c

@@ -83,7 +83,7 @@
 #include "msc/emfat_file.h"
 #endif
 #include "drivers/sdcard/sdcard.h"
-#include "drivers/io_pcf8574.h"
+#include "drivers/io_port_expander.h"
 
 #include "fc/cli.h"
 #include "fc/config.h"
@@ -675,9 +675,8 @@ void init(void)
     }
 #endif
 
-#ifdef USE_PCF8574
+#ifdef USE_I2C_IO_EXPANDER
-    bool pcfActive = pcf8574Init();
+    ioPortExpanderInit();
-    DEBUG_SET(DEBUG_PCF8574, 1, pcfActive);
 #endif
 
     // Considering that the persistent reset reason is only used during init

src/main/programming/logic_condition.c

@@ -41,6 +41,7 @@
 #include "sensors/pitotmeter.h"
 #include "flight/imu.h"
 #include "flight/pid.h"
+#include "drivers/io_port_expander.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
@@ -180,7 +181,12 @@ static int logicConditionCompute(
                 return operandA;
             }
             break;
-
+#ifdef USE_I2C_IO_EXPANDER
+        case LOGIC_CONDITION_PORT_SET:
+            ioPortExpanderSet((uint8_t)operandA, (uint8_t)operandB);
+            return operandB;
+            break;
+#endif
         default:
             return false;
             break; 
@@ -456,6 +462,9 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) {
     for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
         logicConditionProcess(i);
     }
+#ifdef USE_I2C_IO_EXPANDER
+    ioPortExpanderSync();
+#endif
 }
 
 void logicConditionReset(void) {

src/main/programming/logic_condition.h

@@ -29,27 +29,28 @@
 #define MAX_LOGIC_CONDITIONS 16
 
 typedef enum {
-    LOGIC_CONDITION_TRUE = 0,       // 0
+    LOGIC_CONDITION_TRUE            = 0,
-    LOGIC_CONDITION_EQUAL,          // 1
+    LOGIC_CONDITION_EQUAL           = 1,
-    LOGIC_CONDITION_GREATER_THAN,   // 2
+    LOGIC_CONDITION_GREATER_THAN    = 2,
-    LOGIC_CONDITION_LOWER_THAN,     // 3
+    LOGIC_CONDITION_LOWER_THAN      = 3,
-    LOGIC_CONDITION_LOW,            // 4
+    LOGIC_CONDITION_LOW             = 4,
-    LOGIC_CONDITION_MID,            // 5
+    LOGIC_CONDITION_MID             = 5,
-    LOGIC_CONDITION_HIGH,           // 6
+    LOGIC_CONDITION_HIGH            = 6,
-    LOGIC_CONDITION_AND,            // 7
+    LOGIC_CONDITION_AND             = 7,
-    LOGIC_CONDITION_OR,             // 8
+    LOGIC_CONDITION_OR              = 8,
-    LOGIC_CONDITION_XOR,            // 9
+    LOGIC_CONDITION_XOR             = 9,
-    LOGIC_CONDITION_NAND,           // 10
+    LOGIC_CONDITION_NAND            = 10,
-    LOGIC_CONDITION_NOR,            // 11
+    LOGIC_CONDITION_NOR             = 11,
-    LOGIC_CONDITION_NOT,            // 12
+    LOGIC_CONDITION_NOT             = 12,
-    LOGIC_CONDITION_STICKY,         // 13
+    LOGIC_CONDITION_STICKY          = 13,
-    LOGIC_CONDITION_ADD,            // 14
+    LOGIC_CONDITION_ADD             = 14,
-    LOGIC_CONDITION_SUB,            // 15
+    LOGIC_CONDITION_SUB             = 15,
-    LOGIC_CONDITION_MUL,            // 16
+    LOGIC_CONDITION_MUL             = 16,
-    LOGIC_CONDITION_DIV,            // 17
+    LOGIC_CONDITION_DIV             = 17,
-    LOGIC_CONDITION_GVAR_SET,       // 18
+    LOGIC_CONDITION_GVAR_SET        = 18,
-    LOGIC_CONDITION_GVAR_INC,       // 19
+    LOGIC_CONDITION_GVAR_INC        = 19,
-    LOGIC_CONDITION_GVAR_DEC,       // 20
+    LOGIC_CONDITION_GVAR_DEC        = 20,
+    LOGIC_CONDITION_PORT_SET        = 128,
     LOGIC_CONDITION_LAST
 } logicOperation_e;
 

src/main/target/common.h

@@ -122,7 +122,6 @@
 #define USE_ANTIGRAVITY
 
 #define USE_I2C_IO_EXPANDER
-#define USE_PCF8574
 
 #else // FLASH_SIZE < 256
 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR

src/main/target/common_hardware.c

@@ -353,7 +353,7 @@
     BUSDEV_REGISTER_I2C(busdev_irlock,      DEVHW_IRLOCK,       IRLOCK_I2C_BUS,     0x54,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);
 #endif
 
-#if defined(USE_I2C) && defined(USE_PCF8574)
+#if defined(USE_I2C) && defined(USE_I2C_IO_EXPANDER)
 
     #if !defined(PCF8574_I2C_BUS) && defined(EXTERNAL_I2C_BUS)
         #define PCF8574_I2C_BUS EXTERNAL_I2C_BUS