Add IR-Lock optical positioning system support (#5677)

make/source.mk

@@ -44,6 +44,7 @@ COMMON_SRC = \
             drivers/exti.c \
             drivers/io.c \
             drivers/io_pca9685.c \
+            drivers/irlock.c \
             drivers/light_led.c \
             drivers/osd.c \
             drivers/persistent.c \
@@ -144,13 +145,14 @@ COMMON_SRC = \
             scheduler/scheduler.c \
             sensors/acceleration.c \
             sensors/battery.c \
-            sensors/temperature.c \
             sensors/boardalignment.c \
             sensors/compass.c \
             sensors/diagnostics.c \
             sensors/gyro.c \
             sensors/initialisation.c \
             sensors/esc_sensor.c \
+            sensors/irlock.c \
+            sensors/temperature.c \
             uav_interconnect/uav_interconnect_bus.c \
             uav_interconnect/uav_interconnect_rangefinder.c \
             blackbox/blackbox.c \

src/main/build/debug.h

@@ -70,5 +70,6 @@ typedef enum {
     DEBUG_NAV_YAW,
     DEBUG_DYNAMIC_FILTER,
     DEBUG_DYNAMIC_FILTER_FREQUENCY,
+    DEBUG_IRLOCK,
     DEBUG_COUNT
 } debugType_e;

src/main/common/maths.h

@@ -60,6 +60,8 @@
 #define CENTIMETERS_TO_FEET(cm)                 (cm * (328 / 10000.0))
 #define CENTIMETERS_TO_METERS(cm)               (cm / 100)
 
+#define METERS_TO_CENTIMETERS(m)                (m * 100)
+
 // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70
 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar)         \
     ( __extension__ ({                                      \

src/main/drivers/bus.h

@@ -143,6 +143,7 @@ typedef enum {
     DEVHW_M25P16,       // SPI NOR flash
     DEVHW_UG2864,       // I2C OLED display
     DEVHW_SDCARD,       // Generic SD-Card
+    DEVHW_IRLOCK,       // IR-Lock visual positioning hardware
 } devHardwareType_e;
 
 typedef enum {

src/main/drivers/irlock.c

@@ -0,0 +1,88 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav 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.
+ *
+ * iNav 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 iNav.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <math.h>
+
+#include "platform.h"
+
+#include "drivers/sensor.h"
+#include "drivers/irlock.h"
+#include "drivers/time.h"
+
+#define IRLOCK_OBJECT_SYNC ((uint16_t)0xaa55)
+#define IRLOCK_FRAME_SYNC ((uint32_t)(IRLOCK_OBJECT_SYNC | (IRLOCK_OBJECT_SYNC << 16)))
+
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+static bool irlockHealthy = false;
+
+static bool irlockFrameSync(irlockDev_t *irlockDev)
+{
+    uint32_t sync_word = 0;
+    uint8_t count = 10;
+    while (count-- && sync_word != IRLOCK_FRAME_SYNC) {
+        uint8_t sync_byte;
+        irlockHealthy = busRead(irlockDev->busDev, 0xFF, &sync_byte);
+        if (!(irlockHealthy && sync_byte)) return false;
+        sync_word = (sync_word >> 8) | (((uint32_t)sync_byte) << 24);
+    }
+    return sync_word == IRLOCK_FRAME_SYNC;
+}
+
+static bool irlockRead(irlockDev_t *irlockDev, irlockData_t *irlockData)
+{
+    if (irlockFrameSync(irlockDev) && busReadBuf(irlockDev->busDev, 0xFF, (void*)irlockData, sizeof(*irlockData))) {
+        uint16_t cksum = irlockData->signature + irlockData->posX + irlockData->posY + irlockData->sizeX + irlockData->sizeY;
+        if (irlockData->cksum == cksum) return true;
+    }
+    return false;
+}
+
+static bool deviceDetect(irlockDev_t *irlockDev)
+{
+    uint8_t buf;
+    bool detected = busRead(irlockDev->busDev, 0xFF, &buf);
+    return !!detected;
+}
+
+bool irlockDetect(irlockDev_t *irlockDev)
+{
+    irlockDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_IRLOCK, 0, OWNER_IRLOCK);
+    if (irlockDev->busDev == NULL) {
+        return false;
+    }
+
+    if (!deviceDetect(irlockDev)) {
+        busDeviceDeInit(irlockDev->busDev);
+        return false;
+    }
+
+    irlockDev->read = irlockRead;
+
+    return true;
+}
+
+bool irlockIsHealthy(void)
+{
+    return irlockHealthy;
+}
+
+#endif /* USE_IRLOCK */

src/main/drivers/irlock.h

@@ -0,0 +1,45 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav 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.
+ *
+ * iNav 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 iNav.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "drivers/sensor.h"
+#include "drivers/io_types.h"
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+#define IRLOCK_RES_X 320
+#define IRLOCK_RES_Y 200
+
+typedef struct {
+    uint16_t cksum;
+    uint16_t signature;
+    uint16_t posX;
+    uint16_t posY;
+    uint16_t sizeX;
+    uint16_t sizeY;
+} irlockData_t;
+
+typedef struct irlockDev_s {
+    busDevice_t *busDev;
+    bool (*read)(struct irlockDev_s *irlockDev, irlockData_t *irlockData);
+} irlockDev_t;
+
+bool irlockDetect(irlockDev_t *irlockDev);
+bool irlockIsHealthy(void);
+
+#endif /* USE_IRLOCK */

src/main/drivers/resource.c

@@ -22,7 +22,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
     "RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
     "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER",
     "NRF24", "VTX", "SPI_PREINIT", "COMPASS", "TEMPERATURE", "1-WIRE", "AIRSPEED", "OLED DISPLAY",
-    "PINIO"
+    "PINIO", "IRLOCK"
 };
 
 const char * const resourceNames[RESOURCE_TOTAL_COUNT] = {

src/main/drivers/resource.h

@@ -56,6 +56,7 @@ typedef enum {
     OWNER_AIRSPEED,
     OWNER_OLED_DISPLAY,
     OWNER_PINIO,
+    OWNER_IRLOCK,
     OWNER_TOTAL_COUNT
 } resourceOwner_e;
 

src/main/fc/fc_tasks.c

@@ -78,6 +78,7 @@
 #include "sensors/battery.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
+#include "sensors/irlock.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
 #include "sensors/opflow.h"
@@ -209,6 +210,14 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
 }
 #endif
 
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+void taskUpdateIrlock(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    irlockUpdate();
+}
+#endif
+
 #ifdef USE_OPFLOW
 void taskUpdateOpticalFlow(timeUs_t currentTimeUs)
 {
@@ -347,6 +356,9 @@ void fcTasksInit(void)
 #ifdef USE_GLOBAL_FUNCTIONS
     setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true);
 #endif
+#ifdef USE_IRLOCK
+    setTaskEnabled(TASK_IRLOCK, irlockHasBeenDetected());
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -454,6 +466,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_IRLOCK
+    [TASK_IRLOCK] = {
+        .taskName = "IRLOCK",
+        .taskFunc = taskUpdateIrlock,
+        .desiredPeriod = TASK_PERIOD_HZ(100),
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
 #ifdef USE_DASHBOARD
     [TASK_DASHBOARD] = {
         .taskName = "DASHBOARD",

src/main/fc/settings.yaml

@@ -81,7 +81,8 @@ tables:
     values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
-      "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY"]
+      "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
+      "IRLOCK"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator

src/main/scheduler/scheduler.h

@@ -118,6 +118,9 @@ typedef enum {
 #endif
 #ifdef USE_RPM_FILTER
     TASK_RPM_FILTER,
+#endif
+#ifdef USE_IRLOCK
+    TASK_IRLOCK,
 #endif
     /* Count of real tasks */
     TASK_COUNT,

src/main/sensors/initialisation.c

@@ -27,16 +27,17 @@
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 
-#include "sensors/sensors.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
-#include "sensors/pitotmeter.h"
-#include "sensors/gyro.h"
 #include "sensors/compass.h"
-#include "sensors/rangefinder.h"
-#include "sensors/opflow.h"
-#include "sensors/temperature.h"
+#include "sensors/gyro.h"
 #include "sensors/initialisation.h"
+#include "sensors/irlock.h"
+#include "sensors/opflow.h"
+#include "sensors/pitotmeter.h"
+#include "sensors/rangefinder.h"
+#include "sensors/sensors.h"
+#include "sensors/temperature.h"
 
 uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
@@ -102,6 +103,10 @@ bool sensorsAutodetect(void)
     }
 #endif
 
+#ifdef USE_IRLOCK
+    irlockInit();
+#endif
+
     if (eepromUpdatePending) {
         writeEEPROM();
     }

src/main/sensors/irlock.c

@@ -0,0 +1,136 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav 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.
+ *
+ * iNav 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 iNav.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "stdbool.h"
+#include "stdint.h"
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/log.h"
+#include "common/printf.h"
+
+#include "drivers/irlock.h"
+#include "drivers/time.h"
+
+#include "fc/runtime_config.h"
+
+#include "flight/imu.h"
+
+#include "navigation/navigation.h"
+
+#include "sensors/sensors.h"
+#include "sensors/barometer.h"
+#include "sensors/irlock.h"
+
+
+#define IRLOCK_TIMEOUT 100
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+enum {
+    DEBUG_IRLOCK_DETECTED,
+    DEBUG_IRLOCK_MEAS_VALID,
+    DEBUG_IRLOCK_POS_X,
+    DEBUG_IRLOCK_POS_Y
+};
+
+static irlockDev_t irlockDev;
+static bool irlockDetected = false;
+static bool measurementValid = false;
+static irlockData_t irlockData;
+static timeMs_t lastUpdateMs = 0;
+
+void irlockInit(void)
+{
+    irlockDetected = irlockDetect(&irlockDev);
+    DEBUG_SET(DEBUG_IRLOCK, DEBUG_IRLOCK_DETECTED, irlockDetected);
+}
+
+bool irlockHasBeenDetected(void)
+{
+    return irlockDetected;
+}
+
+bool irlockMeasurementIsValid(void) {
+    return measurementValid;
+}
+
+void irlockUpdate(void)
+{
+    if (irlockDetected && irlockDev.read(&irlockDev, &irlockData)) lastUpdateMs = millis();
+    measurementValid = millis() - lastUpdateMs < IRLOCK_TIMEOUT;
+
+    if (debugMode == DEBUG_IRLOCK) {
+        float distX, distY;
+        bool valid = irlockGetPosition(&distX, &distY);
+        debug[DEBUG_IRLOCK_MEAS_VALID] = valid;
+        debug[DEBUG_IRLOCK_POS_X] = lrintf(distX * 100);
+        debug[DEBUG_IRLOCK_POS_Y] = lrintf(distY * 100);
+    }
+}
+
+#define X_TO_DISTANCE_FACTOR -0.0029387573f
+#define X_TO_DISTANCE_OFFSET 0.4702011635f
+#define Y_TO_DISTANCE_FACTOR -0.0030568431f
+#define Y_TO_DISTANCE_OFFSET 0.3056843086f
+
+#define LENS_X_CORRECTION 4.4301355e-6f
+#define LENS_Y_CORRECTION 4.7933139e-6f
+
+// calculate distance relative to center at 1 meter distance from absolute object coordinates and taking into account lens distortion
+static void irlockCoordinatesToDistance(uint16_t pixX, uint16_t pixY, float *distX, float *distY)
+{
+    int16_t xCenterOffset = pixX - IRLOCK_RES_X / 2;
+    int16_t yCenterOffset = pixY - IRLOCK_RES_Y / 2;
+    float lensDistortionCorrectionFactor = LENS_X_CORRECTION * xCenterOffset * xCenterOffset + LENS_Y_CORRECTION * yCenterOffset * yCenterOffset - 1.0f;
+    *distX = (X_TO_DISTANCE_FACTOR * pixX + X_TO_DISTANCE_OFFSET) / lensDistortionCorrectionFactor;
+    *distY = (Y_TO_DISTANCE_FACTOR * pixY + Y_TO_DISTANCE_OFFSET) / lensDistortionCorrectionFactor;
+}
+
+bool irlockGetPosition(float *distX, float *distY)
+{
+    if (!measurementValid) return false;
+
+    // calculate edges of the object
+    int16_t corner1X = irlockData.posX - irlockData.sizeX / 2;
+    int16_t corner1Y = irlockData.posY - irlockData.sizeY / 2;
+    int16_t corner2X = irlockData.posX + irlockData.sizeX / 2;
+    int16_t corner2Y = irlockData.posY + irlockData.sizeY / 2;
+
+    // convert pixel position to distance
+    float corner1DistX, corner1DistY, corner2DistX, corner2DistY;
+    irlockCoordinatesToDistance(corner1X, corner1Y, &corner1DistX, &corner1DistY);
+    irlockCoordinatesToDistance(corner2X, corner2Y, &corner2DistX, &corner2DistY);
+
+    // return center of object
+    float uDistX = (corner1DistX + corner2DistX) / 2.0f; // lateral movement, positive means the aircraft is to the left of the beacon
+    float uDistY = (corner1DistY + corner2DistY) / 2.0f; // longitudinal movement, positive means the aircraft is in front of the beacon
+
+    // compensate for altitude and attitude
+    float altitude = CENTIMETERS_TO_METERS(getEstimatedActualPosition(Z));
+    *distX = altitude * tan_approx(atan2_approx(uDistX, 1.0f) - DECIDEGREES_TO_RADIANS(attitude.values.roll));
+    *distY = altitude * tan_approx(atan2_approx(uDistY, 1.0f) + DECIDEGREES_TO_RADIANS(attitude.values.pitch));
+
+    return true;
+}
+
+#endif /* USE_IRLOCK */

src/main/sensors/irlock.h

@@ -0,0 +1,29 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav 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.
+ *
+ * iNav 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 iNav.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+void irlockInit(void);
+bool irlockHasBeenDetected(void);
+
+void irlockUpdate(void);
+bool irlockMeasurementIsValid(void);
+bool irlockGetPosition(float *distX, float *distY);
+
+#endif /* USE_IRLOCK */

src/main/target/common_hardware.c

@@ -332,4 +332,13 @@
     #endif
 #endif
 
+#if defined(USE_IRLOCK) && defined(USE_I2C)
+    #if !defined(IRLOCK_I2C_BUS) && defined(EXTERNAL_I2C_BUS)
+        #define IRLOCK_I2C_BUS EXTERNAL_I2C_BUS
+    #else
+        #define IRLOCK_I2C_BUS BUS_I2C1
+    #endif
+    BUSDEV_REGISTER_I2C(busdev_irlock,      DEVHW_IRLOCK,       IRLOCK_I2C_BUS,     0x54,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+#endif
+
 #endif  // USE_TARGET_HARDWARE_DESCRIPTORS