[modules] Add TFMini lidar (#2395)

conf/airframes/tudelft/splash3.xml

@@ -59,6 +59,7 @@
     <module name="logger" type="sd_chibios"/>
     <module name="flight_recorder"/>
 
+    <module name="lidar" type="tfmini"/>
     <module name="air_data"/>
     <module name="send_imu_mag_current"/>
   </firmware>

conf/modules/lidar_tfmini.xml

@@ -0,0 +1,49 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="lidar_tfmini" dir="lidar">
+  <doc>
+    <description>
+      TFMini Lidar using a single UART for communication
+    </description>
+    <configure name="TFMINI_PORT" value="UART3" description="UART device to use for lidar"/>
+    <configure name="TFMINI_BAUD" value="B115200" description="baudrate of the TFMini UART port"/>
+    <configure name="USE_TFMINI_AGL" value="0" description="use this lidar for AGL measurements"/>
+    <configure name="TFMINI_COMPENSATE_ROTATION" value="1" description="compensate AGL measurements for body rotation"/>
+  </doc>
+
+  <settings>
+    <dl_settings NAME="Lidar TFMini">
+      <dl_settings NAME="Lidar">
+        <dl_setting MAX="1" MIN="0" STEP="1" VAR="tfmini.compensate_rotation" shortname="derotate_agl"/>
+        <dl_setting MAX="1" MIN="0" STEP="1" VAR="tfmini.update_agl" shortname="update_agl"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
+  <header>
+    <file name="tfmini.h"/>
+  </header>
+  <init fun="tfmini_init()"/>
+  <event fun="tfmini_event()"/>
+  <makefile>
+    <!-- Configure default UART port and baudrate -->
+    <configure name="TFMINI_PORT" default="UART3" case="upper|lower"/>
+    <configure name="TFMINI_BAUD" default="B115200"/>
+
+    <!-- Enable UART and set baudrate -->
+    <define name="USE_$(TFMINI_PORT_UPPER)"/>
+    <define name="$(TFMINI_PORT_UPPER)_BAUD" value="$(TFMINI_BAUD)"/>
+    <define name="TFMINI_PORT" value="$(TFMINI_PORT_LOWER)"/>
+
+    <configure name="USE_TFMINI_AGL" default="0"/>
+    <configure name="TFMINI_COMPENSATE_ROTATION" default="0"/>
+    <define name="USE_TFMINI_AGL" value="$(USE_TFMINI_AGL)"/>
+    <define name="TFMINI_COMPENSATE_ROTATION" value="$(TFMINI_COMPENSATE_ROTATION)"/>
+    <file name="tfmini.c"/>
+  </makefile>
+  <makefile target="nps">
+    <define name="USE_SONAR" value="1"/><!-- in NPS use a virtual sonar to simulate lidar measurements -->
+  </makefile>
+</module>
+
+

conf/telemetry/default_rotorcraft.xml

@@ -33,6 +33,7 @@
       <message name="DIVERGENCE"               period="0.05"/>
       <message name="DRAGSPEED"                period="0.02"/>
       <message name="LOGGER_STATUS"            period="5.1"/>
+      <message name="LIDAR"                    period="1.2"/>
     </mode>
 
     <mode name="ppm">
@@ -195,6 +196,7 @@
       <message name="IMU_GYRO_SCALED"          period=".075"/>
       <message name="IMU_ACCEL_SCALED"         period=".075"/>
       <message name="IMU_MAG_SCALED"           period=".2"/>
+      <message name="LIDAR"                    period="0.05"/>
     </mode>
   </process>
 

conf/userconf/tudelft/conf.xml

@@ -371,7 +371,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/ahrs_float_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
+   settings_modules="modules/air_data.xml modules/lidar_tfmini.xml modules/ahrs_float_cmpl_quat.xml modules/stabilization_rate.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
    gui_color="blue"
   />
   <aircraft

sw/airborne/modules/lidar/tfmini.c

@@ -0,0 +1,173 @@
+/*
+ * Copyright (C) 2019 Freek van Tienen <freek.v.tienen@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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file modules/lidar/tfmini.c
+ *  @brief driver for the TFMini lidar
+ *
+ */
+#include "tfmini.h"
+#include "mcu_periph/uart.h"
+#include "subsystems/abi.h"
+
+// State interface for rotation compensation
+#include "state.h"
+
+// Messages
+#include "pprzlink/messages.h"
+#include "subsystems/datalink/downlink.h"
+
+struct TFMini tfmini = {
+  .parse_status = TFMINI_INITIALIZE
+};
+
+static void tfmini_parse(uint8_t byte);
+
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+
+/**
+ * Downlink message lidar
+ */
+static void tfmini_send_lidar(struct transport_tx *trans, struct link_device *dev)
+{
+  pprz_msg_send_LIDAR(trans, dev, AC_ID,
+                      &tfmini.distance,
+                      &tfmini.mode,
+                      &tfmini.parse_status);
+}
+
+#endif
+
+/**
+ * Initialization function
+ */
+void tfmini_init(void)
+{
+  tfmini.device = &((TFMINI_PORT).device);
+
+  tfmini.update_agl = USE_TFMINI_AGL;
+  tfmini.compensate_rotation = TFMINI_COMPENSATE_ROTATION;
+
+  tfmini.strength = 0;
+  tfmini.distance = 0;
+  tfmini.parse_status = TFMINI_PARSE_HEAD;
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_LIDAR, tfmini_send_lidar);
+#endif
+}
+
+/**
+ * Lidar event function
+ * Receive bytes from the UART port and parse them
+ */
+void tfmini_event(void)
+{
+  while (tfmini.parse_status != TFMINI_INITIALIZE && tfmini.device->char_available(tfmini.device->periph)) {
+    tfmini_parse(tfmini.device->get_byte(tfmini.device->periph));
+  }
+}
+
+/**
+ * Parse the lidar bytes 1 by 1
+ */
+static void tfmini_parse(uint8_t byte)
+{
+  switch (tfmini.parse_status) {
+    case TFMINI_INITIALIZE:
+      break;
+    case TFMINI_PARSE_HEAD:
+      if (byte == 0x59) {
+        tfmini.parse_crc = byte;
+        tfmini.parse_status++;
+      }
+      break;
+    case TFMINI_PARSE_HEAD2:
+      if (byte == 0x59) {
+        tfmini.parse_crc += byte;
+        tfmini.parse_status++;
+      } else {
+        tfmini.parse_status = TFMINI_PARSE_HEAD;
+      }
+      break;
+
+    case TFMINI_PARSE_DIST_L:
+      tfmini.raw_dist = byte;
+      tfmini.parse_crc += byte;
+      tfmini.parse_status++;
+      break;
+    case TFMINI_PARSE_DIST_H:
+      tfmini.raw_dist |= (byte << 8);
+      tfmini.parse_crc += byte;
+      tfmini.parse_status++;
+      break;
+
+    case TFMINI_PARSE_STRENGTH_L:
+      tfmini.raw_strength = byte;
+      tfmini.parse_crc += byte;
+      tfmini.parse_status++;
+      break;
+    case TFMINI_PARSE_STRENGTH_H:
+      tfmini.raw_strength |= (byte << 8);
+      tfmini.parse_crc += byte;
+      tfmini.parse_status++;
+      break;
+
+    case TFMINI_PARSE_MODE:
+      tfmini.raw_mode = byte;
+      tfmini.parse_crc += byte;
+      tfmini.parse_status++;
+      break;
+    case TFMINI_PARSE_BYTE7:
+      tfmini.parse_crc += byte;
+      tfmini.parse_status++;
+      break;
+
+    case TFMINI_PARSE_CHECKSUM:
+      // When the CRC matches
+      if (tfmini.parse_crc == byte) {
+        tfmini.distance = tfmini.raw_dist / 100.f;
+        tfmini.strength = tfmini.raw_strength;
+        tfmini.mode = tfmini.raw_mode;
+
+        // When the distance is valid
+        if (tfmini.distance != 0xFFFF) {
+          // compensate AGL measurement for body rotation
+          if (tfmini.compensate_rotation) {
+            float phi = stateGetNedToBodyEulers_f()->phi;
+            float theta = stateGetNedToBodyEulers_f()->theta;
+            float gain = (float)fabs((double)(cosf(phi) * cosf(theta)));
+            tfmini.distance = tfmini.distance / gain;
+          }
+
+          // send message (if requested)
+          if (tfmini.update_agl) {
+            AbiSendMsgAGL(AGL_LIDAR_TFMINI_ID, tfmini.distance);
+          }
+        }
+      }
+
+      // Start reading again
+      tfmini.parse_status = TFMINI_PARSE_HEAD;
+      break;
+  }
+}

sw/airborne/modules/lidar/tfmini.h

@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2019 Freek van Tienen <freek.v.tienen@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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file modules/lidar/tfmini.h
+ *  @brief driver for the TFMini lidar
+ *
+ */
+#ifndef LIDAR_TFMINI_H
+#define LIDAR_TFMINI_H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+
+enum TFMiniParseStatus {
+  TFMINI_INITIALIZE,
+  TFMINI_PARSE_HEAD,
+  TFMINI_PARSE_HEAD2,
+  TFMINI_PARSE_DIST_L,
+  TFMINI_PARSE_DIST_H,
+  TFMINI_PARSE_STRENGTH_L,
+  TFMINI_PARSE_STRENGTH_H,
+  TFMINI_PARSE_MODE,
+  TFMINI_PARSE_BYTE7,
+  TFMINI_PARSE_CHECKSUM
+};
+
+struct TFMini {
+  struct link_device *device;
+  enum TFMiniParseStatus parse_status;
+  uint8_t parse_crc;
+  uint16_t raw_dist;
+  uint16_t raw_strength;
+  uint8_t raw_mode;
+
+  uint16_t strength;
+  float distance; // [m]
+  uint8_t mode;
+  bool update_agl;
+  bool compensate_rotation;
+};
+
+extern struct TFMini tfmini;
+
+extern void tfmini_init(void);
+extern void tfmini_event(void);
+extern void tfmini_downlink(void);
+
+#endif /* LIDAR_TFMINI_H */
+

sw/airborne/subsystems/abi_sender_ids.h

@@ -157,6 +157,10 @@
 #define AGL_RAY_SENSOR_GAZEBO_ID 10
 #endif
 
+#ifndef AGL_LIDAR_TFMINI_ID
+#define AGL_LIDAR_TFMINI_ID 11
+#endif
+
 /*
  * IDs of magnetometer sensors (including IMUs with mag)
  */