Merge pull request #807 from jdavidberger/master

Added optional transmit channel for raw lighthouse data

Makefile

@@ -263,7 +263,7 @@ PROJ_OBJ += filter.o cpuid.o cfassert.o  eprintf.o crc32.o num.o debug.o
 PROJ_OBJ += version.o FreeRTOS-openocd.o
 PROJ_OBJ += configblockeeprom.o
 PROJ_OBJ += sleepus.o statsCnt.o rateSupervisor.o
-PROJ_OBJ += lighthouse_core.o pulse_processor.o pulse_processor_v1.o pulse_processor_v2.o lighthouse_geometry.o ootx_decoder.o lighthouse_calibration.o lighthouse_deck_flasher.o lighthouse_position_est.o lighthouse_storage.o
+PROJ_OBJ += lighthouse_core.o pulse_processor.o pulse_processor_v1.o pulse_processor_v2.o lighthouse_geometry.o ootx_decoder.o lighthouse_calibration.o lighthouse_deck_flasher.o lighthouse_position_est.o lighthouse_storage.o lighthouse_transmit.o
 PROJ_OBJ += kve_storage.o kve.o
 
 ifeq ($(DEBUG_PRINT_ON_SEGGER_RTT), 1)

src/modules/interface/lighthouse/lighthouse_transmit.h

@@ -0,0 +1,30 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2019 - 2020 Bitcraze AB
+ *
+ * This program 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, in version 3.
+ *
+ * This program 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/>.
+ *
+ *
+ * lighthouse_transmit.h - Transmit raw lighthouse data over radio
+ */
+#include "lighthouse_core.h"
+
+void lighthouseTransmitProcessFrame(const lighthouseUartFrame_t* frame);
+void lighthouseTransmitProcessTimeout();

src/modules/src/lighthouse/lighthouse_core.c

@@ -56,6 +56,8 @@
 #include "test_support.h"
 #include "static_mem.h"
 
+#include "lighthouse_transmit.h"
+
 static const uint32_t MAX_WAIT_TIME_FOR_HEALTH_MS = 4000;
 
 static pulseProcessorResult_t angles;
@@ -206,7 +208,9 @@ TESTABLE_STATIC bool getUartFrameRaw(lighthouseUartFrame_t *frame) {
   int syncCounter = 0;
 
   for(int i = 0; i < UART_FRAME_LENGTH; i++) {
-    uart1Getchar(&data[i]);
+    while(!uart1GetDataWithTimeout((uint8_t*)&data[i], 2)) {
+      lighthouseTransmitProcessTimeout();      
+    }
     if ((unsigned char)data[i] == 0xff) {
       syncCounter += 1;
     }
@@ -482,7 +486,8 @@ void lighthouseCoreTask(void *param) {
       // Now we are receiving items
       else if(!frame.isSyncFrame) {
         STATS_CNT_RATE_EVENT(&frameRate);
-
+	lighthouseTransmitProcessFrame(&frame);
+
         deckHealthCheck(&lighthouseCoreState, &frame, now_ms);
         lighthouseUpdateSystemType();
         if (pulseProcessorProcessPulse) {

src/modules/src/lighthouse/lighthouse_transmit.c

@@ -0,0 +1,178 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2019 - 2020 Bitcraze AB
+ *
+ * This program 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, in version 3.
+ *
+ * This program 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/>.
+ *
+ *
+ * lighthouse_transmit.c - Transmit raw lighthouse data over radio
+ */
+#include "crtp.h"
+#include "lighthouse_transmit.h"
+#include "param.h"
+
+#define DEBUG_MODULE "LH"
+#include "debug.h"
+
+typedef struct lighthouseTransmitState {
+  uint8_t enabled;
+
+  CRTPPacket txPacket;  
+  uint8_t queuedPackets;
+
+  uint32_t last_timestamp;
+  uint32_t ref_timestamp;
+  uint32_t sensor_timestamps[4];
+  uint32_t offset;
+  int8_t channel;
+  bool slowbit;
+  int8_t sensor_flags;
+
+} lighthouseTransmitState;
+
+static lighthouseTransmitState gState = {
+					 .enabled = 0,
+	 .channel = -1,
+	 .txPacket = {
+		      .port = 6,
+		      .channel = 3
+         }
+};
+
+static void lighthouseResetCurrent(lighthouseTransmitState* state) {
+  state->channel = -1;
+  state->sensor_flags = 0;
+  state->offset = 0;
+  state->ref_timestamp = 0;
+}
+
+static void lighthouseTransmit(lighthouseTransmitState* state) {
+  if(state->txPacket.size) {
+    crtpSendPacket(&state->txPacket);
+    state->txPacket.size = 0;
+    state->queuedPackets = 0;
+  }
+}
+
+static inline int32_t timediff_24bit(int32_t first_ts, int32_t second_ts) {
+  int32_t diff = second_ts - first_ts;
+  if(abs(diff) > 1 << 23) {
+    diff -= 1 << 24;
+  }
+  return diff; 
+}
+
+static void lighthouseQueue(lighthouseTransmitState* state) {
+  if(state->channel == -1) return;
+
+  // Basic strategy:
+  // 3     Bytes: 0bCCCC 00OY 0xYY 0xYY - Channel, OOTX bit, 17 bits of sync data
+  // 3     Bytes: 0xTT TT TT - 24mhz Timestamp of first packet
+  // 2 * 4 Bytes: 0xOO OO - 2 Byte offset from timestamp per sensor
+  // 14 bytes; so we do two at a time
+  int offset = state->queuedPackets * 14;
+  uint8_t* data = &state->txPacket.data[offset];
+  uint32_t ref_timestamp = state->ref_timestamp;
+
+  uint32_t channel_ootx_sync =
+    ((uint32_t)state->channel << 20) |
+    (state->slowbit << 17) |
+    (state->offset / 4);
+
+  memcpy(data, &channel_ootx_sync, 3);
+  memcpy(&data[3], ((uint8_t*)&state->ref_timestamp), 3);
+
+  if(state->offset > state->ref_timestamp) {
+    state->offset -= (1 << 24);
+  }
+
+  for(int i = 0;i < 4;i++) {
+    int16_t delta16 = 0x7FFF;
+    if(state->sensor_flags & (1 << i)) {
+      int32_t ts = state->sensor_timestamps[i];
+
+      int32_t delta = ts - ref_timestamp;
+      if(abs(delta) < 0x7FFF) {
+	delta16 = delta;
+      }
+    }
+    memcpy(&data[6 + 2 * i], &delta16, 2);
+  }
+
+  state->txPacket.size += 14; 
+  lighthouseResetCurrent(state);
+  if(state->queuedPackets) {
+    lighthouseTransmit(state);
+  } else {
+    state->queuedPackets = 1;
+  }
+}
+
+void lighthouseTransmitProcessFrame(const lighthouseUartFrame_t* frame) {
+  lighthouseTransmitState* state = &gState;
+  if(!state->enabled) {
+    return;
+  }
+
+  int32_t timediff = 0;
+  if(frame->data.timestamp > state->ref_timestamp) timediff = frame->data.timestamp - state->last_timestamp;
+  else if(frame->data.timestamp > state->ref_timestamp) timediff = state->last_timestamp - frame->data.timestamp;
+  state->last_timestamp = frame->data.timestamp;
+
+  if(state->channel != -1 && timediff > 0xFFFF) {
+    lighthouseQueue(state);    
+  }
+
+  uint32_t ts_mid = frame->data.timestamp + (frame->data.width / 2);
+  if(frame->data.channelFound) {   
+    if(state->channel == -1) {
+      state->channel = frame->data.channel;
+      state->slowbit = frame->data.slowbit;
+      state->ref_timestamp = frame->data.timestamp;
+    } else if (state->channel != frame->data.channel) {
+      lighthouseQueue(state);
+      state->channel = frame->data.channel;
+      state->ref_timestamp = frame->data.timestamp;
+    }
+  }
+
+  if(frame->data.offset) {
+    state->offset = frame->data.offset;
+    state->ref_timestamp = frame->data.timestamp;
+  }
+
+  state->sensor_timestamps[frame->data.sensor] = ts_mid;
+  state->sensor_flags |= 1 << frame->data.sensor;
+  if(state->sensor_flags == 0xf) {
+    lighthouseQueue(state);
+  }
+
+}
+void lighthouseTransmitProcessTimeout() {
+  lighthouseTransmitState* state = &gState;
+  if(!state->enabled) {
+    return;
+  }
+  lighthouseQueue(state);
+  lighthouseTransmit(state);
+}
+PARAM_GROUP_START(lighthouse)
+  PARAM_ADD(PARAM_UINT8, enLhRawStream, &gState.enabled)
+PARAM_GROUP_STOP(lighthouse)

test/modules/src/lighthouse/test_lighthouse_core.c

@@ -6,6 +6,7 @@
 #include "mock_pulse_processor.h"
 #include "mock_pulse_processor_v1.h"
 #include "mock_pulse_processor_v2.h"
+#include "mock_lighthouse_transmit.h"
 #include "mock_lighthouse_deck_flasher.h"
 #include "mock_lighthouse_position_est.h"
 #include "mock_lighthouse_calibration.h"
@@ -247,7 +248,6 @@ void testThatSlowBitIsDecodedInUartFrame() {
 }
 
 // Test support ----------------------------------------------------------------------------------------------------
-
 static void uart1ReadCallback(char* ch, int cmock_num_calls) {
     if (uart1BytesRead >= uart1SequenceLength) {
         TEST_FAIL_MESSAGE("Too many bytes read from uart1");
@@ -257,10 +257,16 @@ static void uart1ReadCallback(char* ch, int cmock_num_calls) {
     uart1BytesRead++;
 }
 
+static bool uart1GetDataWithTimeoutCallback(char* ch, const uint32_t timeoutTicks, int cmock_num_calls) {
+    uart1ReadCallback(ch, cmock_num_calls);
+    return true;
+}
+
 static void uart1SetSequence(char* sequence, int length) {
     uart1BytesRead = 0;
     uart1Sequence = sequence;
     uart1SequenceLength = length;
 
     uart1Getchar_StubWithCallback(uart1ReadCallback);
+    uart1GetDataWithTimeout_StubWithCallback(uart1GetDataWithTimeoutCallback);
 }