Add vl53l5cx driver (#3240)

Based on St ULD
paparazzi · Feb 29, 2024 · ae5f47a · ae5f47a
1 parent edff4bc
commit ae5f47a
Show file tree

Hide file tree

Showing 13 changed files with 24,608 additions and 22 deletions.
diff --git a/conf/abi.xml b/conf/abi.xml
@@ -248,6 +248,15 @@
       <field name="vel_sp" type="struct FloatVect3 *" unit="m/s"/>
     </message>
 
+    <message name="LIDAR_DATA" id="37">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="numRows" type="uint32_t">number of rows of the matrix</field>
+      <field name="numCols" type="uint32_t">number of columns of the matrix</field>
+      <field name="size" type="uint16_t">size of a matrix element</field>
+      <field name="subtype" type="uint8_t">subtype of data, driver dependent (e.g. distance, reflectance, ...)</field>
+      <field name="data" type="uint8_t*">pointer to the data of the matrix</field>
+    </message>
+
   </msg_class>
 
 </protocol>
diff --git a/conf/modules/lidar_vl53l5cx.xml b/conf/modules/lidar_vl53l5cx.xml
@@ -0,0 +1,29 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+<module name="lidar_vl53l5cx" dir="lidar" task="sensors">
+  <doc>
+    <description>
+      VL53L5CX multizone range sensor.
+      Sends LIDAR_DATA Abi messages.
+    </description>
+    <configure name="LIDAR_VL53L5CX_I2C_DEV" value="i2c2" description="I2C device to use for the VL53L5CX sensor"/>
+    <define name="LIDAR_VL53L5CX_I2C_ADDR" value="0x52" description="I2C address"/>
+  </doc>
+  <dep>
+    <depends>i2c</depends>
+  </dep>
+  <header>
+    <file name="lidar_vl53l5cx.h"/>
+  </header>
+  <init fun="lidar_vl53l5cx_init()"/>
+  <periodic fun="lidar_vl53l5cx_periodic()" freq="5.0"/>
+  <makefile>
+    <configure name="LIDAR_VL53L5CX_I2C_DEV" default="i2c2" case="lower|upper"/>
+    <define name="LIDAR_VL53L5CX_I2C_DEV" value="$(LIDAR_VL53L5CX_I2C_DEV_LOWER)"/>
+    <define name="USE_$(LIDAR_VL53L5CX_I2C_DEV_UPPER)"/>
+
+    <file_arch name="lidar_vl53l5cx.c"/>
+    <file name="vl53l5cx_api.c" dir="peripherals"/>
+    <file_arch name="vl53l5cx_platform.c"/>
+    <test arch="chibios"/>
+  </makefile>
+</module>
diff --git a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c b/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c
@@ -123,6 +123,8 @@ static void handle_i2c_thd(struct i2c_periph *p)
   // wait for a transaction to be pushed in the queue
   chSemWait(&i->sem);
 
+  i2cAcquireBus((I2CDriver *)p->reg_addr);
+
   if (p->trans_insert_idx == p->trans_extract_idx) {
     p->status = I2CIdle;
     // no transaction pending
@@ -231,6 +233,8 @@ static void handle_i2c_thd(struct i2c_periph *p)
     default:
       break;
   }
+
+  i2cReleaseBus((I2CDriver *)p->reg_addr);
 }
 
 /**

diff --git a/sw/airborne/arch/chibios/modules/lidar/lidar_vl53l5cx.c b/sw/airborne/arch/chibios/modules/lidar/lidar_vl53l5cx.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2024 Fabien-B <name.surname@gmail.com>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/lidar/lidar_vl53l5cx.c"
+ * @author Fabien-B <name.surname@gmail.com>
+ * VL53L5CX multizone range sensor.
+ */
+
+#include "modules/lidar/lidar_vl53l5cx.h"
+#include "mcu_periph/i2c.h"
+#include "ch.h"
+#include "lidar/vl53l5cx_platform.h"
+#include "peripherals/vl53l5cx_api.h"
+#include "modules/core/abi.h"
+#include "modules/datalink/downlink.h"
+#include "ch.h"
+#include "hal.h"
+#include "mcu_periph/ram_arch.h"
+
+#ifndef LIDAR_VL53L5CX_I2C_ADDR
+#define LIDAR_VL53L5CX_I2C_ADDR 0x29
+#endif
+
+#define SUBTYPE_DISTANCE 0
+
+static IN_DMA_SECTION(VL53L5CX_Configuration  vl53l5cx_dev);
+static IN_DMA_SECTION(VL53L5CX_ResultsData  vl53l5cx_results);
+
+static THD_WORKING_AREA(wa_thd_lidar_vl53l5cx, 1024);
+static void thd_lidar_vl53l5cx(void *arg);
+
+
+char *VL53L5CX_ERROR_MSGS[] = {
+  "VL53L5CX_NO_ERROR",
+  "VL53L5CX_NOT_DETECTED",
+  "VL53L5CX_ULD_LOADING_FAILED",
+  "VL53L5CX_SET_RESOLUTION_FAILED",
+  "VL53L5CX_RUNTIME_ERROR",
+};
+
+
+void lidar_vl53l5cx_init(void)
+{
+  vl53l5cx_dev.platform.i2cdev = &LIDAR_VL53L5CX_I2C_DEV;
+  vl53l5cx_dev.platform.address = LIDAR_VL53L5CX_I2C_ADDR;
+  vl53l5cx_dev.platform.user_data = NULL;
+  vl53l5cx_dev.platform.error_code = VL53L5CX_NO_ERROR;
+
+  // Create thread
+  vl53l5cx_dev.platform.user_data = chThdCreateStatic(wa_thd_lidar_vl53l5cx, sizeof(wa_thd_lidar_vl53l5cx),
+                                        NORMALPRIO, thd_lidar_vl53l5cx, (void *)&vl53l5cx_dev);
+}
+
+void lidar_vl53l5cx_periodic(void)
+{
+
+  if (vl53l5cx_dev.platform.user_data != NULL) {
+    thread_t* thread_handle = (thread_t*) vl53l5cx_dev.platform.user_data;
+    // check thread status
+    if (thread_handle->state == CH_STATE_FINAL) {
+      vl53l5cx_dev.platform.error_code = (enum VL53L5CX_ERRORS) chThdWait(thread_handle);
+      vl53l5cx_dev.platform.user_data = NULL;
+    }
+  } else {
+
+    // thread exited, send error code periodically
+    size_t len = strlen(VL53L5CX_ERROR_MSGS[vl53l5cx_dev.platform.error_code]);
+    // send exitcode to telemetry
+    RunOnceEvery(10, DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, len,
+                                            VL53L5CX_ERROR_MSGS[vl53l5cx_dev.platform.error_code]));
+    ;
+  }
+
+
+  if (vl53l5cx_dev.platform.data_available) {
+    uint32_t now_ts = get_sys_time_usec();
+    AbiSendMsgLIDAR_DATA(LIDAR_DATA_VL53L5CX_ID, now_ts,
+                         8, 8, sizeof(vl53l5cx_dev.platform.distances_mm[0]), SUBTYPE_DISTANCE,
+                         (uint8_t *)vl53l5cx_dev.platform.distances_mm);
+
+    vl53l5cx_dev.platform.data_available = false;
+  }
+
+
+}
+
+
+
+static void thd_lidar_vl53l5cx(void *arg)
+{
+  chRegSetThreadName("vl53l5cx");
+  VL53L5CX_Configuration *dev = (VL53L5CX_Configuration *) arg;
+
+  uint8_t         status, isAlive, isReady;
+
+
+  chThdSleepMilliseconds(2000);
+
+  status = vl53l5cx_is_alive(dev, &isAlive);
+  if (!isAlive || status) {
+    chThdExit(VL53L5CX_NOT_DETECTED);
+  }
+
+  status = vl53l5cx_init(dev);
+  if (status) {
+    chThdExit(VL53L5CX_ULD_LOADING_FAILED);
+  }
+
+  status = vl53l5cx_set_resolution(dev, VL53L5CX_RESOLUTION_4X4);
+  if (status) {
+    chThdExit(VL53L5CX_SET_RESOLUTION_FAILED);
+  }
+
+  status = vl53l5cx_start_ranging(dev);
+
+  while (true) {
+    status = vl53l5cx_check_data_ready(dev, &isReady);
+    if (isReady) {
+      status = vl53l5cx_get_ranging_data(dev, &vl53l5cx_results);
+      if (status == 0) {
+        memcpy(dev->platform.distances_mm, vl53l5cx_results.distance_mm, 64 * sizeof(vl53l5cx_results.distance_mm[0]));
+        dev->platform.data_available = true;
+      }
+    }
+
+    chThdSleepMilliseconds(100);
+  }
+
+
+}
+
diff --git a/sw/airborne/arch/chibios/modules/lidar/lidar_vl53l5cx.h b/sw/airborne/arch/chibios/modules/lidar/lidar_vl53l5cx.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2024 Fabien-B <name.surname@gmail.com>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/lidar/lidar_vl53l5cx.h"
+ * @author Fabien-B <name.surname@gmail.com>
+ * VL53L5CX multizone range sensor.
+ */
+
+#ifndef LIDAR_VL53L5CX_H
+#define LIDAR_VL53L5CX_H
+
+#include "stdint.h"
+#include "peripherals/vl53l5cx_api.h"
+
+extern void lidar_vl53l5cx_init(void);
+extern void lidar_vl53l5cx_periodic(void);
+
+#endif  // LIDAR_VL53L5CX_H
diff --git a/sw/airborne/arch/chibios/modules/lidar/vl53l5cx_platform.c b/sw/airborne/arch/chibios/modules/lidar/vl53l5cx_platform.c
@@ -0,0 +1,118 @@
+
+/*
+ * Copyright (C) 2024 Fabien-B <name.surname@gmail.com>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi 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 2, or (at your option)
+ * any later version.
+ */
+
+
+#include "lidar/vl53l5cx_platform.h"
+#include "hal.h"
+
+#define VL53L5_I2C_TIMEOUT chTimeMS2I(100)
+
+
+uint8_t RdByte(
+		VL53L5CX_Platform *dev,
+		uint16_t index,
+		uint8_t *p_data)
+{
+	return RdMulti(dev, index, p_data, 1);
+}
+
+uint8_t WrByte(
+		VL53L5CX_Platform *dev,
+		uint16_t index,
+		uint8_t data)
+{
+	return WrMulti(dev, index, &data, 1);
+}
+
+uint8_t WrMulti(
+		VL53L5CX_Platform *dev,
+		uint16_t index,
+		uint8_t *pdata,
+		uint32_t count)
+{
+	I2CDriver * i2cd = (I2CDriver *)dev->i2cdev->reg_addr;
+
+	i2cAcquireBus(i2cd);
+
+	msg_t ret;
+
+	for(size_t offset = 0; offset<count; offset += VL53L5CX_I2C_BUF_SIZE) {
+		uint16_t reg_addr = index + offset;
+		dev->buf[0] = (reg_addr & 0xFF00) >> 8; // MSB first
+		dev->buf[1] = (reg_addr & 0x00FF);
+		size_t size = Min(count-offset, VL53L5CX_I2C_BUF_SIZE);
+		memcpy((uint8_t *) dev->buf + 2, (pdata + offset), size);
+		cacheBufferFlush(dev->buf, size+2);
+		ret = i2cMasterTransmitTimeout(i2cd, dev->address, dev->buf, size+2, NULL, 0, VL53L5_I2C_TIMEOUT);
+	}
+
+
+	i2cReleaseBus(i2cd);
+
+	return ret;
+}
+
+uint8_t RdMulti(
+		VL53L5CX_Platform *dev,
+		uint16_t index,
+		uint8_t *pdata,
+		uint32_t count)
+{
+	I2CDriver * i2cd = (I2CDriver *)dev->i2cdev->reg_addr;
+
+	i2cAcquireBus(i2cd);
+
+	msg_t ret = 0;
+	for(size_t offset = 0; offset<count; offset += VL53L5CX_I2C_BUF_SIZE) {
+		uint16_t reg_addr = index + offset;
+		dev->buf[0] = (reg_addr & 0xFF00) >> 8; // MSB first
+		dev->buf[1] = (reg_addr & 0x00FF);
+		size_t size = Min(count-offset, VL53L5CX_I2C_BUF_SIZE);
+
+		cacheBufferFlush(dev->buf, 2);
+		ret = i2cMasterTransmitTimeout(i2cd, dev->address, dev->buf, 2, dev->buf, size, VL53L5_I2C_TIMEOUT);
+		cacheBufferInvalidate(dev->buf, count);
+		memcpy(pdata+offset, dev->buf, count);
+	}
+
+	i2cReleaseBus(i2cd);
+
+	return ret;
+}
+
+void SwapBuffer(
+		uint8_t 		*buffer,
+		uint16_t 	 	 size)
+{
+	uint32_t i, tmp;
+
+	/* Example of possible implementation using <string.h> */
+	for(i = 0; i < size; i = i + 4) 
+	{
+		tmp = (
+		  buffer[i]<<24)
+		|(buffer[i+1]<<16)
+		|(buffer[i+2]<<8)
+		|(buffer[i+3]);
+
+		memcpy(&(buffer[i]), &tmp, 4);
+	}
+}	
+
+uint8_t WaitMs(
+		VL53L5CX_Platform *p_platform,
+		uint32_t TimeMs)
+{
+	(void)p_platform;
+	chThdSleepMilliseconds(TimeMs);
+	return 0;
+}