Add driver for Lighthouse receiver prototype

Makefile

@@ -73,7 +73,7 @@ VPATH_CF2 += $(LIB)/CMSIS/STM32F4xx/Source/
 VPATH_CF2 += $(LIB)/STM32_USB_Device_Library/Core/src
 VPATH_CF2 += $(LIB)/STM32_USB_OTG_Driver/src
 VPATH_CF2 += src/deck/api src/deck/core src/deck/drivers/src src/deck/drivers/src/test
-VPATH_CF2 += src/utils/src/tdoa
+VPATH_CF2 += src/utils/src/tdoa src/utils/src/lighthouse
 CRT0_CF2 = startup_stm32f40xx.o system_stm32f4xx.o
 
 # Should maybe be in separate file?
@@ -193,6 +193,7 @@ PROJ_OBJ_CF2 += outlierFilter.o
 PROJ_OBJ_CF2 += flowdeck_v1v2.o
 PROJ_OBJ_CF2 += oa.o
 PROJ_OBJ_CF2 += multiranger.o
+PROJ_OBJ_CF2 += lighthouse.o
 
 ifeq ($(LPS_TDOA_ENABLE), 1)
 CFLAGS += -DLPS_TDOA_ENABLE
@@ -217,6 +218,8 @@ PROJ_OBJ += filter.o cpuid.o cfassert.o  eprintf.o crc.o num.o debug.o
 PROJ_OBJ += version.o FreeRTOS-openocd.o
 PROJ_OBJ_CF2 += configblockeeprom.o crc_bosch.o
 PROJ_OBJ_CF2 += sleepus.o
+PROJ_OBJ_CF2 += pulseProcessor.o lighthouseGeometry.o
+
 
 # Libs
 PROJ_OBJ_CF2 += libarm_math.a
@@ -250,6 +253,7 @@ INCLUDES_CF2 += -I$(LIB)/STM32_USB_OTG_Driver/inc
 INCLUDES_CF2 += -Isrc/deck/interface -Isrc/deck/drivers/interface
 INCLUDES_CF2 += -Isrc/utils/interface/clockCorrection
 INCLUDES_CF2 += -Isrc/utils/interface/tdoa
+INCLUDES_CF2 += -Isrc/utils/interface/lighthouse
 INCLUDES_CF2 += -Ivendor/libdw1000/inc
 INCLUDES_CF2 += -I$(LIB)/FatFS
 INCLUDES_CF2 += -I$(LIB)/vl53l1

src/deck/drivers/src/lighthouse.c

@@ -0,0 +1,184 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2018 Bitcraze AB
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * lighthouse.c: lighthouse tracking system receiver
+ */
+
+#include "system.h"
+#include "deck.h"
+#include "log.h"
+
+#include "config.h"
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "debug.h"
+#include "uart1.h"
+
+#include "pulseProcessor.h"
+#include "lighthouseGeometry.h"
+
+#include "estimator_kalman.h"
+
+
+static float angles[2][2];
+
+typedef union frame_u {
+  struct {
+    uint32_t timestamp:29;
+    uint32_t sensor:3;
+    uint16_t width;
+    uint8_t sync;
+  } __attribute__((packed));
+  char data[7];
+} __attribute__((packed)) frame_t;
+
+static bool getFrame(frame_t *frame)
+{
+  int syncCounter = 0;
+  for(int i=0; i<7; i++) {
+    uart1Getchar(&frame->data[i]);
+    if (frame->data[i] != 0) {
+      syncCounter += 1;
+    }
+  }
+  return (frame->sync == 0 || (syncCounter==7));
+}
+
+static vec3d position;
+
+baseStationGeometry_t baseStationsGeometry[] = {
+  {.origin = {-2.029516, 2.391417, -1.356382, }, .mat = {{-0.718327, 0.285313, -0.634511, }, {0.066982, 0.936164, 0.345125, }, {0.692474, 0.205412, -0.691582, }, }},
+  {.origin = {1.027486, 2.587440, 1.884445, }, .mat = {{0.846093, -0.256320, 0.467361, }, {-0.021730, 0.859477, 0.510712, }, {-0.532592, -0.442266, 0.721628, }, }},
+};
+
+static positionMeasurement_t ext_pos;
+
+static void lighthouseTask(void *param)
+{
+  bool synchronized = false;
+  int syncCounter = 0;
+  char c;
+  static frame_t frame;
+  static pulseProcessor_t ppState = {};
+
+  float angle;
+  int basestation;
+  int axis;
+
+  float delta;
+
+  systemWaitStart();
+
+  while(1) {
+    // Synchronize
+    // DEBUG_PRINT("Resynchronizing ...!\n");
+    syncCounter = 0;
+    while (!synchronized) {
+
+      uart1Getchar(&c);
+      if (c != 0) {
+        syncCounter += 1;
+      } else {
+        syncCounter = 0;
+      }
+      synchronized = syncCounter == 7;
+    }
+
+    // DEBUG_PRINT("Sync!\n");
+
+
+    // Receive data until being desynchronized
+    synchronized = getFrame(&frame);
+    while(synchronized) {
+      if (frame.sync != 0) {
+        // DEBUG_PRINT("Sync!\n");
+        synchronized = getFrame(&frame);
+        continue;
+      }
+
+      if (frame.sensor == 0) {
+        // DEBUG_PRINT("Reading %08X:%04X\n", frame.timestamp, frame.width);
+        if (processPulse(&ppState, frame.timestamp, frame.width, &angle, &basestation, &axis)) {
+          angles[basestation][axis] = angle;
+
+          if (basestation == 1 && axis == 1) {
+            lhgeometryGetPosition(baseStationsGeometry, (void*)angles, position, &delta);
+
+            ext_pos.x = position[0];
+            ext_pos.y = -position[2];
+            ext_pos.z = position[1];
+            ext_pos.stdDev = 0.01;
+            estimatorKalmanEnqueuePosition(&ext_pos);
+          }
+        }
+      } else {
+        DEBUG_PRINT("Receiving from wrong sensor?!?!?!\n");
+      }
+
+      synchronized = getFrame(&frame);
+      if (frame.sync != 0) {
+        // DEBUG_PRINT("Sync!\n");
+        synchronized = getFrame(&frame);
+        continue;
+      }
+    }
+  }
+}
+
+
+static void lighthouseInit(DeckInfo *info)
+{
+  uart1Init(115200);
+
+  xTaskCreate(lighthouseTask, "LH",
+              configMINIMAL_STACK_SIZE, NULL, /*priority*/1, NULL);
+}
+
+
+static const DeckDriver lighthouse_deck = {
+  .name = "bcLH8",
+
+  .usedGpio = 0,  // FIXME: set the used pins
+  .requiredEstimator = kalmanEstimator,
+
+  .init = lighthouseInit,
+};
+
+DECK_DRIVER(lighthouse_deck);
+
+LOG_GROUP_START(lighthouse)
+LOG_ADD(LOG_FLOAT, angle0x, &angles[0][0])
+LOG_ADD(LOG_FLOAT, angle0y, &angles[1][0])
+LOG_ADD(LOG_FLOAT, angle1x, &angles[0][1])
+LOG_ADD(LOG_FLOAT, angle1y, &angles[1][1])
+LOG_ADD(LOG_FLOAT, x, &position[0])
+LOG_ADD(LOG_FLOAT, y, &position[1])
+LOG_ADD(LOG_FLOAT, z, &position[2])
+LOG_GROUP_STOP(lighthouse)

src/modules/interface/system.h

@@ -27,6 +27,7 @@
 #ifndef __SYSTEM_H__
 #define __SYSTEM_H__
 
+#include <stdbool.h>
 #include <stdint.h>
 
 void systemInit(void);

src/utils/interface/lighthouse/lighthouseGeometry.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <stdbool.h>
+
+// Naive 3d vector type.
+#define vec3d_size 3
+typedef float vec3d[vec3d_size];
+
+typedef struct baseStationGeometry_s {
+  float mat[3][3];
+  float origin[3];
+} baseStationGeometry_t;
+
+bool lhgeometryGetPosition(baseStationGeometry_t baseStations[2], float angles[4], vec3d position, float *position_delta);

src/utils/interface/lighthouse/pulseProcessor.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define TIMESTAMP_BITWIDTH 29
+
+
+
+typedef struct pulseProcessor_s {
+  int currentFrame;
+  uint32_t lastSync;
+  uint32_t currentSync;
+  int currentBs;
+  int currentAxis;
+} pulseProcessor_t;
+
+// If returns true, the angle, base station and direction are written
+bool processPulse(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, float *angle, int *baseStation, int *axis);

src/utils/src/lighthouse/lighthouseGeometry.c

@@ -0,0 +1,129 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Code addapted from ashtuchkin's Vive DIY position sensor
+ * see https://github.com/ashtuchkin/vive-diy-position-sensor/
+ * 
+ * Copyright (c) 2016 Alexander Shtuchkin
+ * Copyright (C) 2018 Bitcraze AB
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * lighthouseGeometry.c: lighthouse tracking system geometry functions
+ */
+
+#include "lighthouseGeometry.h"
+
+#include <arm_math.h>
+
+static void vec_cross_product(const vec3d a, const vec3d b, vec3d res) {
+    res[0] = a[1]*b[2] - a[2]*b[1];
+    res[1] = a[2]*b[0] - a[0]*b[2];
+    res[2] = a[0]*b[1] - a[1]*b[0];
+}
+
+static float vec_length(vec3d vec) {
+    float pow, res;
+
+    arm_power_f32(vec, vec3d_size, &pow); // returns sum of squares
+    arm_sqrt_f32(pow, &res);
+
+    return res;
+}
+
+static void calc_ray_vec(baseStationGeometry_t *bs, float angle1, float angle2, vec3d res, vec3d origin) {
+    vec3d a = {arm_cos_f32(angle1), 0, -arm_sin_f32(angle1)};  // Normal vector to X plane
+    vec3d b = {0, arm_cos_f32(angle2), arm_sin_f32(angle2)};   // Normal vector to Y plane
+
+    vec3d ray = {};
+    vec_cross_product(b, a, ray); // Intersection of two planes -> ray vector.
+    float len = vec_length(ray);
+    arm_scale_f32(ray, 1/len, ray, vec3d_size); // Normalize ray length.
+
+    arm_matrix_instance_f32 source_rotation_matrix = {3, 3, (float32_t *)bs->mat};
+    arm_matrix_instance_f32 ray_vec = {3, 1, ray};
+    arm_matrix_instance_f32 ray_rotated_vec = {3, 1, res};
+    arm_mat_mult_f32(&source_rotation_matrix, &ray_vec, &ray_rotated_vec);
+
+    // TODO: Make geometry adjustments within base station.
+    vec3d rotated_origin_delta = {};
+    //vec3d base_origin_delta = {-0.025f, -0.025f, 0.f};  // Rotors are slightly off center in base station.
+    // arm_matrix_instance_f32 origin_vec = {3, 1, base_origin_delta};
+    // arm_matrix_instance_f32 origin_rotated_vec = {3, 1, rotated_origin_delta};
+    // arm_mat_mult_f32(&source_rotation_matrix, &origin_vec, &origin_rotated_vec);
+    arm_add_f32(bs->origin, rotated_origin_delta, origin, vec3d_size);
+}
+
+
+static bool intersect_lines(vec3d orig1, vec3d vec1, vec3d orig2, vec3d vec2, vec3d res, float *dist) {
+    // Algoritm: http://geomalgorithms.com/a07-_distance.html#Distance-between-Lines
+
+    vec3d w0 = {};
+    arm_sub_f32(orig1, orig2, w0, vec3d_size);
+
+    float a, b, c, d, e;
+    arm_dot_prod_f32(vec1, vec1, vec3d_size, &a);
+    arm_dot_prod_f32(vec1, vec2, vec3d_size, &b);
+    arm_dot_prod_f32(vec2, vec2, vec3d_size, &c);
+    arm_dot_prod_f32(vec1, w0, vec3d_size, &d);
+    arm_dot_prod_f32(vec2, w0, vec3d_size, &e);
+
+    float denom = a * c - b * b;
+    if (fabsf(denom) < 1e-5f)
+        return false;
+
+    // Closest point to 2nd line on 1st line
+    float t1 = (b * e - c * d) / denom;
+    vec3d pt1 = {};
+    arm_scale_f32(vec1, t1, pt1, vec3d_size);
+    arm_add_f32(pt1, orig1, pt1, vec3d_size);
+
+    // Closest point to 1st line on 2nd line
+    float t2 = (a * e - b * d) / denom;
+    vec3d pt2 = {};
+    arm_scale_f32(vec2, t2, pt2, vec3d_size);
+    arm_add_f32(pt2, orig2, pt2, vec3d_size);
+
+    // Result is in the middle
+    vec3d tmp = {};
+    arm_add_f32(pt1, pt2, tmp, vec3d_size);
+    arm_scale_f32(tmp, 0.5f, res, vec3d_size);
+
+    // Dist is distance between pt1 and pt2
+    arm_sub_f32(pt1, pt2, tmp, vec3d_size);
+    *dist = vec_length(tmp);
+
+    return true;
+}
+
+bool lhgeometryGetPosition(baseStationGeometry_t baseStations[2], float angles[4], vec3d position, float *position_delta)
+{
+  static vec3d ray1, ray2, origin1, origin2;
+
+  calc_ray_vec(&baseStations[0], angles[0], angles[1], ray1, origin1);
+  calc_ray_vec(&baseStations[1], angles[2], angles[3], ray2, origin2);
+
+  return intersect_lines(origin1, ray1, origin2, ray2, position, position_delta);
+}