bsp/lh2: backport implementation updates from lh2_on_rp2040 repository

bsp/bsp.emProject

@@ -29,6 +29,7 @@
       project_directory="."
       project_type="Library" />
     <file file_name="nrf/lh2.c" />
+    <file file_name="nrf/lh2_decoder.c" />
     <file file_name="lh2.h" />
   </project>
   <project Name="00bsp_dotbot_rpm">

bsp/lh2.h

@@ -23,7 +23,7 @@
 
 //=========================== defines ==========================================
 
-#define LH2_BASESTATION_COUNT 4                          ///< Number of supported concurrent basestations
+#define LH2_BASESTATION_COUNT 16                         ///< Number of supported concurrent basestations
 #define LH2_POLYNOMIAL_COUNT  LH2_BASESTATION_COUNT * 2  ///< Number of supported LFSR polynomials, two per basestation
 #define LH2_SWEEP_COUNT       2                          ///< Number of laser sweeps per basestations rotation
 

bsp/lh2_decoder.h

@@ -0,0 +1,92 @@
+#ifndef __LH2_DECODER_H_
+#define __LH2_DECODER_H_
+
+/**
+ * @defgroup    bsp_lh2_decoder LightHouse 2 decoder algorithms
+ * @ingroup     bsp_lh2
+ * @brief       Algorithm to decode the lighthouse pulses, no hardware dependencies.
+ *
+ * @{
+ * @file
+ * @author Said Alvarado-Marin <said-alexander.alvarado-marin@inria.fr>
+ * @copyright Inria, 2025-present
+ * @}
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "lh2.h"
+
+//=========================== defines ==========================================
+
+#define LH2_POLYNOMIAL_ERROR_INDICATOR  0xFF        ///< indicate the polynomial index is invalid
+#define LH2_LFSR_SEARCH_ERROR_INDICATOR 0xFFFFFFFF  ///< indicate the polynomial index is invalid
+// Un-comment the following line if you want to enable the Anti-Mocap fiter
+#define LH2_MOCAP_FILTER 1  ///< Defined when the LH2 needs to coexits with a Qualysis Mocap system. It enables harsher anti-outlier filters
+
+// Dynamic checkpoints for the lsfr index search
+typedef struct {
+    uint32_t bits[LH2_POLYNOMIAL_COUNT][LH2_SWEEP_COUNT];   ///< lfsr pseudo-random bits of the checkpoints
+    uint32_t count[LH2_POLYNOMIAL_COUNT][LH2_SWEEP_COUNT];  ///< corresponding lfsr index of the checkpoints
+} _lfsr_checkpoint_t;
+
+//=========================== public ===========================================
+/**
+ * @brief
+ * @param[in] sample_buffer: SPI samples loaded into a local buffer
+ * @return chipsH: 64-bits of demodulated data
+ */
+uint64_t _demodulate_light(uint8_t *sample_buffer);
+
+/**
+ * @brief from a 17-bit sequence and a polynomial, generate up to 64-17=47 bits as if the LFSR specified by poly were run forwards for numbits cycles, all little endian
+ *
+ * @param[in] poly:     17-bit polynomial
+ * @param[in] bits:     starting seed
+ * @param[in] numbits:  number of bits
+ *
+ * @return sequence of bits resulting from running the LFSR forward
+ */
+uint64_t _poly_check(uint32_t poly, uint32_t bits, uint8_t numbits);
+
+/**
+ * @brief find out which LFSR polynomial the bit sequence is a member of
+ *
+ * @param[in] chipsH1: input sequences of bits from demodulation
+ * @param[in] start_val: number of bits between the envelope falling edge and the beginning of the sequence where valid data has been found
+ *
+ * @return polynomial, indicating which polynomial was found, or FF for error (polynomial not found).
+ */
+uint8_t _determine_polynomial(uint64_t chipsH1, int8_t *start_val);
+
+/**
+ * @brief counts the number of 1s in a 64-bit
+ *
+ * @param[in] bits_in: arbitrary bits
+ *
+ * @return cumulative number of 1s inside of bits_in
+ */
+uint64_t _hamming_weight(uint64_t bits_in);
+
+/**
+ * @brief finds the position of a 17-bit sequence (bits) in the sequence generated by polynomial3 with initial seed 1
+ *
+ * @param[in] checkpoints: structure with dynamic checkpoints
+ * @param[in] index: index of polynomial
+ * @param[in] bits: 17-bit sequence
+ *
+ * @return count: location of the sequence
+ */
+uint32_t _lfsr_index_search(_lfsr_checkpoint_t *checkpoints, uint8_t index, uint32_t bits);
+
+/**
+ * @brief checks an SPI capture for signs of Qualysis Mocap pulses interference.
+ *        returns true if interference is found, returns false otherwise
+ *
+ * @param[in] arr: pointer to the array with the SPI capture to check
+ * @param[in] size: size of the buffer array to check
+ * @return True if interference is found, False otherwise
+ */
+bool _check_mocap_interference(uint8_t *arr, uint8_t size);
+
+#endif /* __LH2_DECODER_H_ */