Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 05:45:59 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

[modules] Add TFMini lidar (#2395)

Browse Source
This commit is contained in:
Freek van Tienen
2019-02-27 13:32:02 +01:00
committed by Gautier Hattenberger
parent 33e02c9f1e
commit 27c12a5dc6
7 changed files with 298 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/splash3.xml
+1
View File
@@ -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
+49
View File
@@ -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
+2
View File
@@ -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
+1 -1
View File
@@ -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
+173
View File
@@ -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
+68
View File
@@ -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
+4
View File
@@ -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)
*/