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

Add rangesensor module (#2158)

Browse Source 
* added rangesensors to gazebo model and NPS
* added module to handle the range sensors
* added sender IDS and added to abi messages
* added abi message to flight plan guided
This commit is contained in:
Kirk Scheper
2017-11-06 17:16:13 +01:00
committed by Gautier Hattenberger
parent a2991ee9b3
commit e84ff368c2
35 changed files with 2419 additions and 806 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/Makefile.stm32-upload
+1 -1
View File
@@ -189,7 +189,7 @@ upload: $(OBJDIR)/$(TARGET).bin
else
upload:
@echo unknown FLASH_MODE $(FLASH_MODE)
@echo Available FLASH_MODEs: DFU, DFU-UTIL, SWD, JTAG, JTAG_BMP, STLINK, SERIAL
@echo Available FLASH_MODEs: DFU, DFU-UTIL, SWD, JTAG, JTAG_BMP, STLINK, SERIAL, SWD_NOPWR
endif
.PHONY : upload
conf/abi.xml
+9 -2
View File
@@ -93,8 +93,9 @@
</message>
<message name="OBSTACLE_DETECTION" id="17">
<field name="distance_obstacle" type="float" unit="m"/>
<field name="relative_heading" type="float" unit="rad"/>
<field name="distance" type="float" unit="m"/>
<field name="elevation" type="float" unit="rad"/>
<field name="heading" type="float" unit="rad"/>
</message>
<message name="POSITION_ESTIMATE" id="18">
@@ -113,6 +114,12 @@
<field name="h" type="float" unit="m"/>
</message>
<message name="RANGE_FORCEFIELD" id="20">
<field name="vel_body_x_FF" type="float" unit="m/s"/>
<field name="vel_body_y_FF" type="float" unit="m/s"/>
<field name="vel_body_z_FF" type="float" unit="m/s"/>
</message>
</msg_class>
</protocol>
conf/airframes/KS/ks_bebop2_stereo.xml
+2 -2
View File
@@ -204,8 +204,8 @@
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="GAZEBO_AC_NAME" value="simple_x_quad" type="string"/>
<define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="double[]"/>
<define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="double[]"/>
<define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/>
<define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="float[]"/>
<!--define name="DEBUG_STEREOCAM" value="1"/-->
<!--define name="NO_GPS" value="1"/-->
conf/airframes/examples/ardrone2_gazebo.xml
+2 -3
View File
@@ -188,12 +188,11 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="double[]"/>
<define name="ACTUATOR_TORQUES" value="0.155, -0.155, 0.155, -0.155" type="double[]"/>
<define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/>
<define name="ACTUATOR_TORQUES" value="0.155, -0.155, 0.155, -0.155" type="float[]"/>
<define name="JSBSIM_MODEL" value="simple_ardrone2" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_ardrone2.h" type="string"/>
<define name="BYPASS_AHRS" value="1"/>
<define name="SIMULATE_VIDEO" value="1"/>
</section>
<section name="AUTOPILOT">
conf/airframes/tudelft/ladybird_lisa_mxs.xml
+2 -2
View File
@@ -199,8 +199,8 @@
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="GAZEBO_AC_NAME" value="simple_x_quad" type="string"/>
<define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="double[]"/>
<define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="double[]"/>
<define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/>
<define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="float[]"/>
</section>
<section name="AUTOPILOT">
conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
+311
View File
@@ -0,0 +1,311 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- this is a customized 3D printed frame (with origami design) equiped with Lisa/MXS 1.0 -->
<!-- The LadyBird frame comes with four brushed motors in an + (plus) configuration. -->
<!--
The motor and rotor configuration is the following:
Front (facing towards stereo camera
^
\
Motor2(NW) Motor1(NE)
CW CCW
\ /
,___,
| |
| |
|___|
/ \
CCW CW
Motor3(SW) Motor0(SE)
-->
<airframe name="quadrotor_lisa_mxs">
<firmware name="rotorcraft">
<target name="ap" board="lisa_mxs_1.0">
<module name="stabilization" type="indi_simple"/>
</target>
<target name="nps" board="pc">
<module name="fdm" type="gazebo"/>
<module name="stabilization" type="int_quat"/>
</target>
<define name="REMAP_UART3" value="TRUE"/>
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="UART3"/>
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="radio_control" type="datalink"/>
<module name="motor_mixing"/>
<module name="actuators" type="pwm">
<!-- This airframe is using the servo PWM outputs directly to drive mosfets, which in turn directly drive brushed motors.
With the following settings we will be driving the motors with a 36KHz signal that should be well above anyone's hearing range.
If we went with the 500Hz that woul be the default we would be hearing a very annoying high pitched sound coming from the motors.-->
<!-- Setting the PWM timer base frequency to 36MHz -->
<define name="PWM_BASE_FREQ" value="36000000"/>
<!-- Setting the PWM interval to 36KHz -->
<define name="SERVO_HZ" value="36000"/>
</module>
<module name="imu" type="lisa_mx_v2.1"/>
<module name="gps" type="datalink"/>
<module name="ahrs" type="int_cmpl_quat"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
<!-- INS: accelerometer measurements are really bad on tiny quadcopter (+- 50m/s²)
so in anycase, the state filters should not put much trust in those-->
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
<module name="ins" type="hff_extended">
<define name="VFF_EXTENDED_ACCEL_NOISE" value="4"/>
<define name="HFF_ACCEL_NOISE" value="4"/>
</module>
<!--Modules -->
<module name="send_imu_mag_current"/>
<module name="air_data">
<define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE"/>
</module>
<module name="stereocam">
<define name="STEREO_UART" value="UART1"/>
</module>
<module name="laser_range_array">
<configure name="LASER_RANGE_ARRAY_BAUD" value="38400"/>
<configure name="LASER_RANGE_ARRAY_PORT" value="UART2"/>
</module>
<module name="range_forcefield">
<define name="RANGE_FORCEFIELD_MAX_VEL" value="0.6"/>
</module>
</firmware>
<servos driver="Pwm">
<servo name="RIGHT" no="1" min="0" neutral="50" max="1000"/>
<servo name="BACK" no="0" min="0" neutral="50" max="1000"/>
<servo name="LEFT" no="3" min="0" neutral="50" max="1000"/>
<servo name="FRONT" no="2" min="0" neutral="50" max="1000"/>
</servos>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="TYPE" value="QUAD_PLUS"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
<set servo="FRONT" value="motor_mixing.commands[MOTOR_FRONT]"/>
<set servo="RIGHT" value="motor_mixing.commands[MOTOR_RIGHT]"/>
<set servo="BACK" value="motor_mixing.commands[MOTOR_BACK]"/>
<set servo="LEFT" value="motor_mixing.commands[MOTOR_LEFT]"/>
</command_laws>
<section name="STEREOCAM" prefix="STEREO_BODY_TO_STEREO_">
<define name= "PHI" value="90." unit="deg"/>
<define name= "THETA" value="0." unit="deg"/>
<define name= "PSI" value="90." unit="deg"/>
</section>
<!--Orientation of the laser array with 5 sensors relative from the body fixed coordinates
, with the hand right rule with eulerangle order of (azimuth, bearing) in [rad]:
0) (0, -1.57) -> left
1) (1.57, 0) -> up
2) (0, 1.57) -> right
4) (-1.57, 0) -> down
5) (0, 0) -> front
-->
<section name="LASER_RANGE_ARRAY" prefix="LASER_RANGE_ARRAY_">
<define name="NUM_SENSORS" value="5"/>
<define name="ORIENTATIONS" value="0.,-1.57, 1.57,0., 0.,1.57, -1.57,0., 0.,0." type="float[]"/>
<define name="SEND_AGL" value="true"/>
</section>
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0" unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="135." unit="deg"/>
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<!--define name="ACCEL_X_SENS" value="4.8" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.8" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.8" integer="16"/--><!--18 : 4.57 19: 4.31 -->
<!-- MAGNETO CALIBRATION DELFT -->
<define name="MAG_X_NEUTRAL" value="286"/>
<define name="MAG_Y_NEUTRAL" value="-72"/>
<define name="MAG_Z_NEUTRAL" value="97"/>
<define name="MAG_X_SENS" value="3.94431833863" integer="16"/>
<define name="MAG_Y_SENS" value="4.14629702271" integer="16"/>
<define name="MAG_Z_SENS" value="4.54518768636" integer="16"/>
<!-- MAGNETO CURRENT CALIBRATION -->
<define name="MAG_X_CURRENT_COEF" value="0.0103422023767"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0084568317783"/>
<define name="MAG_Z_CURRENT_COEF" value="-0.01935617335"/>
</section>
<!-- local magnetic field -->
<section name="AHRS" prefix="AHRS_">
<define name="H_X" value=" 0.47577"/>
<define name="H_Y" value=" 0.11811"/>
<define name="H_Z" value=" 0.87161"/>
<!-- Since the accelerometer is useless on these small
quadcopters, the gravity vector should not be estimated
by the accelerometers -->
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
</section>
<section name="INS" prefix="INS_">
<!--define name="INT_GPS_ID" value="ABI_DISABLE"/-->
<define name="USE_GPS_ALT" value="true"/>
<define name="USE_GPS_SPEED" value="true"/>
<define name="INS_VFF_R_GPS" value="0.1"/>
<define name="INS_VFF_VZ_R_GPS" value="0.1"/>
<define name="SONAR_UPDATE_ON_AGL" value="true"/>
</section>
<section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoint limits for attitude stabilization rc flight -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_PSI" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="SP_MAX_P" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>
</section>
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<!-- attitude reference generation model -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="850"/>
<define name="PHI_DGAIN" value="500"/>
<define name="PHI_IGAIN" value="0"/>
<define name="THETA_PGAIN" value="850"/>
<define name="THETA_DGAIN" value="500"/>
<define name="THETA_IGAIN" value="0"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="700"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="100"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.067204"/>
<define name="G1_Q" value="0.066647"/>
<define name="G1_R" value="0.003575"/>
<define name="G2_R" value="0.098106"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="170.0"/>
<define name="REF_ERR_Q" value="170.0"/>
<define name="REF_ERR_R" value="100.0"/>
<define name="REF_RATE_P" value="17.0"/>
<define name="REF_RATE_Q" value="17.0"/>
<define name="REF_RATE_R" value="17.0"/>
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="50.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="50.0"/>
<define name="FILT_ZETA_R" value="0.55"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.03"/>
<define name="ACT_DYN_Q" value="0.03"/>
<define name="ACT_DYN_R" value="0.03"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<!-- Guidance in vertical plane especially for use of range sensor-->
<define name="HOVER_KP" value="450"/>
<define name="HOVER_KD" value="250"/>
<define name="HOVER_KI" value="50"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.65"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="0"/>
<define name="DGAIN" value="260"/>
<define name="IGAIN" value="0"/>
<define name="MAX_BANK" value="15" unit="deg"/>
<define name="REF_MAX_SPEED" value="0.5"/>
<define name="USE_SPEED_REF" value="TRUE"/>
</section>
<section name="MISC">
<define name="VoltageOfAdc(adc)" value="(adc)*0.00162f" />
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="GAZEBO_AC_NAME" value="simple_quad" type="string"/>
<define name="GAZEBO_WORLD" value="square.world" type="string"/>
<define name="ACTUATOR_NAMES" value="front_motor,right_motor,back_motor,left_motor" type="string[]"/>
<define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/>
<define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="float[]"/>
<define name="JSBSIM_MODEL" value="simple_quad" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="BYPASS_AHRS" value="1"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_GUIDED"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_GUIDED"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="14000"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="3.3" unit="V"/>
<define name="LOW_BAT_LEVEL" value="3.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="4.1" unit="V"/>
</section>
</airframe>
conf/boards/lisa_mxs_1.0.makefile
+2
View File
@@ -25,4 +25,6 @@ DFU_ADDR = 0x8004000
DFU_PRODUCT = Lisa/Lia
endif
FLASH_MODE ?= SWD_NOPWR
include $(PAPARAZZI_SRC)/conf/boards/lisa_m_defaults.makefile
conf/flight_plans/TUDELFT/tudelft_rotorcraft_guided_flightplan.xml
+129
View File
@@ -0,0 +1,129 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="1" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="20" name="Rotorcraft Optitrack (Delft)" security_height="0.3">
<header>
#include "autopilot_guided.h"
// Useful Combination of the flags fir the autopilot_guided_update
#define GUIDED_FLAG_XY_VEL_BODY GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL
#define GUIDED_FLAG_XY_VEL_BODY_YAW_RATE GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL|GUIDED_FLAG_YAW_RATE
#define GUIDED_FLAG_XY_OFFSET_Z_VEL GUIDED_FLAG_XY_OFFSET|GUIDED_FLAG_Z_VEL
#define GUIDED_FLAG_XY_OFFSET_YAW_OFFSET GUIDED_FLAG_XY_OFFSET|GUIDED_FLAG_YAW_OFFSET
#ifdef NAV_C
static float obstacle_dist, obstacle_azimuth, obstacle_bearing;
static inline void obstacle_detection_cb(uint8_t sender_id __attribute__((unused)), float _obstacle_dist, float _obstacle_azimuth, float _obstacle_bearing)
{
if (And(CloseRadAngles(_obstacle_azimuth,0), CloseRadAngles(_obstacle_bearing,0))) {
obstacle_dist = _obstacle_dist;
obstacle_azimuth = _obstacle_azimuth;
obstacle_bearing = _obstacle_bearing;
}
}
#endif
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="STDBY" x="0.0" y="0.0"/>
</waypoints>
<variables>
<variable var="desired_heading" init="0.0f" type="float" min="0." max="10." step="0.1"/>
<variable var="turn_angle" init="90.0f" type="float" min="-180" max="180" step="1"/>
<variable var="turn_rate" init="0.5f" type="float" min="0." max="10." step="0.1"/>
<variable var="climb_descent_vel" init="0.3f" type="float" min="0." max="1" step="0.1"/>
<variable var="forward_vel" init="0.5f" type="float" min="0." max="2" step="0.1"/>
<variable var="nominal_alt" init="NAV_DEFAULT_ALT" type="float" min="0." max="10." step="0.1"/>
<abi_binding name="OBSTACLE_DETECTION" handler="obstacle_detection_cb"/>
<abi_binding name="RANGE_FORCEFIELD" vars="vel_body_x_FF, vel_body_y_FF, vel_body_z_FF"/>
</variables>
<!--Example Guided flight plan:
1. Flies in a small square, just based on guided flight commands (no waypoints)
2. If it detects an obstacle (example shows only from color detector), it will turn the 90 degrees more quickly
Remember to choose AP_MODE_GUIDED or else it won't work!!!
-->
<blocks>
<block name="FPInit">
<while cond="!GpsFixValid()"/>
<set var = "obstacle_dist" value="10.0"/><!--Or else it will turn everytime if the abi message is not received-->
<set var = "vel_body_x_FF" value="0."/>
<set var = "vel_body_y_FF" value="0."/>
<set var = "vel_body_z_FF" value="0."/>
<call_once fun="ins_reset_altitude_ref()"/>
<while cond="1"/>
</block>
<block name="Start Engine">
<call_once fun="autopilot_set_motors_on(TRUE)"/>
<while cond="1"/>
</block>
<!--Take off:
1. The MAV will first climb with 0.3 m/s upwards (z is down)
2. If it detects that the MAV has surpased the nominal altitude (given above in the flightplan)"
3. It will hold the nominal altitude and wait for 1 second before it moves to Go Forward-->
<block name="Take off">
<set var="desired_heading" value="stateGetNedToBodyEulers_f()->psi"/>
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET_Z_VEL,0.,0.,-climb_descent_vel,desired_heading)"/>
<while cond="LessThan(stateGetPositionEnu_f()->z,nominal_alt-0.2)"/>
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET,0.,0.,-nominal_alt,desired_heading)"/>
<while cond="LessThan(stage_time,1)"/>
</block>
<!-- Go Forward:
1. The MAV will move forward with a constant speed (0.5 m/s), keeping the sideways speed zero while maintaining it's height on the nominal Altitude
2. If 5 seconds has passed in the block, It will go to the "Turn 90 deg" block
An exception is triggered if an obstacle is detected (in this example given by the color detector). Then it will go to the "Turn 90 deg" block immediatly
NOTE: A velocity forcefield in the horizontal plane applied here, so if an obstacle comes within a certain distance (adjust in settings), it will give an opposite wanted velocity to get out of the way.
-->
<block name="Go Forward">
<while cond="1"><!--LessThan(block_time,5)"/-->
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_VEL_BODY,forward_vel + vel_body_x_FF,vel_body_y_FF,-nominal_alt,desired_heading)"/>
</while>
<exception cond="1.5>obstacle_dist" deroute="Turn 90 deg"/>
</block>
<!-- Turn 90 deg:
1. First the MAV will hover with all velocities on zero for 1 second
2. An wanted heading is determined from the MAVs current state (90 deg offset is default)
3. Then the drone turns with an constant rate (default: 0.5 rad/s)
4. Once the desired heading is reached, the MAV will stop turning and keep this heading
5. After 1 second, it will go back to the "Go Forward" block -->
<block name="Turn 90 deg">
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_VEL_BODY,0.,0.,-nominal_alt,desired_heading)"/>
<while cond="LessThan(stage_time,1)"/>
<set var="desired_heading" value="stateGetNedToBodyEulers_f()->psi + RadOfDeg(turn_angle)"/>
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_VEL_BODY_YAW_RATE,0.,0.,-nominal_alt,turn_rate)"/>
<while cond="!CloseRadAngles(stateGetNedToBodyEulers_f()->psi,desired_heading)"/>
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_VEL_BODY,0.,0.,-nominal_alt,desired_heading)"/>
<while cond="LessThan(stage_time,1)"/>
<set var="obstacle_dist" value="10"/><!--reset distance-->
<deroute block="Go Forward"/>
</block>
<!-- Land:
1. Same as take off, but now it descents with an constant speed (default: 0.3 m/s)
2. If the MAV is close to the ground (0.1 m), the drone will set its desired height to zero
3. After 3 seconds, it will shut off its motors -->
<block name="Land">
<set var="desired_heading" value="stateGetNedToBodyEulers_f()->psi"/>
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET_Z_VEL,0.,0.,climb_descent_vel,desired_heading)"/>
<while cond="MoreThan(stateGetPositionEnu_f()->z,0.1)"/>
<call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET_YAW_OFFSET,0.,0.,0.,0.)"/>
<while cond="LessThan(block_time,5)"/>
<deroute block="Kill Engines"/>
</block>
<block name="Kill Engines">
<call_once fun="autopilot_set_motors_on(FALSE)"/>
<while cond="1"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/flight_plan.dtd
+3 -2
View File
@@ -117,8 +117,9 @@ values CDATA #IMPLIED>
<!ATTLIST abi_binding
name CDATA #REQUIRED
vars CDATA #REQUIRED
id CDATA #IMPLIED>
vars CDATA #IMPLIED
id CDATA #IMPLIED
handler CDATA #IMPLIED>
<!ATTLIST modules>
conf/modules/fdm_gazebo.xml
+2 -2
View File
@@ -47,8 +47,8 @@
b) Add actuator thrusts and torques to the SIMULATOR section:
&lt;section name=&quot;SIMULATOR&quot; prefix=&quot;NPS_&quot;&gt;
&lt;define name=&quot;ACTUATOR_NAMES&quot; value=&quot;nw_motor, ne_motor, se_motor, sw_motor&quot; type=&quot;string[]&quot;/&gt;
&lt;define name=&quot;ACTUATOR_THRUSTS&quot; value=&quot;1.55, 1.55, 1.55, 1.55&quot; type=&quot;double[]&quot;/&gt;
&lt;define name=&quot;ACTUATOR_TORQUES&quot; value=&quot;0.155, -0.155, 0.155, -0.155&quot; type=&quot;double[]&quot;/&gt;
&lt;define name=&quot;ACTUATOR_THRUSTS&quot; value=&quot;1.55, 1.55, 1.55, 1.55&quot; type=&quot;float[]&quot;/&gt;
&lt;define name=&quot;ACTUATOR_TORQUES&quot; value=&quot;0.155, -0.155, 0.155, -0.155&quot; type=&quot;float[]&quot;/&gt;
...
&lt;section&gt;
The thrusts and torques are expressed in SI units (N, Nm) and should
conf/modules/laser_range_array.xml
+34
View File
@@ -0,0 +1,34 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="laser_range_array" dir="range_finder">
<doc>
<description>Reads out values through uart of an laser range ring (array), containing multiple ToF IR laser range modules</description>
<configure name="LASER_RANGE_ARRAY_PORT" value="UART2" description="select which uart it is connected to"/>
<configure name="LASER_RANGE_ARRAY_BAUD" value="38400" description="set the baudrate of the uart"/>
<define name="LASER_RANGE_ARRAY_NUM_SENSORS" value="0" description="Total amount of laser range sensors on the airframe"/>
<define name="LASER_RANGE_ARRAY_ORIENTATIONS" value="0,0" type="float[]" description="The orientation per laser range sensor in the order: elevation,heading. The array is built up like (elv_1, head_1, elv_2, head_2,...elv_n, head_n) with n=LASER_RANGE_ARRAY_NUM_SENSORS"/>
</doc>
<header>
<file name="laser_range_array.h"/>
</header>
<init fun="laser_range_array_init()"/>
<event fun="laser_range_array_event()"/>
<makefile>
<!-- Configure default UART port and baudrate -->
<configure name="LASER_RANGE_ARRAY_PORT" default="UART5" case="upper|lower"/>
<configure name="LASER_RANGE_ARRAY_BAUD" default="38400"/>
<!-- Enable UART and set baudrate -->
<define name="USE_$(LASER_RANGE_ARRAY_PORT_UPPER)"/>
<define name="USE_$(LASER_RANGE_ARRAY_PORT_UPPER)_TX" value="FALSE"/>
<define name="$(LASER_RANGE_ARRAY_PORT_UPPER)_BAUD" value="$(LASER_RANGE_ARRAY_BAUD)"/>
<define name="LASER_RANGE_ARRAY_PORT" value="$(LASER_RANGE_ARRAY_PORT_LOWER)"/>
<file name="laser_range_array.c"/>
<file name="pprz_transport.c" dir="pprzlink/src"/>
</makefile>
<makefile target="nps">
<define name="NPS_SIMULATE_LASER_RANGE_ARRAY" value="1"/>
</makefile>
</module>
conf/modules/range_forcefield.xml
+36
View File
@@ -0,0 +1,36 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="range_forcefield" dir="range_forcefield">
<doc>
<description>This module generates a forcefield based on range sensor measurements the use of single point range sensor </description>
<section name="RANGE_FORCEFIELD" prefix="RANGE_FORCEFIELD_">
<define name="INNER_LIMIT" value="1." description="The inner border of the range forcefield, where the MAV will avoid the obstacle with the maximum velocity as given by MIN_VEL (in meters)"/>
<define name="OUTER_LIMIT" value="1.4" description="The outer border of the range forcefield, where the MAV will start avoiding with velocity determined on the range measurement (starts with MIN_VEL)"/>
<define name="MIN_VEL" value="0.0" description="The minimum velocity which forces the MAV out of the forcefield (in [m/s])"/>
<define name="MAX_VEL" value="0.5" description="The maximum velocity which forces the MAV out of the forcefield (in [m/s])"/>
</section>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="Range Velocity Force Field">
<dl_setting var="range_forcefield_param.inner_limit" min="0.5" step="0.1" max="3." module="range_forcefield/range_forcefield" shortname="inner_limit" param="RANGE_FORCEFIELD_INNER_LIMIT"/>
<dl_setting var="range_forcefield_param.outer_limit" min="0.5" step="0.1" max="3." module="range_forcefield/range_forcefield" shortname="outer_limit" param="RANGE_FORCEFIELD_OUTER_LIMIT"/>
<dl_setting var="range_forcefield_param.min_vel" min="0.0" step="0.1" max="1." module="range_forcefield/range_forcefield" shortname="min_vel" param="RANGE_FORCEFIELD_MIN_VEL"/>
<dl_setting var="range_forcefield_param.max_vel" min="0.1" step="0.1" max="5." module="range_forcefield/range_forcefield" shortname="max_vel" param="RANGE_FORCEFIELD_MIN_VEL"/>
</dl_settings>
</dl_settings>
</settings>
<depends>laser_range_array</depends>
<header>
<file name="range_forcefield.h"/>
</header>
<init fun="range_forcefield_init()"/>
<periodic fun="range_forcefield_periodic()" freq="10"/>
<makefile>
<file name="range_forcefield.c"/>
</makefile>
</module>
conf/simulator/gazebo/models/ardrone/ardrone.sdf
+232 -232
View File
@@ -1,249 +1,249 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="ardrone">
<pose>0 0 .1 0 0 0</pose>
<model name="ardrone">
<pose>0 0 .1 0 0 0</pose>
<link name="chassis">
<velocity_decay>
<linear>0.001</linear>
</velocity_decay>
<inertial><!-- Converted to SI from Paparazzi's ARDrone model for JSBsim -->
<mass>0.4</mass>
<inertia>
<ixx> 0.00678 </ixx>
<iyy> 0.00678 </iyy>
<izz> 0.01356 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<link name="chassis">
<velocity_decay>
<linear>0.001</linear>
</velocity_decay>
<inertial><!-- Converted to SI from Paparazzi's ARDrone model for JSBsim -->
<mass>0.4</mass>
<inertia>
<ixx> 0.00678 </ixx>
<iyy> 0.00678 </iyy>
<izz> 0.01356 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 0.05</size>
</box>
</geometry>
</visual>
<sensor name="contactsensor" type="contact">
<contact>
<collision>collision</collision>
</contact>
</sensor>
</link>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 0.05</size>
</box>
</geometry>
</visual>
<sensor name="contactsensor" type="contact">
<contact>
<collision>collision</collision>
</contact>
</sensor>
</link>
<!-- MOTORS -->
<link name="nw_motor">
<pose>0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<!-- MOTORS -->
<link name="nw_motor">
<pose>0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="nw_motor_joint">
<parent>chassis</parent>
<child>nw_motor</child>
</joint>
<joint type="fixed" name="nw_motor_joint">
<parent>chassis</parent>
<child>nw_motor</child>
</joint>
<link name="se_motor">
<pose>-0.12 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="se_motor">
<pose>-0.12 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="se_motor_joint">
<parent>chassis</parent>
<child>se_motor</child>
</joint>
<joint type="fixed" name="se_motor_joint">
<parent>chassis</parent>
<child>se_motor</child>
</joint>
<link name="ne_motor">
<pose>0.12 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="ne_motor">
<pose>0.12 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="ne_motor_joint">
<parent>chassis</parent>
<child>ne_motor</child>
</joint>
<joint type="fixed" name="ne_motor_joint">
<parent>chassis</parent>
<child>ne_motor</child>
</joint>
<link name="sw_motor">
<pose>-0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="sw_motor">
<pose>-0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="sw_motor_joint">
<parent>chassis</parent>
<child>sw_motor</child>
</joint>
<joint type="fixed" name="sw_motor_joint">
<parent>chassis</parent>
<child>sw_motor</child>
</joint>
<!-- CAMERAS -->
<!-- CAMERAS -->
<link name="front_camera">
<pose>0.15 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="front_camera">
<update_rate>30.0</update_rate>
<camera name="front_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="front_camera_joint" type="fixed">
<parent>chassis</parent>
<child>front_camera</child>
</joint>
<link name="bottom_camera">
<pose>0 0 -.03 0 1.57 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="bottom_camera">
<update_rate>30.0</update_rate>
<camera name="bottom_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="bottom_camera_joint" type="fixed">
<parent>chassis</parent>
<child>bottom_camera</child>
</joint>
</model>
<link name="front_camera">
<pose>0.15 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="front_camera">
<update_rate>30.0</update_rate>
<camera name="front_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="front_camera_joint" type="fixed">
<parent>chassis</parent>
<child>front_camera</child>
</joint>
<link name="bottom_camera">
<pose>0 0 -.03 0 1.57 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="bottom_camera">
<update_rate>30.0</update_rate>
<camera name="bottom_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="bottom_camera_joint" type="fixed">
<parent>chassis</parent>
<child>bottom_camera</child>
</joint>
</model>
</sdf>
conf/simulator/gazebo/models/range_sensors/model.config
+16
View File
@@ -0,0 +1,16 @@
<?xml version="1.0"?>
<model>
<name>ARDrone2 (Paparazzi)</name>
<version>1.0</version>
<sdf version='1.6'>range_sensors.sdf</sdf>
<author>
<name>Kimberly McGuire</name>
<email>knmcguire@users.noreply.github.com</email>
</author>
<description>
Model for a simple single beam ray sensor construction, pointed in all 3D directions (up down, front back, left right)
</description>
</model>
conf/simulator/gazebo/models/range_sensors/range_sensors.sdf
+214
View File
@@ -0,0 +1,214 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="range_sensors">
<link name="base">
<inertial>
<mass>0.001</mass>
<inertia>
<ixx> 0.005 </ixx>
<iyy> 0.005 </iyy>
<izz> 0.010 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>.4 .4 .05</size>
</box>
</geometry>
</collision>
</link>
<!-- RANGE SENSORS -->
<link name="down_range_sensor">
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type= "ray" name="down_range_sensor">
<pose>0 0 0 0 1.57 0</pose>
<visualize>true</visualize>
<update_rate>30</update_rate>
<ray>
<range>
<min>0.06</min>
<max>2</max>
<resolution>0.02</resolution>
</range>
</ray>
</sensor>
</link>
<joint name="down_range_sensor_joint" type="fixed">
<parent>base</parent>
<child>down_range_sensor</child>
</joint>
<link name="up_range_sensor">
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type= "ray" name="up_range_sensor">
<pose>0 0 0 0 -1.57 0</pose>
<visualize>true</visualize>
<update_rate>30</update_rate>
<ray>
<range>
<min>0.06</min>
<max>2</max>
<resolution>0.02</resolution>
</range>
</ray>
</sensor>
</link>
<joint name="up_range_sensor_joint" type="fixed">
<parent>base</parent>
<child>up_range_sensor</child>
</joint>
<link name="left_range_sensor">
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type= "ray" name="left_range_sensor">
<pose>0 0 0 0 0 1.57</pose>
<visualize>true</visualize>
<update_rate>30</update_rate>
<ray>
<range>
<min>0.22</min>
<max>2</max>
<resolution>0.02</resolution>
</range>
</ray>
</sensor>
</link>
<joint name="left_range_sensor_joint" type="fixed">
<parent>base</parent>
<child>left_range_sensor</child>
</joint>
<link name="right_range_sensor">
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type= "ray" name="right_range_sensor">
<pose>0 0 0. 0 0 -1.57</pose>
<visualize>true</visualize>
<update_rate>30</update_rate>
<ray>
<range>
<min>0.22</min>
<max>2</max>
<resolution>0.02</resolution>
</range>
</ray>
</sensor>
</link>
<joint name="right_range_sensor_joint" type="fixed">
<parent>base</parent>
<child>right_range_sensor</child>
</joint>
<link name="front_range_sensor">
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type= "ray" name="front_range_sensor">
<pose>0 0 0. 0 0 0</pose>
<visualize>true</visualize>
<update_rate>30</update_rate>
<ray>
<range>
<min>0.22</min>
<max>2</max>
<resolution>0.02</resolution>
</range>
</ray>
</sensor>
</link>
<joint name="front_range_sensor_joint" type="fixed">
<parent>base</parent>
<child>front_range_sensor</child>
</joint>
<link name="back_range_sensor">
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>.0001</ixx>
<iyy>.0001</iyy>
<izz>.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type= "ray" name="back_range_sensor">
<pose>0 0 0. 0 0 3.14</pose>
<visualize>true</visualize>
<update_rate>30</update_rate>
<ray>
<range>
<min>0.22</min>
<max>2</max>
<resolution>0.02</resolution>
</range>
</ray>
</sensor>
</link>
<joint name="back_range_sensor" type="fixed">
<parent>base</parent>
<child>back_range_sensor</child>
</joint>
</model>
</sdf>
conf/simulator/gazebo/models/simple_quad/simple_quad.sdf
+232 -232
View File
@@ -1,249 +1,249 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="simple_quad">
<pose>0 0 .1 0 0 0</pose>
<model name="simple_quad">
<pose>0 0 .1 0 0 0</pose>
<link name="chassis">
<velocity_decay>
<linear>0.001</linear>
</velocity_decay>
<inertial><!-- Converted to SI from Paparazzi's ARDrone model for JSBsim -->
<mass>0.4</mass>
<inertia>
<ixx> 0.00678 </ixx>
<iyy> 0.00678 </iyy>
<izz> 0.01356 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<link name="chassis">
<velocity_decay>
<linear>0.001</linear>
</velocity_decay>
<inertial><!-- Converted to SI from Paparazzi's ARDrone model for JSBsim -->
<mass>0.4</mass>
<inertia>
<ixx> 0.00678 </ixx>
<iyy> 0.00678 </iyy>
<izz> 0.01356 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 0.05</size>
</box>
</geometry>
</visual>
<sensor name="contactsensor" type="contact">
<contact>
<collision>collision</collision>
</contact>
</sensor>
</link>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 0.05</size>
</box>
</geometry>
</visual>
<sensor name="contactsensor" type="contact">
<contact>
<collision>collision</collision>
</contact>
</sensor>
</link>
<!-- MOTORS -->
<link name="front_motor">
<pose>0.12 0 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<!-- MOTORS -->
<link name="front_motor">
<pose>0.12 0 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="front_motor_joint">
<parent>chassis</parent>
<child>front_motor</child>
</joint>
<joint type="fixed" name="front_motor_joint">
<parent>chassis</parent>
<child>front_motor</child>
</joint>
<link name="back_motor">
<pose>-0.12 0 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="back_motor">
<pose>-0.12 0 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="back_motor_joint">
<parent>chassis</parent>
<child>back_motor</child>
</joint>
<joint type="fixed" name="back_motor_joint">
<parent>chassis</parent>
<child>back_motor</child>
</joint>
<link name="right_motor">
<pose>0 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="right_motor">
<pose>0 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="right_motor_joint">
<parent>chassis</parent>
<child>right_motor</child>
</joint>
<joint type="fixed" name="right_motor_joint">
<parent>chassis</parent>
<child>right_motor</child>
</joint>
<link name="left_motor">
<pose>0 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="left_motor">
<pose>0 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="left_motor_joint">
<parent>chassis</parent>
<child>left_motor</child>
</joint>
<joint type="fixed" name="left_motor_joint">
<parent>chassis</parent>
<child>left_motor</child>
</joint>
<!-- CAMERAS -->
<!-- CAMERAS -->
<link name="front_camera">
<pose>0.15 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="front_camera">
<update_rate>30.0</update_rate>
<camera name="front_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="front_camera_joint" type="fixed">
<parent>chassis</parent>
<child>front_camera</child>
</joint>
<link name="bottom_camera">
<pose>0 0 -.03 0 1.57 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="bottom_camera">
<update_rate>30.0</update_rate>
<camera name="bottom_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="bottom_camera_joint" type="fixed">
<parent>chassis</parent>
<child>bottom_camera</child>
</joint>
</model>
<link name="front_camera">
<pose>0.15 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="front_camera">
<update_rate>30.0</update_rate>
<camera name="front_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="front_camera_joint" type="fixed">
<parent>chassis</parent>
<child>front_camera</child>
</joint>
<link name="bottom_camera">
<pose>0 0 -.03 0 1.57 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="bottom_camera">
<update_rate>30.0</update_rate>
<camera name="bottom_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="bottom_camera_joint" type="fixed">
<parent>chassis</parent>
<child>bottom_camera</child>
</joint>
</model>
</sdf>
conf/simulator/gazebo/models/simple_x_quad/simple_x_quad.sdf
+232 -232
View File
@@ -1,249 +1,249 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="simple_x_quad">
<pose>0 0 .1 0 0 0</pose>
<model name="simple_x_quad">
<pose>0 0 .1 0 0 0</pose>
<link name="chassis">
<velocity_decay>
<linear>0.001</linear>
</velocity_decay>
<inertial><!-- Converted to SI from Paparazzi's ARDrone model for JSBsim -->
<mass>0.4</mass>
<inertia>
<ixx> 0.00678 </ixx>
<iyy> 0.00678 </iyy>
<izz> 0.01356 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<link name="chassis">
<velocity_decay>
<linear>0.001</linear>
</velocity_decay>
<inertial><!-- Converted to SI from Paparazzi's ARDrone model for JSBsim -->
<mass>0.4</mass>
<inertia>
<ixx> 0.00678 </ixx>
<iyy> 0.00678 </iyy>
<izz> 0.01356 </izz>
<ixy> 0. </ixy>
<ixz> 0. </ixz>
<iyz> 0. </iyz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
<collision name="collision">
<geometry>
<box>
<size>0.4 0.4 0.05</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 0.05</size>
</box>
</geometry>
</visual>
<sensor name="contactsensor" type="contact">
<contact>
<collision>collision</collision>
</contact>
</sensor>
</link>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 0.05</size>
</box>
</geometry>
</visual>
<sensor name="contactsensor" type="contact">
<contact>
<collision>collision</collision>
</contact>
</sensor>
</link>
<!-- MOTORS -->
<link name="nw_motor">
<pose>0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<!-- MOTORS -->
<link name="nw_motor">
<pose>0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="nw_motor_joint">
<parent>chassis</parent>
<child>nw_motor</child>
</joint>
<joint type="fixed" name="nw_motor_joint">
<parent>chassis</parent>
<child>nw_motor</child>
</joint>
<link name="se_motor">
<pose>-0.12 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="se_motor">
<pose>-0.12 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="se_motor_joint">
<parent>chassis</parent>
<child>se_motor</child>
</joint>
<joint type="fixed" name="se_motor_joint">
<parent>chassis</parent>
<child>se_motor</child>
</joint>
<link name="ne_motor">
<pose>0.12 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="ne_motor">
<pose>0.12 -0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="ne_motor_joint">
<parent>chassis</parent>
<child>ne_motor</child>
</joint>
<joint type="fixed" name="ne_motor_joint">
<parent>chassis</parent>
<child>ne_motor</child>
</joint>
<link name="sw_motor">
<pose>-0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.07</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="sw_motor">
<pose>-0.12 0.12 0 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.10</radius>
<length>0.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="fixed" name="sw_motor_joint">
<parent>chassis</parent>
<child>sw_motor</child>
</joint>
<joint type="fixed" name="sw_motor_joint">
<parent>chassis</parent>
<child>sw_motor</child>
</joint>
<!-- CAMERAS -->
<!-- CAMERAS -->
<link name="front_camera">
<pose>0.15 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="front_camera">
<update_rate>30.0</update_rate>
<camera name="front_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="front_camera_joint" type="fixed">
<parent>chassis</parent>
<child>front_camera</child>
</joint>
<link name="bottom_camera">
<pose>0 0 -.03 0 1.57 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="bottom_camera">
<update_rate>30.0</update_rate>
<camera name="bottom_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="bottom_camera_joint" type="fixed">
<parent>chassis</parent>
<child>bottom_camera</child>
</joint>
</model>
<link name="front_camera">
<pose>0.15 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="front_camera">
<update_rate>30.0</update_rate>
<camera name="front_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="front_camera_joint" type="fixed">
<parent>chassis</parent>
<child>front_camera</child>
</joint>
<link name="bottom_camera">
<pose>0 0 -.03 0 1.57 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<iyy>0.0001</iyy>
<izz>0.0001</izz>
<ixy>0</ixy>
<iyz>0</iyz>
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type="camera" name="bottom_camera">
<update_rate>30.0</update_rate>
<camera name="bottom_camera">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame. That pixel's
noise value is added to each of its color channels, which at that point lie
in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
</sensor>
</link>
<joint name="bottom_camera_joint" type="fixed">
<parent>chassis</parent>
<child>bottom_camera</child>
</joint>
</model>
</sdf>
conf/simulator/gazebo/world/empty.world
+34 -34
View File
@@ -1,38 +1,38 @@
<?xml version='1.0'?>
<sdf version="1.6">
<world name="empty">
<physics type="ode">
<max_step_size>0.001</max_step_size>
<real_time_update_rate>0</real_time_update_rate><!-- Handled by Paparazzi! -->
</physics>
<scene>
<ambient>1 1 1 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>0</shadows>
</scene>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose frame=''>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.5 -1</direction>
</light>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>51.9906340</latitude_deg>
<longitude_deg>4.3767889</longitude_deg>
<elevation>45.110</elevation>
<heading_deg>180</heading_deg><!-- Temporary fix for issue https://bitbucket.org/osrf/gazebo/issues/2022/default-sphericalcoordinates-frame-should -->
</spherical_coordinates>
<world name="empty">
<physics type="ode">
<max_step_size>0.001</max_step_size>
<real_time_update_rate>0</real_time_update_rate><!-- Handled by Paparazzi! -->
</physics>
<scene>
<ambient>1 1 1 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>0</shadows>
</scene>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose frame=''>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.5 -1</direction>
</light>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>51.9906340</latitude_deg>
<longitude_deg>4.3767889</longitude_deg>
<elevation>45.110</elevation>
<heading_deg>180</heading_deg><!-- Temporary fix for issue https://bitbucket.org/osrf/gazebo/issues/2022/default-sphericalcoordinates-frame-should -->
</spherical_coordinates>
<include>
<uri>model://ground_plane</uri>
</include>
</world>
<include>
<uri>model://ground_plane</uri>
</include>
</world>
</sdf>
conf/simulator/gazebo/world/square.world
+232
View File
@@ -0,0 +1,232 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from imav_indoor.world.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<sdf version="1.6">
<world name="square_world">
<light name="sun" type="directional">
<cast_shadows>1</cast_shadows>
<pose>0 10 0 0 0 -1.57079632679</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>51.9906340</latitude_deg>
<longitude_deg>4.3767889</longitude_deg>
<elevation>45.110</elevation>
<heading_deg>180</heading_deg><!-- Temporary fix for issue https://bitbucket.org/osrf/gazebo/issues/2022/default-sphericalcoordinates-frame-should -->
</spherical_coordinates>
<physics type="ode">
<max_step_size>0.001</max_step_size>
<real_time_update_rate>0</real_time_update_rate><!-- Handled by Paparazzi! -->
</physics>
<gravity>0 0 -9.8</gravity>
<gui fullscreen="0">
<camera name="user_camera">
<pose>1.49016 -3.72367 3.62379 -2.88294e-17 0.273797 1.4042</pose>
<view_controller>orbit</view_controller>
</camera>
</gui>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<sky>
<clouds>
<speed>12</speed>
</clouds>
</sky>
<shadows>0</shadows>
<grid>0</grid>
</scene>
<model name="floor">
<static>1</static>
<pose>0.0 0.0 0 0 0 0</pose>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>10 10</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
</friction>
<bounce/>
<contact>
<ode/>
</contact>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name="visual">
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>15 15</size>
</plane>
</geometry>
<material>
<script>
<name>Kitchen/Grass</name>
</script>
</material>
</visual>
<velocity_decay>
<linear>0</linear>
<angular>0</angular>
</velocity_decay>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
<model name="wall1">
<static>true</static>
<pose>0 7.5 2.5 0 0 0</pose>
<link name="wall_link">
<inertial>
<mass>10000</mass>
</inertial>
<collision name="wall_brand_collision">
<geometry>
<box>
<size>15 0.2 5.0</size>
</box>
</geometry>
<surface>
<contact>
<collide_bitmask>0x02</collide_bitmask>
</contact>
</surface>
</collision>
<visual name="wall_brand_visual">
<geometry>
<box>
<size>15 0.2 5.0</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Bricks</name>
</script>
</material>
</visual>
</link>
</model>
<model name="wall2">
<static>true</static>
<pose>-7.5 0 2.5 0 0 -1.5708</pose>
<link name="wall_link">
<inertial>
<mass>10000</mass>
</inertial>
<collision name="wall_collision">
<geometry>
<box>
<size>15 0.2 5.0</size>
</box>
</geometry>
<surface>
<contact>
<collide_bitmask>0x02</collide_bitmask>
</contact>
</surface>
</collision>
<visual name="wall_visual">
<geometry>
<box>
<size>15 0.2 5.0</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Bricks</name>
</script>
</material>
</visual>
</link>
</model>
<model name="wall3">
<static>true</static>
<pose>7.5 0 2.5 0 0 -1.5708</pose>
<link name="wall_link">
<inertial>
<mass>10000</mass>
</inertial>
<collision name="wall_collision">
<geometry>
<box>
<size>15 0.2 5.0</size>
</box>
</geometry>
<surface>
<contact>
<collide_bitmask>0x02</collide_bitmask>
</contact>
</surface>
</collision>
<visual name="wall_visual">
<geometry>
<box>
<size>15 0.2 5.0</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Bricks</name>
</script>
</material>
</visual>
</link>
</model>
<model name="wall4">
<static>true</static>
<pose>0 -7.5 2.5 0 0 0</pose>
<link name="wall_link">
<inertial>
<mass>10000</mass>
</inertial>
<collision name="wall_collision">
<geometry>
<box>
<size>15 0.2 5.0</size>
</box>
</geometry>
<surface>
<contact>
<collide_bitmask>0x02</collide_bitmask>
</contact>
</surface>
</collision>
<visual name="wall_visual">
<geometry>
<box>
<size>15 0.2 5.0</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Bricks</name>
</script>
</material>
</visual>
</link>
</model>
</world>
</sdf>
conf/userconf/tudelft/km_conf.xml
+28 -6
View File
@@ -1,4 +1,26 @@
<conf>
<aircraft
name="ARDrone2_opticflow"
ac_id="12"
airframe="airframes/tudelft/ardrone2_opticflow.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/video_capture.xml modules/cv_opticflow.xml modules/gps_ubx_ucenter.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="red"
/>
<aircraft
name="Bebop_opticflow"
ac_id="14"
airframe="airframes/tudelft/bebop_opticflow.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/video_capture.xml modules/cv_opticflow.xml modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="blue"
/>
<aircraft
name="LadyLisaBluegiga"
ac_id="114"
@@ -7,19 +29,19 @@
telemetry="telemetry/default_rotorcraft_slow.xml"
flight_plan="flight_plans/rotorcraft_optitrack.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml modules/stereocam.xml"
gui_color="blue"
release="4f95168b07005474c8fedf3be5c2f2b4f4c7c393"
/>
<aircraft
name="LadybirdMXS_wifi_indi_stereocam"
name="OrigamiMXS_wifi_indi_stereocam"
ac_id="15"
airframe="airframes/tudelft/ladybird_lisamxs_wifi_indi_stereoboard.xml"
airframe="airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_optitrack.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_guidance.xml"
settings_modules="modules/air_data.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
flight_plan="flight_plans/TUDELFT/tudelft_rotorcraft_guided_flightplan.xml"
settings="settings/rotorcraft_basic.xml settings/nps.xml"
settings_modules="modules/range_forcefield.xml modules/air_data.xml modules/ins_hff_extended.xml modules/ahrs_int_cmpl_quat.xml modules/gps.xml modules/imu_common.xml modules/stabilization_int_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml"
gui_color="blue"
release="c11f888de1eba08d751f1ff0b03bf4a5bec8e8fa"
/>
sw/airborne/firmwares/rotorcraft/navigation.h
+1
View File
@@ -255,6 +255,7 @@ static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
#define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
#define CloseDegAngles(_c1, _c2) ({ int32_t _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
#define CloseRadAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })
/** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/
#define NavQdrCloseTo(x) CloseDegAngles(((x) >> INT32_ANGLE_FRAC), NavCircleQdr())
#define NavCourseCloseTo(x) {}
sw/airborne/math/pprz_algebra.h
+4 -1
View File
@@ -531,7 +531,7 @@ extern "C" {
(_q).qz = (_z); \
}
/* _qc = _qa - _qc */
/* _qc = _qa - _qb */
#define QUAT_DIFF(_qc, _qa, _qb) { \
(_qc).qi = (_qa).qi - (_qb).qi; \
(_qc).qx = (_qa).qx - (_qb).qx; \
@@ -593,6 +593,9 @@ extern "C" {
(_qo).qz = (_qi).qz / (_s); \
}
/* return = _qa * _qb */
#define QUAT_DOT_PRODUCT(_qa, _qb) ((_qa).qi * (_qb).qi + (_qa).qx * (_qb).qx + (_qa).qy * (_qb).qy + (_qa).qz * (_qb).qz)
//
//
// Rotation Matrices
sw/airborne/math/pprz_algebra_double.c
+10
View File
@@ -150,6 +150,16 @@ void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b, struct
vb->z = m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z;
}
/** rotate 3D vector by transposed rotation matrix.
* vb = m_b2a^T * va
*/
void double_rmat_transp_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_b2a, struct DoubleVect3 *va)
{
vb->x = m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z;
vb->y = m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z;
vb->z = m_b2a->m[2] * va->x + m_b2a->m[5] * va->y + m_b2a->m[8] * va->z;
}
/* C n->b rotation matrix */
void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q)
{
sw/airborne/math/pprz_algebra_double.h
+7
View File
@@ -179,6 +179,13 @@ extern void double_rmat_comp(struct DoubleRMat *m_a2c, struct DoubleRMat *m_a2b,
*/
extern void double_rmat_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_a2b,
struct DoubleVect3 *va);
/** rotate 3D vector by transposed rotation matrix.
* vb = m_b2a^T * va
*/
extern void double_rmat_transp_vmult(struct DoubleVect3 *vb, struct DoubleRMat *m_b2a,
struct DoubleVect3 *va);
extern void double_rmat_of_quat(struct DoubleRMat *rm, struct DoubleQuat *q);
static inline void double_rmat_of_eulers(struct DoubleRMat *rm, struct DoubleEulers *e)
{
sw/airborne/math/pprz_stat.c
+14 -5
View File
@@ -91,12 +91,12 @@ int32_t covariance_i(int32_t *array1, int32_t *array2, uint32_t n_elements)
* Float implementations
*********/
/** Compute the mean value of an array (float)
/** Compute the sum array elements (float)
* @param[in] *array The array
* @param[in] n_elements Number of elements in the array
* @return mean
* @param[in] n_elements Number of elements in the arrays
* @return array sum
*/
float mean_f(float *array, uint32_t n_elements)
float sum_f(float *array, uint32_t n_elements)
{
// determine the mean for the vector:
float sum = 0.f;
@@ -104,8 +104,17 @@ float mean_f(float *array, uint32_t n_elements)
for (i = 0; i < n_elements; i++) {
sum += array[i];
}
return sum;
}
return (sum / n_elements);
/** Compute the mean value of an array (float)
* @param[in] *array The array
* @param[in] n_elements Number of elements in the array
* @return mean
*/
float mean_f(float *array, uint32_t n_elements)
{
return (sum_f(array, n_elements) / n_elements);
}
/** Compute the variance of an array of values (float).
sw/airborne/math/pprz_stat.h
+8
View File
@@ -63,6 +63,14 @@ extern int32_t variance_i(int32_t *array, uint32_t n_elements);
*/
extern int32_t covariance_i(int32_t *array1, int32_t *array2, uint32_t n_elements);
/** Compute the sum array elements
* @param[in] *array The first array
* @param[in] n_elements Number of elements in the arrays
* @return array sum
*/
extern float sum_f(float *array, uint32_t n_elements);
/** Compute the mean value of an array (float)
* @param[in] *array The array
* @param[in] n_elements Number of elements in the array
sw/airborne/modules/computer_vision/colorfilter.c
+6 -5
View File
@@ -71,12 +71,13 @@ struct image_t *colorfilter_func(struct image_t *img)
if (COLORFILTER_SEND_OBSTACLE) {
if (color_count > 20)
AbiSendMsgOBSTACLE_DETECTION(CV_COLORDETECTION, 1,
0);
{
AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_COLOR_ID, 1.f, 0.f, 0.f);
}
else
AbiSendMsgOBSTACLE_DETECTION(CV_COLORDETECTION, 10,
0);
{
AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_COLOR_ID, 10.f, 0.f, 0.f);
}
}
return img; // Colorfilter did not make a new image
sw/airborne/modules/range_finder/laser_range_array.c
+114
View File
@@ -0,0 +1,114 @@
/*
* Copyright (C) 2017 K. N. McGuire
*
* 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/range_finder/laser_range_array.c"
* @author K. N. McGuire
* Reads out values through uart of an laser range ring (array), containing multiple ToF IR laser range modules
*/
#include "modules/range_finder/laser_range_array.h"
#include "generated/airframe.h"
#include "pprzlink/pprz_transport.h"
#include "pprzlink/intermcu_msg.h"
#include "mcu_periph/uart.h"
#include "subsystems/abi.h"
#include "message_pragmas.h"
#include "pprzlink/messages.h"
#include "subsystems/datalink/downlink.h"
/* Main device strcuture */
struct laser_range_array_t {
struct link_device *device; ///< The device which is uses for communication
struct pprz_transport transport; ///< The transport layer (PPRZ)
bool msg_available; ///< If we received a message
};
static struct laser_range_array_t laser_range_array = {
.device = (&((LASER_RANGE_ARRAY_PORT).device)),
.msg_available = false
};
static uint8_t lra_msg_buf[128] __attribute__((aligned)); ///< The message buffer
PRINT_CONFIG_VAR(LASER_RANGE_ARRAY_NUM_SENSORS)
static float laser_range_array_orientations[] = LASER_RANGE_ARRAY_ORIENTATIONS;
static uint8_t agl_id = 255;
#define VL53L0_MAX_VAL 8.191f
void laser_range_array_init(void)
{
// Initialize transport protocol
pprz_transport_init(&laser_range_array.transport);
#if LASER_RANGE_ARRAY_SEND_AGL
// find sensor looking down
for (int k = 0; k < LASER_RANGE_ARRAY_NUM_SENSORS; k++) {
if (fabsf(laser_range_array_orientations[k * 2] + M_PI_2) < RadOfDeg(5)) {
agl_id = k;
break;
}
}
#endif
}
/* Parse the InterMCU message */
static void laser_range_array_parse_msg(void)
{
uint8_t msg_id = lra_msg_buf[1];
// Get Time of Flight laser range sensor ring messages
switch (msg_id) {
case DL_IMCU_REMOTE_GROUND: {
uint8_t id = DL_IMCU_REMOTE_GROUND_id(lra_msg_buf);
if (id < LASER_RANGE_ARRAY_NUM_SENSORS) {
float range = DL_IMCU_REMOTE_GROUND_range(lra_msg_buf) / 1000.f;
// wait till all sensors received before sending update
AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_RANGE_ARRAY_ID, range, laser_range_array_orientations[id*2],
laser_range_array_orientations[id*2 + 1]);
if (id == agl_id && range > 1e-5 && range < VL53L0_MAX_VAL) {
AbiSendMsgAGL(AGL_VL53L0_LASER_ARRAY_ID, range);
}
}
break;
}
default:
break;
}
}
void laser_range_array_event(void)
{
pprz_check_and_parse(laser_range_array.device, &laser_range_array.transport, lra_msg_buf,
&laser_range_array.msg_available);
if (laser_range_array.msg_available) {
laser_range_array_parse_msg();
laser_range_array.msg_available = false;
}
}
sw/airborne/modules/range_finder/laser_range_array.h
+33
View File
@@ -0,0 +1,33 @@
/*
* Copyright (C) 2017 K. N. McGuire
*
* 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/range_finder/laser_range_array.h"
* @author K. N. McGuire
* Reads out values through uart of an laser range ring (array), containing multiple ToF IR laser range modules
*/
#ifndef LASER_RANGE_ARRAY_H
#define LASER_RANGE_ARRAY_H
extern void laser_range_array_init(void);
extern void laser_range_array_event(void);
#endif
sw/airborne/modules/range_forcefield/range_forcefield.c
+180
View File
@@ -0,0 +1,180 @@
/*
* Copyright (C) 2017 K. N. McGuire
*
* 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/range_forcefield/range_forcefield.c"
* @author K. N. McGuire
* This module generates a forcefield based on range sensor measurements the use of single point range sensors.
*/
#include "modules/range_forcefield/range_forcefield.h"
#include "subsystems/abi.h"
//abi for range sensors
#ifndef RANGE_FORCEFIELD_RECIEVE_ID
#define RANGE_FORCEFIELD_RECIEVE_ID ABI_BROADCAST
#endif
#ifndef RANGE_FORCEFIELD_INNER_LIMIT
#define RANGE_FORCEFIELD_INNER_LIMIT 1.0f
#endif
PRINT_CONFIG_VAR(RANGE_FORCEFIELD_INNER_LIMIT)
#ifndef RANGE_FORCEFIELD_OUTER_LIMIT
#define RANGE_FORCEFIELD_OUTER_LIMIT 1.4f
#endif
PRINT_CONFIG_VAR(RANGE_FORCEFIELD_OUTER_LIMIT)
#ifndef RANGE_FORCEFIELD_MIN_VEL
#define RANGE_FORCEFIELD_MIN_VEL 0.0f
#endif
PRINT_CONFIG_VAR(RANGE_FORCEFIELD_MIN_VEL)
#ifndef RANGE_FORCEFIELD_MAX_VEL
#define RANGE_FORCEFIELD_MAX_VEL 0.5f
#endif
PRINT_CONFIG_VAR(RANGE_FORCEFIELD_MAX_VEL)
struct range_forcefield_param_t range_forcefield_param;
static struct FloatVect3 ff_nearest_obs_pos; // nearest obstacle on positive axis
static struct FloatVect3 ff_nearest_obs_neg; // nearest obstacle on negative axis
static float compute_ff_vel(float range);
static void store_min_dist(float pos, float *nearest_pos, float range);
static abi_event obstacle_ev;
static void obstacle_cb(uint8_t sender_id __attribute__((unused)), float range, float azimuth,
float bearing)
{
static struct FloatEulers body_to_sensors_eulers;
body_to_sensors_eulers.phi = 0;
body_to_sensors_eulers.theta = azimuth;
body_to_sensors_eulers.psi = bearing;
range_forcefield_update(range, &body_to_sensors_eulers);
}
void range_forcefield_init(void)
{
range_forcefield_param.inner_limit = RANGE_FORCEFIELD_INNER_LIMIT;
range_forcefield_param.outer_limit = RANGE_FORCEFIELD_OUTER_LIMIT;
range_forcefield_param.min_vel = RANGE_FORCEFIELD_MIN_VEL;
range_forcefield_param.max_vel = RANGE_FORCEFIELD_MAX_VEL;
ff_nearest_obs_pos.x = RANGE_FORCEFIELD_OUTER_LIMIT;
ff_nearest_obs_pos.y = RANGE_FORCEFIELD_OUTER_LIMIT;
ff_nearest_obs_pos.z = RANGE_FORCEFIELD_OUTER_LIMIT;
ff_nearest_obs_neg.x = -RANGE_FORCEFIELD_OUTER_LIMIT;
ff_nearest_obs_neg.y = -RANGE_FORCEFIELD_OUTER_LIMIT;
ff_nearest_obs_neg.z = -RANGE_FORCEFIELD_OUTER_LIMIT;
AbiBindMsgOBSTACLE_DETECTION(RANGE_FORCEFIELD_RECIEVE_ID, &obstacle_ev, obstacle_cb);
}
/* Compute range forcefield and send abi message
*
*/
void range_forcefield_periodic(void)
{
static struct FloatVect3 ff_vel_body;
ff_vel_body.x = compute_ff_vel(ff_nearest_obs_pos.x) - compute_ff_vel(fabsf(ff_nearest_obs_neg.x));
ff_vel_body.y = compute_ff_vel(ff_nearest_obs_pos.y) - compute_ff_vel(fabsf(ff_nearest_obs_neg.y));
ff_vel_body.z = compute_ff_vel(ff_nearest_obs_pos.z) - compute_ff_vel(fabsf(ff_nearest_obs_neg.z));
AbiSendMsgRANGE_FORCEFIELD(RANGE_FORCEFIELD_ID, ff_vel_body.x, ff_vel_body.y, ff_vel_body.z);
// apply forgetting factor to forcefield, handles if obstacle no longer in sight / updated
ff_nearest_obs_pos.x = ff_nearest_obs_pos.x < RANGE_FORCEFIELD_OUTER_LIMIT ? ff_nearest_obs_pos.x*1.1 : RANGE_FORCEFIELD_OUTER_LIMIT;
ff_nearest_obs_pos.y = ff_nearest_obs_pos.y < RANGE_FORCEFIELD_OUTER_LIMIT ? ff_nearest_obs_pos.y*1.1 : RANGE_FORCEFIELD_OUTER_LIMIT;
ff_nearest_obs_pos.z = ff_nearest_obs_pos.z < RANGE_FORCEFIELD_OUTER_LIMIT ? ff_nearest_obs_pos.z*1.1 : RANGE_FORCEFIELD_OUTER_LIMIT;
ff_nearest_obs_neg.x = ff_nearest_obs_neg.x > -RANGE_FORCEFIELD_OUTER_LIMIT ? ff_nearest_obs_neg.x*1.1 : -RANGE_FORCEFIELD_OUTER_LIMIT;
ff_nearest_obs_neg.y = ff_nearest_obs_neg.y > -RANGE_FORCEFIELD_OUTER_LIMIT ? ff_nearest_obs_neg.y*1.1 : -RANGE_FORCEFIELD_OUTER_LIMIT;
ff_nearest_obs_neg.z = ff_nearest_obs_neg.z > -RANGE_FORCEFIELD_OUTER_LIMIT ? ff_nearest_obs_neg.z*1.1 : -RANGE_FORCEFIELD_OUTER_LIMIT;
}
/* range_sensor_update_forcefield: This stores the range sensor measurement in
*
* @param[in] range, input range measurement
* @param[in] body_to_sensors_eulers, euler angles of the orientation of the range sensor in question[rad]
* */
void range_forcefield_update(float range, struct FloatEulers *body_to_sensor_eulers)
{
// generate vector to obstacle location based on range and rotation
struct FloatVect3 obs_loc, range_vec = {range, 0, 0};
struct FloatRMat range_sensor_to_body;
float_rmat_of_eulers(&range_sensor_to_body, body_to_sensor_eulers);
float_rmat_transp_vmult(&obs_loc, &range_sensor_to_body, &range_vec);
// store closest distance in each axis
store_min_dist(obs_loc.x, &(ff_nearest_obs_pos.x), range);
store_min_dist(obs_loc.x, &(ff_nearest_obs_neg.x), range);
store_min_dist(obs_loc.y, &(ff_nearest_obs_pos.y), range);
store_min_dist(obs_loc.y, &(ff_nearest_obs_neg.y), range);
store_min_dist(obs_loc.z, &(ff_nearest_obs_pos.z), range);
store_min_dist(obs_loc.z, &(ff_nearest_obs_neg.z), range);
}
/* Compute the forcefield velocity command given the distance to the obstacle and the forcefield parameters
* @param range Distance to obstacle
* @param ff_params The forcefield settings to generate the velocity
*/
static float compute_ff_vel(float range)
{
float ff_vel = 0.f;
// Calculate avoidance velocity
if (range < 0.001f) {
//do nothing
} else if (range < range_forcefield_param.inner_limit) {
ff_vel = -range_forcefield_param.max_vel;
} else if (range < range_forcefield_param.outer_limit) {
// Linear
ff_vel = -(range_forcefield_param.max_vel - range_forcefield_param.min_vel) * (range_forcefield_param.outer_limit - range)
/ (range_forcefield_param.outer_limit - range_forcefield_param.inner_limit);
}
return ff_vel;
}
/* Store the minimum directional distance
* pos directional position on axis
* nearest_pos pointer to store nearest position
* range magnitude of the range vector being stored
*/
static void store_min_dist(float pos, float *nearest_pos, float range)
{
// check if obstacle in range for this axis
// 1/2 of total distance vector relates to an angle of 60deg in plane
if (fabsf(pos) >= range / 2.f)
{
// if obstacle should be considered in x axis, remember extremum in either direction
if (pos > 0.f && pos < *nearest_pos)
{
*nearest_pos = pos;
} else if (pos < 0.f && pos > *nearest_pos)
{
*nearest_pos = pos;
}
}
}
sw/airborne/modules/range_forcefield/range_forcefield.h
+45
View File
@@ -0,0 +1,45 @@
/*
* Copyright (C) 2017 K. N. McGuire
*
* 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/range_forcefield/range_forcefield.h"
* @author K. N. McGuire
* This module generates a forcefield based on range sensor measurements the use of single point range sensors.
*/
#ifndef RANGE_FORCEFIELD_H
#define RANGE_FORCEFIELD_H
#include <std.h>
#include "math/pprz_algebra_float.h"
struct range_forcefield_param_t {
float inner_limit;
float outer_limit;
float min_vel;
float max_vel;
};
extern struct range_forcefield_param_t range_forcefield_param;
extern void range_forcefield_init(void);
extern void range_forcefield_periodic(void);
extern void range_forcefield_update(float range, struct FloatEulers *body_to_sensor_eulers);
#endif
sw/airborne/subsystems/abi_sender_ids.h
+26 -3
View File
@@ -123,6 +123,13 @@
#define AGL_LIDAR_SF11_ID 8
#endif
#ifndef AGL_VL53L0_LASER_ARRAY_ID
#define AGL_VL53L0_LASER_ARRAY_ID 9
#endif
#ifndef AGL_RAY_SENSOR_GAZEBO_ID
#define AGL_RAY_SENSOR_GAZEBO_ID 10
#endif
/*
* IDs of magnetometer sensors (including IMUs with mag)
*/
@@ -310,11 +317,27 @@
#endif
/*
* IDs of Computer Vision Calculated variables
* IDs of Obstacle detection systems
*/
#ifndef CV_COLORDETECTION
#define CV_COLORDETECTION 1
#ifndef OBS_DETECTION_COLOR_ID
#define OBS_DETECTION_COLOR_ID 1
#endif
#ifndef OBS_DETECTION_RANGE_ARRAY_ID
#define OBS_DETECTION_RANGE_ARRAY_ID 2
#endif
#ifndef OBS_DETECTION_RANGE_ARRAY_NPS_ID
#define OBS_DETECTION_RANGE_ARRAY_NPS_ID 3
#endif
/*
* ID's of forcefield generating type functions
*/
#ifndef RANGE_FORCEFIELD_ID
#define RANGE_FORCEFIELD_ID 1
#endif
sw/airborne/subsystems/ins/ins_int.c
+10 -8
View File
@@ -269,14 +269,16 @@ void ins_reset_local_origin(void)
void ins_reset_altitude_ref(void)
{
#if USE_GPS
struct LlaCoor_i lla = {
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
ltp_def_from_lla_i(&ins_int.ltp_def, &lla);
ins_int.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_int.ltp_def);
if (GpsFixValid()) {
struct LlaCoor_i lla = {
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
ltp_def_from_lla_i(&ins_int.ltp_def, &lla);
ins_int.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_int.ltp_def);
}
#endif
ins_int.vf_reset = true;
}
sw/simulator/nps/nps_fdm_gazebo.cpp
+187 -24
View File
@@ -76,7 +76,7 @@ using namespace std;
#endif
// Add video handling functions if req'd.
#ifdef NPS_SIMULATE_VIDEO
#if NPS_SIMULATE_VIDEO
extern "C" {
#include "modules/computer_vision/cv.h"
#include "modules/computer_vision/video_thread_nps.h"
@@ -97,14 +97,24 @@ static struct gazebocam_t gazebo_cams[VIDEO_THREAD_MAX_CAMERAS] =
{ { NULL, 0 } };
#endif
struct gazebo_actuators_t
{
struct gazebo_actuators_t {
string names[NPS_COMMANDS_NB];
double thrusts[NPS_COMMANDS_NB];
double torques[NPS_COMMANDS_NB];
};
struct gazebo_actuators_t gazebo_actuators = {NPS_ACTUATOR_NAMES, {NPS_ACTUATOR_THRUSTS}, {NPS_ACTUATOR_THRUSTS}};
struct gazebo_actuators_t gazebo_actuators = {NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_THRUSTS};
#if NPS_SIMULATE_LASER_RANGE_ARRAY
extern "C" {
#include "subsystems/abi.h"
}
static void gazebo_init_range_sensors(void);
static void gazebo_read_range_sensors(void);
#endif
/// Holds all necessary NPS FDM state information
struct NpsFdm fdm;
@@ -168,6 +178,15 @@ inline struct DoubleRates to_pprz_rates(gazebo::math::Vector3 body_g)
}
inline struct DoubleEulers to_pprz_eulers(gazebo::math::Quaternion quat)
{
struct DoubleEulers eulers;
eulers.psi = -quat.GetYaw();
eulers.theta = -quat.GetPitch();
eulers.phi = quat.GetRoll();
return eulers;
}
inline struct DoubleEulers to_global_pprz_eulers(gazebo::math::Quaternion quat)
{
struct DoubleEulers eulers;
eulers.psi = -quat.GetYaw() - M_PI / 2;
@@ -218,8 +237,11 @@ void nps_fdm_run_step(
if (!gazebo_initialized) {
init_gazebo();
gazebo_read();
#ifdef NPS_SIMULATE_VIDEO
#if NPS_SIMULATE_VIDEO
init_gazebo_video();
#endif
#if NPS_SIMULATE_LASER_RANGE_ARRAY
gazebo_init_range_sensors();
#endif
gazebo_initialized = true;
}
@@ -229,9 +251,13 @@ void nps_fdm_run_step(
gazebo::sensors::run_once();
gazebo_write(act_commands, commands_nb);
gazebo_read();
#ifdef NPS_SIMULATE_VIDEO
#if NPS_SIMULATE_VIDEO
gazebo_read_video();
#endif
#if NPS_SIMULATE_LASER_RANGE_ARRAY
gazebo_read_range_sensors();
#endif
}
// TODO Atmosphere functions have not been implemented yet.
@@ -290,16 +316,32 @@ static void init_gazebo(void)
gazebo::common::SystemPaths::Instance()->AddModelPaths(
gazebodir + "models/");
cout << "Load world: " << gazebodir + "world/" + NPS_GAZEBO_WORLD << endl;
// get vehicles
cout << "Load vehicle: " << gazebodir + "models/" + NPS_GAZEBO_AC_NAME + "/" + NPS_GAZEBO_AC_NAME + ".sdf" << endl;
sdf::SDFPtr vehicle_sdf(new sdf::SDF());
sdf::init(vehicle_sdf);
sdf::readFile(gazebodir + "models/" + NPS_GAZEBO_AC_NAME + "/" + NPS_GAZEBO_AC_NAME + ".sdf", vehicle_sdf);
// add sensors
#if NPS_SIMULATE_LASER_RANGE_ARRAY
vehicle_sdf->Root()->GetFirstElement()->AddElement("include")->GetElement("uri")->Set("model://range_sensors");
sdf::ElementPtr range_joint = vehicle_sdf->Root()->GetFirstElement()->AddElement("joint");
range_joint->GetAttribute("name")->Set("range_sensor_joint");
range_joint->GetAttribute("type")->Set("fixed");
range_joint->GetElement("parent")->Set("chassis");
range_joint->GetElement("child")->Set("range_sensors::base");
#endif
// get world
cout << "Load world: " << gazebodir + "world/" + NPS_GAZEBO_WORLD << endl;
sdf::SDFPtr world_sdf(new sdf::SDF());
sdf::init(world_sdf);
sdf::readFile(gazebodir + "world/" + NPS_GAZEBO_WORLD, world_sdf);
world_sdf->Root()->GetFirstElement()->AddElement("include")->GetElement("uri")->Set((string)"model://" + NPS_GAZEBO_AC_NAME);
world_sdf->Write(pprz_home + "/var/gazebo.world");
// add vehicles
world_sdf->Root()->GetFirstElement()->InsertElement(vehicle_sdf->Root()->GetFirstElement());
// TODO add sensors to the gazebo model in the same way as the vehicle.
world_sdf->Write(pprz_home + "/var/gazebo.world");
gazebo::physics::WorldPtr world = gazebo::loadWorld(pprz_home + "/var/gazebo.world");
if (!world) {
@@ -330,8 +372,7 @@ static void init_gazebo(void)
#ifdef MOTOR_MIXING_YAW_COEF
const double yaw_coef[] = MOTOR_MIXING_YAW_COEF;
for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++)
{
for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
gazebo_actuators.torques[i] = -fabs(gazebo_actuators.torques[i]) * yaw_coef[i] / fabs(yaw_coef[i]);
}
#endif
@@ -422,17 +463,12 @@ static void gazebo_read(void)
fdm.ltpprz_ecef_accel = fdm.ltp_ecef_accel; // ???
fdm.body_inertial_accel = fdm.body_ecef_accel; // Approximate, unused.
// only use accelerometer if no collisions or if not on ground
if (ct->Contacts().contact_size() || pose.pos.z < 0.03){
fdm.body_accel = to_pprz_body(
pose.rot.RotateVectorReverse(-world->Gravity()));
} else {
fdm.body_accel = to_pprz_body(
fdm.body_accel = to_pprz_body(
pose.rot.RotateVectorReverse(accel.Ign() - world->Gravity()));
}
/* attitude */
// ecef_to_body_quat: unused
fdm.ltp_to_body_eulers = to_pprz_eulers(local_to_global_quat * pose.rot);
fdm.ltp_to_body_eulers = to_global_pprz_eulers(local_to_global_quat * pose.rot);
double_quat_of_eulers(&(fdm.ltp_to_body_quat), &(fdm.ltp_to_body_eulers));
fdm.ltpprz_to_body_quat = fdm.ltp_to_body_quat; // unused
fdm.ltpprz_to_body_eulers = fdm.ltp_to_body_eulers; // unused
@@ -460,7 +496,7 @@ static void gazebo_read(void)
#else
// Gazebo versions < 8 do not have atmosphere or wind support
// Use placeholder values. Valid for low altitude, low speed flights.
fdm.wind = (struct DoubleVect3){0, 0, 0};
fdm.wind = (struct DoubleVect3) {0, 0, 0};
fdm.pressure_sl = 101325; // Pa
fdm.airspeed = 0;
@@ -509,7 +545,7 @@ static void gazebo_write(double act_commands[], int commands_nb)
}
}
#ifdef NPS_SIMULATE_VIDEO
#if NPS_SIMULATE_VIDEO
/**
* Set up cameras.
*
@@ -593,8 +629,8 @@ static void gazebo_read_video(void)
struct image_t img;
read_image(&img, cam);
#ifdef NPS_DEBUG_VIDEO
cv::Mat RGB_cam(cam->ImageHeight(),cam->ImageWidth(),CV_8UC3,(uint8_t *)cam->ImageData());
#if NPS_DEBUG_VIDEO
cv::Mat RGB_cam(cam->ImageHeight(), cam->ImageWidth(), CV_8UC3, (uint8_t *)cam->ImageData());
cv::cvtColor(RGB_cam, RGB_cam, cv::COLOR_RGB2BGR);
cv::namedWindow(cameras[i]->dev_name, cv::WINDOW_AUTOSIZE); // Create a window for display.
cv::imshow(cameras[i]->dev_name, RGB_cam);
@@ -653,6 +689,133 @@ static void read_image(
img->pprz_ts = ts.Double() * 1e6;
img->buf_idx = 0; // unused
}
#endif // NPS_SIMULATE_VIDEO
#if NPS_SIMULATE_LASER_RANGE_ARRAY
/*
* Simulate range sensors:
*
* In the airframe file, set NPS_SIMULATE_LASER_RANGE_ARRAY if you want to make use of the integrated Ray sensors.
* These are defined in their own model which is added to the ardrone.sdf (called range_sensors). Here you can add
* single ray point sensors with a specified position and orientation. It is also possible add noise.
*
* Within the airframe file (see e.g ardrone2_rangesensors_gazebo.xml), the amount of sensors
* (LASER_RANGE_ARRAY_NUM_SENSORS) and their orientations
* (LASER_RANGE_ARRAY_ORIENTATIONS={phi_1,theta_1,psi_1...phi_n,theta_n,psi_n} n = number of sensors) need to be
* specified as well. This is to keep it generic since this need to be done on the real platform with an external
* ray sensor. The function will compare the orientations from the ray sensors of gazebo, with the ones specified
* in the airframe, and will fill up an array to send and abi message to be used by other modules.
*
* NPS_GAZEBO_RANGE_SEND_AGL defines if the sensor that is defined as down should be used to send an AGL message.
* to send an OBSTACLE_DETECTION message.
*
* Functions:
* gazebo_init_range_sensors() -> Finds and initializes all ray sensors in gazebo
* gazebo_read_range_sensors() -> Reads and evaluates the ray sensors values, and sending it to other pprz modules
*/
struct gazebo_ray_t {
gazebo::sensors::RaySensorPtr sensor;
gazebo::common::Time last_measurement_time;
};
static struct gazebo_ray_t rays[LASER_RANGE_ARRAY_NUM_SENSORS] = {{NULL, 0}};
static float range_orientation[] = LASER_RANGE_ARRAY_ORIENTATIONS;
static uint8_t ray_sensor_agl_index = 255;
#define VL53L0_MAX_VAL 8.191f
static void gazebo_init_range_sensors(void)
{
gazebo::sensors::SensorManager *mgr = gazebo::sensors::SensorManager::Instance();
gazebo::sensors::Sensor_V sensors = mgr->GetSensors(); // list of all sensors
uint16_t sensor_count = model->GetSensorCount();
gazebo::sensors::RaySensorPtr ray_sensor;
uint8_t ray_sensor_count_selected = 0;
// Loop though all sensors and only select ray sensors, which are saved within a struct
for (uint16_t i = 0; i < sensor_count; i++) {
if (ray_sensor_count_selected > LASER_RANGE_ARRAY_NUM_SENSORS) {
break;
}
if (sensors.at(i)->Type() == "ray") {
ray_sensor = static_pointer_cast<gazebo::sensors::RaySensor>(sensors.at(i));
if (!ray_sensor) {
cout << "ERROR: Could not get pointer to raysensor " << i << "!" << endl;
continue;
}
// Read out the pose from per ray sensors in gazebo relative to body
struct DoubleEulers pose_sensor = to_pprz_eulers(ray_sensor->Pose().Rot());
struct DoubleQuat q_ray, q_def;
double_quat_of_eulers(&q_ray, &pose_sensor);
/* Check the orientations of the ray sensors found in gazebo, if they are similar (within 5 deg) to the orientations
* given in the airframe file in LASER_RANGE_ARRAY_RANGE_ORIENTATION
*/
for (int j = 0; j < LASER_RANGE_ARRAY_NUM_SENSORS; j++) {
struct DoubleEulers def = {0, range_orientation[j*2], range_orientation[j*2 + 1]};
double_quat_of_eulers(&q_def, &def);
// get angle between required angle and ray angle
double angle = acos(QUAT_DOT_PRODUCT(q_ray, q_def));
if (fabs(angle) < RadOfDeg(5)) {
ray_sensor_count_selected++;
rays[j].sensor = ray_sensor;
rays[j].sensor->SetActive(true);
#if LASER_RANGE_ARRAY_SEND_AGL
// find the sensor pointing down
if (fabs(range_orientation[j*2] + M_PI_2) < RadOfDeg(5)) {
ray_sensor_agl_index = j;
}
#endif
break;
}
}
}
}
if (ray_sensor_count_selected != LASER_RANGE_ARRAY_NUM_SENSORS) {
cout << "ERROR: you have defined " << LASER_RANGE_ARRAY_NUM_SENSORS << " sensors in your airframe file, but only "
<< ray_sensor_count_selected << " sensors have been found in the gazebo simulator, "
"with the same orientation as in the airframe file " << endl;
exit(0);
}
cout << "Initialized laser range array" << endl;
}
static void gazebo_read_range_sensors(void)
{
static float range;
// Loop through all ray sensors found in gazebo
for (int i = 0; i < LASER_RANGE_ARRAY_NUM_SENSORS; i++) {
if ((rays[i].sensor->LastMeasurementTime() - rays[i].last_measurement_time).Float() < 0.005
|| rays[i].sensor->LastMeasurementTime() == 0) { continue; }
if (rays[i].sensor->Range(0) < 1e-5 || rays[i].sensor->Range(0) > VL53L0_MAX_VAL) {
range = VL53L0_MAX_VAL;
} else {
range = rays[i].sensor->Range(0);
}
AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_RANGE_ARRAY_NPS_ID, range, range_orientation[i*2], range_orientation[i*2 + 1]);
if (i == ray_sensor_agl_index) {
float agl = rays[i].sensor->Range(0);
// Down range sensor as agl
if (agl > 1e-5 && !isinf(agl)) {
AbiSendMsgAGL(AGL_RAY_SENSOR_GAZEBO_ID, agl);
}
}
rays[i].last_measurement_time = rays[i].sensor->LastMeasurementTime();
}
}
#endif // NPS_SIMULATE_LASER_RANGE_ARRAY
#pragma GCC diagnostic pop // Ignore -Wdeprecated-declarations
sw/tools/generators/gen_flight_plan.ml
+21 -10
View File
@@ -818,7 +818,7 @@ let parse_wpt_sector = fun indexes waypoints xml ->
(** FP variables and ABI auto bindings *)
type fp_var = FP_var of (string * string * string) | FP_binding of (string * string list * string)
type fp_var = FP_var of (string * string * string) | FP_binding of (string * string list option * string * string option)
(* get a Hashtbl of ABI messages (name, field_types) *)
let extract_abi_msg = fun filename class_ ->
@@ -836,6 +836,9 @@ let extract_abi_msg = fun filename class_ ->
Not_found -> failwith (sprintf "No msg_class '%s' found" class_)
let parse_variables = fun xml ->
let some_attrib_or_none = fun v n cb ->
try Some (cb (ExtXml.attrib v n)) with _ -> None
in
List.map (fun var ->
match Xml.tag var with
| "variable" ->
@@ -845,10 +848,13 @@ let parse_variables = fun xml ->
FP_var (v, t, i)
| "abi_binding" ->
let n = ExtXml.attrib var "name"
and vs = ExtXml.attrib var "vars"
and i = ExtXml.attrib_or_default var "id" "ABI_BROADCAST" in
let vs = Str.split (Str.regexp "[ ]*,[ ]*") vs in
FP_binding (n, vs, i)
and vs = some_attrib_or_none var "vars" (fun x -> Str.split (Str.regexp "[ ]*,[ ]*") x)
and i = ExtXml.attrib_or_default var "id" "ABI_BROADCAST"
and h = some_attrib_or_none var "handler" (fun x -> x) in
begin match vs, h with
| Some _, Some _ | None, None -> failwith "Gen_flight_plan: either 'vars' or 'handler' should be defined, not both"
| _, _ -> FP_binding (n, vs, i, h)
end
| _ -> failwith "Gen_flight_plan: unexpected variables tag"
) xml
@@ -858,14 +864,17 @@ let print_var_decl abi_msgs = function
let print_var_impl abi_msgs = function
| FP_var (v, t, i) -> printf "%s %s = %s;\n" t v i
| FP_binding (n, vs, _) ->
| FP_binding (n, Some vs, _, None) ->
printf "static abi_event FP_%s_ev;\n" n;
let field_types = Hashtbl.find abi_msgs n in
List.iter2 (fun abi_t user -> if not (user = "_") then printf "static %s %s;\n" abi_t user) field_types vs
| FP_binding (n, None, _, Some _) ->
printf "static abi_event FP_%s_ev;\n" n
| _ -> ()
let print_auto_init_bindings = fun abi_msgs variables ->
let print_cb = function
| FP_binding (n, vs, _) ->
| FP_binding (n, Some vs, _, None) ->
let field_types = Hashtbl.find abi_msgs n in
printf "static void FP_%s_cb(uint8_t sender_id __attribute__((unused))" n;
List.iteri (fun i v ->
@@ -880,8 +889,10 @@ let print_auto_init_bindings = fun abi_msgs variables ->
| _ -> ()
in
let print_bindings = function
| FP_binding (n, _, i) ->
| FP_binding (n, _, i, None) ->
printf " AbiBindMsg%s(%s, &FP_%s_ev, FP_%s_cb);\n" n i n n
| FP_binding (n, _, i, Some h) ->
printf " AbiBindMsg%s(%s, &FP_%s_ev, %s);\n" n i n h
| _ -> ()
in
List.iter print_cb variables;
@@ -978,12 +989,12 @@ let () =
printf "#include \"std.h\"\n";
printf "#include \"generated/modules.h\"\n";
printf "#include \"subsystems/abi.h\"\n";
printf "#include \"autopilot.h\"\n";
printf "#include \"autopilot.h\"\n\n";
begin
try
let header = ExtXml.child (ExtXml.child xml "header") "0" in
printf "%s\n" (Xml.pcdata header)
printf "%s\n\n" (Xml.pcdata header)
with _ -> ()
end;