| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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> |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,109 @@ | ||
| <aerodynamics> | ||
|
|
||
| <axis name="YAW"> | ||
| <function name="elevon_yaw" frame="BODY" unit="FT*LBS"> | ||
| <sum> | ||
| <product> | ||
| <property>fcs/ele_left_lag</property> | ||
| <value> -0.35 </value> | ||
| </product> | ||
| <product> | ||
| <property>fcs/ele_right_lag</property> | ||
| <value> -.35 </value> | ||
| </product> | ||
| </sum> | ||
| </function> | ||
| </axis> | ||
|
|
||
| <axis name="PITCH"> | ||
| <function name="elevon_pitch" frame="BODY" unit="FT*LBS"> | ||
| <sum> | ||
| <product> | ||
| <property>fcs/ele_left_lag</property> | ||
| <value> -0.0417 </value> | ||
| </product> | ||
| <product> | ||
| <property>fcs/ele_right_lag</property> | ||
| <value> .0417 </value> | ||
| </product> | ||
| </sum> | ||
| </function> | ||
| </axis> | ||
|
|
||
| <axis name="LIFT"> | ||
|
|
||
| <function name="aero/force/Lift_alpha"> | ||
| <description>Lift due to alpha</description> | ||
| <product> | ||
| <property>aero/qbar-psf</property> | ||
| <property>metrics/Sw-sqft</property> | ||
| <table> | ||
| <independentVar lookup="row">aero/alpha-deg</independentVar> | ||
| <tableData> | ||
| -180 0.14 | ||
| -140 -0.80 | ||
| -110 -0.63 | ||
| -102 -0.74 | ||
| -98 -0.64 | ||
| -90 0.0 | ||
| -85 0.15 | ||
| -80 0.25 | ||
| -75 0.42 | ||
| 0 0.0 | ||
| 45 0.8 | ||
| 90 0.0 | ||
| 135 -0.8 | ||
| 180 0.0 | ||
| </tableData> | ||
| </table> | ||
| </product> | ||
| </function> | ||
|
|
||
| </axis> | ||
|
|
||
| <axis name="DRAG"> | ||
|
|
||
| <function name="aero/force/Drag_basic"> | ||
| <description>Drag</description> | ||
| <product> | ||
| <property>aero/qbar-psf</property> | ||
| <property>metrics/Sw-sqft</property> | ||
| <table> | ||
| <independentVar lookup="row">aero/alpha-deg</independentVar> | ||
| <tableData> | ||
| -180 1.7 | ||
| -160 1.62 | ||
| -100 0.4 | ||
| -90 0.15 | ||
| -85 0.17 | ||
| -75 0.22 | ||
| -70 0.33 | ||
| -65 0.6128 | ||
| -20 1.36 | ||
| -4 1.45 | ||
| 15 1.35 | ||
| 30 1.12 | ||
| </tableData> | ||
| </table> | ||
| <value>2.0</value> <!-- FIXME why this factor 2 ? --> | ||
| </product> | ||
| </function> | ||
|
|
||
| </axis> | ||
|
|
||
| <axis name="SIDE"> | ||
|
|
||
| <function name="aero/force/Side_beta"> | ||
| <description>Side force due to beta</description> | ||
| <product> | ||
| <property>aero/qbar-psf</property> | ||
| <property>metrics/Sw-sqft</property> | ||
| <property>aero/beta-rad</property> | ||
| <!--value>-4</value--> | ||
| <value>0.02</value> | ||
| </product> | ||
| </function> | ||
|
|
||
| </axis> | ||
|
|
||
| </aerodynamics> |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,394 @@ | ||
| <?xml version="1.0"?> | ||
| <?xml-stylesheet type="text/xsl" href="http://jsbsim.sourceforge.net/JSBSim.xsl"?> | ||
| <fdm_config name="QUAD COMPLETE EXT" version="2.0" release="BETA" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSim.xsd"> | ||
|
|
||
| <fileheader> | ||
| <author>ENAC</author> | ||
| <filecreationdate>09-06-2022</filecreationdate> | ||
| <version>Version 0.92 - beta</version> | ||
| <description>Falcon</description> | ||
| </fileheader> | ||
|
|
||
| <metrics> | ||
| <wingarea unit="M2"> 0.075 </wingarea> | ||
| <wingspan unit="M"> 0.9 </wingspan> | ||
| <chord unit="M"> 0.12 </chord> | ||
| <htailarea unit="M2"> 0 </htailarea> | ||
| <htailarm unit="M"> 0 </htailarm> | ||
| <vtailarea unit="M2"> 0 </vtailarea> | ||
| <vtailarm unit="M"> 0 </vtailarm> | ||
| <wing_incidence unit="DEG"> 90 </wing_incidence> | ||
| <location name="AERORP" unit="M"> | ||
| <x> 0 </x> | ||
| <y> 0 </y> | ||
| <z> 0 </z> | ||
| </location> | ||
| <location name="EYEPOINT" unit="M"> | ||
| <x> 0 </x> | ||
| <y> 0 </y> | ||
| <z> 0 </z> | ||
| </location> | ||
| <location name="VRP" unit="M"> | ||
| <x> 0 </x> | ||
| <y> 0 </y> | ||
| <z> 0 </z> | ||
| </location> | ||
| </metrics> | ||
|
|
||
| <mass_balance> | ||
| <ixx unit="KG*M2"> 0.005 </ixx> | ||
| <iyy unit="KG*M2"> 0.005 </iyy> | ||
| <izz unit="KG*M2"> 0.015 </izz> | ||
| <ixy unit="KG*M2"> 0. </ixy> | ||
| <ixz unit="KG*M2"> 0. </ixz> | ||
| <iyz unit="KG*M2"> 0. </iyz> | ||
| <emptywt unit="KG"> 0.72 </emptywt> | ||
| <location name="CG" unit="M"> | ||
| <x> 0 </x> | ||
| <y> 0 </y> | ||
| <z> 0 </z> | ||
| </location> | ||
| </mass_balance> | ||
|
|
||
| <ground_reactions> | ||
| <contact type="STRUCTURE" name="CONTACT_FRONT"> | ||
| <location unit="M"> | ||
| <x> -0.12 </x> | ||
| <y> 0.0 </y> | ||
| <z>-0.1 </z> | ||
| </location> | ||
| <static_friction> 0.8 </static_friction> | ||
| <dynamic_friction> 0.5 </dynamic_friction> | ||
| <spring_coeff unit="N/M"> 500 </spring_coeff> | ||
| <damping_coeff unit="N/M/SEC"> 100 </damping_coeff> | ||
| <damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound> | ||
| <max_steer unit="DEG"> 0.0 </max_steer> | ||
| <brake_group> NONE </brake_group> | ||
| <retractable>0</retractable> | ||
| </contact> | ||
|
|
||
| <contact type="STRUCTURE" name="CONTACT_BACK"> | ||
| <location unit="M"> | ||
| <x> 0.12</x> | ||
| <y> 0.0</y> | ||
| <z> -0.1</z> | ||
| </location> | ||
| <static_friction> 0.8 </static_friction> | ||
| <dynamic_friction> 0.5 </dynamic_friction> | ||
| <spring_coeff unit="N/M"> 500 </spring_coeff> | ||
| <damping_coeff unit="N/M/SEC"> 100 </damping_coeff> | ||
| <damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound> | ||
| <max_steer unit="DEG"> 0.0 </max_steer> | ||
| <brake_group> NONE </brake_group> | ||
| <retractable>0</retractable> | ||
| </contact> | ||
|
|
||
| <contact type="STRUCTURE" name="CONTACT_RIGHT"> | ||
| <location unit="M"> | ||
| <x> 0.0 </x> | ||
| <y> 0.12</y> | ||
| <z> -0.1 </z> | ||
| </location> | ||
| <static_friction> 0.8 </static_friction> | ||
| <dynamic_friction> 0.5 </dynamic_friction> | ||
| <spring_coeff unit="N/M"> 500 </spring_coeff> | ||
| <damping_coeff unit="N/M/SEC"> 100 </damping_coeff> | ||
| <damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound> | ||
| <max_steer unit="DEG"> 0.0 </max_steer> | ||
| <brake_group> NONE </brake_group> | ||
| <retractable>0</retractable> | ||
| </contact> | ||
|
|
||
| <contact type="STRUCTURE" name="CONTACT_LEFT"> | ||
| <location unit="M"> | ||
| <x> 0.0 </x> | ||
| <y>-0.12</y> | ||
| <z> -0.1 </z> | ||
| </location> | ||
| <static_friction> 0.8 </static_friction> | ||
| <dynamic_friction> 0.5 </dynamic_friction> | ||
| <spring_coeff unit="N/M"> 500 </spring_coeff> | ||
| <damping_coeff unit="N/M/SEC"> 100 </damping_coeff> | ||
| <damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound> | ||
| <max_steer unit="DEG"> 0.0 </max_steer> | ||
| <brake_group> NONE </brake_group> | ||
| <retractable>0</retractable> | ||
| </contact> | ||
| </ground_reactions> | ||
|
|
||
| <flight_control name="actuator_dynamics"> | ||
| <property value="30.">fcs/motor_lag</property> | ||
| <property value="54.">fcs/elevon_lag</property> | ||
|
|
||
| <channel name="filtering"> | ||
|
|
||
| <!--First order filter represents actuator dynamics--> | ||
| <lag_filter name="UR_lag"> | ||
| <input> fcs/UR </input> | ||
| <clipto> | ||
| <min>0</min> | ||
| <max>1</max> | ||
| </clipto> | ||
| <c1> fcs/motor_lag </c1> | ||
| <output> fcs/UR_lag</output> | ||
| </lag_filter> | ||
| <lag_filter name="BR_lag"> | ||
| <input> fcs/BR </input> | ||
| <clipto> | ||
| <min>0</min> | ||
| <max>1</max> | ||
| </clipto> | ||
| <c1> fcs/motor_lag </c1> | ||
| <output> fcs/BR_lag</output> | ||
| </lag_filter> | ||
| <lag_filter name="BL_lag"> | ||
| <input> fcs/BL </input> | ||
| <clipto> | ||
| <min>0</min> | ||
| <max>1</max> | ||
| </clipto> | ||
| <c1> fcs/motor_lag </c1> | ||
| <output> fcs/BL_lag</output> | ||
| </lag_filter> | ||
| <lag_filter name="UL_lag"> | ||
| <input> fcs/UL </input> | ||
| <clipto> | ||
| <min>0</min> | ||
| <max>1</max> | ||
| </clipto> | ||
| <c1> fcs/motor_lag </c1> | ||
| <output> fcs/UL_lag</output> | ||
| </lag_filter> | ||
|
|
||
| <lag_filter name="ele_left_lag"> | ||
| <input> fcs/ele_left </input> | ||
| <c1> fcs/elevon_lag </c1> | ||
| <output> fcs/ele_left_lag</output> | ||
| </lag_filter> | ||
| <lag_filter name="ele_right_lag"> | ||
| <input> fcs/ele_right </input> | ||
| <c1> fcs/elevon_lag </c1> | ||
| <output> fcs/ele_right_lag</output> | ||
| </lag_filter> | ||
| </channel> | ||
| </flight_control> | ||
|
|
||
| <external_reactions> | ||
|
|
||
| <property>fcs/UR</property> | ||
| <property>fcs/BR</property> | ||
| <property>fcs/BL</property> | ||
| <property>fcs/UL</property> | ||
| <property>fcs/UR_lag</property> | ||
| <property>fcs/BR_lag</property> | ||
| <property>fcs/BL_lag</property> | ||
| <property>fcs/UL_lag</property> | ||
| <property>fcs/ele_left</property> | ||
| <property>fcs/ele_left_lag</property> | ||
| <property>fcs/ele_right</property> | ||
| <property>fcs/ele_right_lag</property> | ||
|
|
||
| <property value="6.">fcs/motor_max_thrust</property> <!-- FIXME should be 4. from flight data --> | ||
| <property value="0.00001">fcs/motor_max_torque</property> | ||
|
|
||
| <!-- First the lift forces produced by each propeller --> | ||
|
|
||
| <force name="UR" frame="BODY" unit="LBS"> | ||
| <function> | ||
| <product> | ||
| <property>fcs/UR_lag</property> | ||
| <property>fcs/motor_max_thrust</property> | ||
| <value>0.224808943</value> <!-- N to LBS --> | ||
| <table> | ||
| <independentVar lookup="row">aero/qbar-psf</independentVar> | ||
| <tableData> <!-- 1 psf = 47.88 Pa , 18m/s : 200Pa, 5m/s :15Pa 3m/s:5.5Pa--> | ||
| 0. 1. | ||
| 0.1148 1. | ||
| 4.17 0.7 <!-- 18m/s --> | ||
| </tableData> | ||
| </table> | ||
| </product> | ||
| </function> | ||
| <location unit="M"> | ||
| <x>0.105</x> | ||
| <y>0.12</y> | ||
| <z>0</z> | ||
| </location> | ||
| <direction> | ||
| <x>0.342</x> | ||
| <y>0</y> | ||
| <z>-0.94</z> | ||
| </direction> | ||
| </force> | ||
| <force name="BR" frame="BODY" unit="LBS"> | ||
| <function> | ||
| <product> | ||
| <property>fcs/BR_lag</property> | ||
| <property>fcs/motor_max_thrust</property> | ||
| <value>0.224808943</value> <!-- N to LBS --> | ||
| <table> | ||
| <independentVar lookup="row">aero/qbar-psf</independentVar> | ||
| <tableData> <!-- 1 psf = 47.88 Pa , 18m/s : 200Pa, 5m/s :15Pa 3m/s:5.5Pa--> | ||
| 0. 1. | ||
| 0.1148 1. | ||
| 4.17 0.7 <!-- 18m/s --> | ||
| </tableData> | ||
| </table> | ||
| </product> | ||
| </function> | ||
| <location unit="M"> | ||
| <x>-0.105</x> | ||
| <y>0.12</y> | ||
| <z>0</z> | ||
| </location> | ||
| <direction> | ||
| <x>-0.342</x> | ||
| <y>0</y> | ||
| <z>-0.94</z> | ||
| </direction> | ||
| </force> | ||
| <force name="BL" frame="BODY" unit="LBS"> | ||
| <function> | ||
| <product> | ||
| <property>fcs/BL_lag</property> | ||
| <property>fcs/motor_max_thrust</property> | ||
| <value>0.224808943</value> <!-- N to LBS --> | ||
| <table> | ||
| <independentVar lookup="row">aero/qbar-psf</independentVar> | ||
| <tableData> <!-- 1 psf = 47.88 Pa , 18m/s : 200Pa, 5m/s :15Pa 3m/s:5.5Pa--> | ||
| 0. 1. | ||
| 0.1148 1. | ||
| 4.17 0.7 <!-- 18m/s --> | ||
| </tableData> | ||
| </table> | ||
| </product> | ||
| </function> | ||
| <location unit="M"> | ||
| <x>-0.105</x> | ||
| <y>-0.12</y> | ||
| <z>0</z> | ||
| </location> | ||
| <direction> | ||
| <x>-0.342</x> | ||
| <y>0</y> | ||
| <z>-0.94</z> | ||
| </direction> | ||
| </force> | ||
| <force name="UL" frame="BODY" unit="LBS"> | ||
| <function> | ||
| <product> | ||
| <property>fcs/UL_lag</property> | ||
| <property>fcs/motor_max_thrust</property> | ||
| <value>0.224808943</value> <!-- N to LBS --> | ||
| <table> | ||
| <independentVar lookup="row">aero/qbar-psf</independentVar> | ||
| <tableData> <!-- 1 psf = 47.88 Pa , 18m/s : 200Pa, 5m/s :15Pa 3m/s:5.5Pa--> | ||
| 0. 1. | ||
| 0.1148 1. | ||
| 4.17 0.7 <!-- 18m/s --> | ||
| </tableData> | ||
| </table> | ||
| </product> | ||
| </function> | ||
| <location unit="M"> | ||
| <x>0.105</x> | ||
| <y>-0.12</y> | ||
| <z>0</z> | ||
| </location> | ||
| <direction> | ||
| <x>0.342</x> | ||
| <y>0</y> | ||
| <z>-0.94</z> | ||
| </direction> | ||
| </force> | ||
|
|
||
| <!-- Then the Moment Couples --> | ||
|
|
||
| <moment name="UR_couple" frame="BODY"> | ||
| <function> | ||
| <product> | ||
| <property>fcs/UR_lag</property> | ||
| <property>fcs/motor_max_torque</property> | ||
| <value> 0.738 </value> <!-- N.m to FT.LBS --> | ||
| </product> | ||
| </function> | ||
| <location unit="M"> | ||
| <x>0.105</x> | ||
| <y>0.12</y> | ||
| <z>0</z> | ||
| </location> | ||
| <direction> | ||
| <x>0</x> | ||
| <y>0</y> | ||
| <z>-1</z> | ||
| </direction> | ||
| </moment> | ||
|
|
||
| <moment name="BR_couple" frame="BODY"> | ||
| <function> | ||
| <product> | ||
| <property>fcs/BR_lag</property> | ||
| <property>fcs/motor_max_torque</property> | ||
| <value> 0.738 </value> <!-- N.m to FT.LBS --> | ||
| </product> | ||
| </function> | ||
| <location unit="M"> | ||
| <x>-0.105</x> | ||
| <y>0.12</y> | ||
| <z>0</z> | ||
| </location> | ||
| <direction> | ||
| <x>0</x> | ||
| <y>0</y> | ||
| <z>1</z> | ||
| </direction> | ||
| </moment> | ||
|
|
||
| <moment name="BL_couple" frame="BODY"> | ||
| <function> | ||
| <product> | ||
| <property>fcs/BL_lag</property> | ||
| <property>fcs/motor_max_torque</property> | ||
| <value> 0.738 </value> <!-- N.m to FT.LBS --> | ||
| </product> | ||
| </function> | ||
| <location unit="M"> | ||
| <x>-0.105</x> | ||
| <y>-0.12</y> | ||
| <z>0</z> | ||
| </location> | ||
| <direction> | ||
| <x>0</x> | ||
| <y>0</y> | ||
| <z>-1</z> | ||
| </direction> | ||
| </moment> | ||
|
|
||
| <moment name="UL_couple" frame="BODY"> | ||
| <function> | ||
| <product> | ||
| <property>fcs/UL_lag</property> | ||
| <property>fcs/motor_max_torque</property> | ||
| <value> 0.738 </value> <!-- N.m to FT.LBS --> | ||
| </product> | ||
| </function> | ||
| <location unit="M"> | ||
| <x>0.105</x> | ||
| <y>-0.12</y> | ||
| <z>0</z> | ||
| </location> | ||
| <direction> | ||
| <x>0</x> | ||
| <y>0</y> | ||
| <z>1</z> | ||
| </direction> | ||
| </moment> | ||
|
|
||
| </external_reactions> | ||
|
|
||
| <propulsion/> | ||
|
|
||
| <flight_control name="FGFCS"/> | ||
| <aerodynamics file="Systems/aerodynamics_falcon.xml"/> | ||
|
|
||
| </fdm_config> |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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); | ||
| } | ||
|
|
||
|
|
||
| } | ||
|
|
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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; | ||
| } |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,186 @@ | ||
| /* | ||
| * 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. | ||
| */ | ||
|
|
||
|
|
||
| #ifndef _PLATFORM_H_ | ||
| #define _PLATFORM_H_ | ||
| #pragma once | ||
|
|
||
| #include <stdint.h> | ||
| #include <string.h> | ||
| #include "mcu_periph/i2c.h" | ||
|
|
||
| #define VL53L5CX_I2C_BUF_SIZE 512 | ||
|
|
||
|
|
||
| enum VL53L5CX_ERRORS { | ||
| VL53L5CX_NO_ERROR = 0, | ||
| VL53L5CX_NOT_DETECTED = 1, | ||
| VL53L5CX_ULD_LOADING_FAILED = 2, | ||
| VL53L5CX_SET_RESOLUTION_FAILED = 3, | ||
| VL53L5CX_RUNTIME_FAILURE = 4, | ||
| }; | ||
|
|
||
| /** | ||
| * @brief Structure VL53L5CX_Platform needs to be filled by the customer, | ||
| * depending on his platform. At least, it contains the VL53L5CX I2C address. | ||
| * Some additional fields can be added, as descriptors, or platform | ||
| * dependencies. Anything added into this structure is visible into the platform | ||
| * layer. | ||
| */ | ||
|
|
||
| typedef struct | ||
| { | ||
| /* To be filled with customer's platform. At least an I2C address/descriptor | ||
| * needs to be added */ | ||
| /* Example for most standard platform : I2C address of sensor */ | ||
| uint16_t address; | ||
| //I2CDriver* i2cp; | ||
| struct i2c_periph *i2cdev; | ||
| bool data_available; | ||
| int16_t distances_mm[64]; | ||
|
|
||
| enum VL53L5CX_ERRORS error_code; | ||
| uint8_t buf[VL53L5CX_I2C_BUF_SIZE+2]; | ||
| void* user_data; | ||
|
|
||
| } VL53L5CX_Platform; | ||
|
|
||
| /* | ||
| * @brief The macro below is used to define the number of target per zone sent | ||
| * through I2C. This value can be changed by user, in order to tune I2C | ||
| * transaction, and also the total memory size (a lower number of target per | ||
| * zone means a lower RAM). The value must be between 1 and 4. | ||
| */ | ||
|
|
||
| #define VL53L5CX_NB_TARGET_PER_ZONE 1U | ||
|
|
||
| /* | ||
| * @brief The macro below can be used to avoid data conversion into the driver. | ||
| * By default there is a conversion between firmware and user data. Using this macro | ||
| * allows to use the firmware format instead of user format. The firmware format allows | ||
| * an increased precision. | ||
| */ | ||
|
|
||
| // #define VL53L5CX_USE_RAW_FORMAT | ||
|
|
||
| /* | ||
| * @brief All macro below are used to configure the sensor output. User can | ||
| * define some macros if he wants to disable selected output, in order to reduce | ||
| * I2C access. | ||
| */ | ||
|
|
||
| // #define VL53L5CX_DISABLE_AMBIENT_PER_SPAD | ||
| // #define VL53L5CX_DISABLE_NB_SPADS_ENABLED | ||
| // #define VL53L5CX_DISABLE_NB_TARGET_DETECTED | ||
| // #define VL53L5CX_DISABLE_SIGNAL_PER_SPAD | ||
| // #define VL53L5CX_DISABLE_RANGE_SIGMA_MM | ||
| // #define VL53L5CX_DISABLE_DISTANCE_MM | ||
| // #define VL53L5CX_DISABLE_REFLECTANCE_PERCENT | ||
| // #define VL53L5CX_DISABLE_TARGET_STATUS | ||
| // #define VL53L5CX_DISABLE_MOTION_INDICATOR | ||
|
|
||
| /** | ||
| * @param (VL53L5CX_Platform*) p_platform : Pointer of VL53L5CX platform | ||
| * structure. | ||
| * @param (uint16_t) Address : I2C location of value to read. | ||
| * @param (uint8_t) *p_values : Pointer of value to read. | ||
| * @return (uint8_t) status : 0 if OK | ||
| */ | ||
|
|
||
| uint8_t RdByte( | ||
| VL53L5CX_Platform *p_platform, | ||
| uint16_t RegisterAdress, | ||
| uint8_t *p_value); | ||
|
|
||
| /** | ||
| * @brief Mandatory function used to write one single byte. | ||
| * @param (VL53L5CX_Platform*) p_platform : Pointer of VL53L5CX platform | ||
| * structure. | ||
| * @param (uint16_t) Address : I2C location of value to read. | ||
| * @param (uint8_t) value : Pointer of value to write. | ||
| * @return (uint8_t) status : 0 if OK | ||
| */ | ||
|
|
||
| uint8_t WrByte( | ||
| VL53L5CX_Platform *p_platform, | ||
| uint16_t RegisterAdress, | ||
| uint8_t value); | ||
|
|
||
| /** | ||
| * @brief Mandatory function used to read multiples bytes. | ||
| * @param (VL53L5CX_Platform*) p_platform : Pointer of VL53L5CX platform | ||
| * structure. | ||
| * @param (uint16_t) Address : I2C location of values to read. | ||
| * @param (uint8_t) *p_values : Buffer of bytes to read. | ||
| * @param (uint32_t) size : Size of *p_values buffer. | ||
| * @return (uint8_t) status : 0 if OK | ||
| */ | ||
|
|
||
| uint8_t RdMulti( | ||
| VL53L5CX_Platform *p_platform, | ||
| uint16_t RegisterAdress, | ||
| uint8_t *p_values, | ||
| uint32_t size); | ||
|
|
||
| /** | ||
| * @brief Mandatory function used to write multiples bytes. | ||
| * @param (VL53L5CX_Platform*) p_platform : Pointer of VL53L5CX platform | ||
| * structure. | ||
| * @param (uint16_t) Address : I2C location of values to write. | ||
| * @param (uint8_t) *p_values : Buffer of bytes to write. | ||
| * @param (uint32_t) size : Size of *p_values buffer. | ||
| * @return (uint8_t) status : 0 if OK | ||
| */ | ||
|
|
||
| uint8_t WrMulti( | ||
| VL53L5CX_Platform *p_platform, | ||
| uint16_t RegisterAdress, | ||
| uint8_t *p_values, | ||
| uint32_t size); | ||
|
|
||
| /** | ||
| * @brief Optional function, only used to perform an hardware reset of the | ||
| * sensor. This function is not used in the API, but it can be used by the host. | ||
| * This function is not mandatory to fill if user don't want to reset the | ||
| * sensor. | ||
| * @param (VL53L5CX_Platform*) p_platform : Pointer of VL53L5CX platform | ||
| * structure. | ||
| * @return (uint8_t) status : 0 if OK | ||
| */ | ||
|
|
||
| // uint8_t Reset_Sensor( | ||
| // VL53L5CX_Platform *p_platform); | ||
|
|
||
| /** | ||
| * @brief Mandatory function, used to swap a buffer. The buffer size is always a | ||
| * multiple of 4 (4, 8, 12, 16, ...). | ||
| * @param (uint8_t*) buffer : Buffer to swap, generally uint32_t | ||
| * @param (uint16_t) size : Buffer size to swap | ||
| */ | ||
|
|
||
| void SwapBuffer( | ||
| uint8_t *buffer, | ||
| uint16_t size); | ||
| /** | ||
| * @brief Mandatory function, used to wait during an amount of time. It must be | ||
| * filled as it's used into the API. | ||
| * @param (VL53L5CX_Platform*) p_platform : Pointer of VL53L5CX platform | ||
| * structure. | ||
| * @param (uint32_t) TimeMs : Time to wait in ms. | ||
| * @return (uint8_t) status : 0 if wait is finished. | ||
| */ | ||
|
|
||
| uint8_t WaitMs( | ||
| VL53L5CX_Platform *p_platform, | ||
| uint32_t TimeMs); | ||
|
|
||
| #endif // _PLATFORM_H_ |