diff --git a/conf/airframes/tudelft/splash3.xml b/conf/airframes/tudelft/splash3.xml index 6573915535..09c7e28500 100644 --- a/conf/airframes/tudelft/splash3.xml +++ b/conf/airframes/tudelft/splash3.xml @@ -59,6 +59,7 @@ + diff --git a/conf/modules/lidar_tfmini.xml b/conf/modules/lidar_tfmini.xml new file mode 100644 index 0000000000..0279febee3 --- /dev/null +++ b/conf/modules/lidar_tfmini.xml @@ -0,0 +1,49 @@ + + + + + + TFMini Lidar using a single UART for communication + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml index bb830f7ac7..4fea279964 100644 --- a/conf/telemetry/default_rotorcraft.xml +++ b/conf/telemetry/default_rotorcraft.xml @@ -33,6 +33,7 @@ + @@ -195,6 +196,7 @@ + diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 11feef2f5b..bed86e05a6 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/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" /> + * + * 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; + } +} diff --git a/sw/airborne/modules/lidar/tfmini.h b/sw/airborne/modules/lidar/tfmini.h new file mode 100644 index 0000000000..ddc08461d8 --- /dev/null +++ b/sw/airborne/modules/lidar/tfmini.h @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2019 Freek van Tienen + * + * 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 */ + diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index beef38c221..6f8c62db51 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/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) */