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

Files for PDF exercise (#67) (#2687)

Browse Source 
* files for PDF exercise

* Explicit call to python2 for systems that default to python3

* Description and comments fixes

[flight_plan] remove warning about dummy WP

Reformatted indents to 2 spaces

Added configs for ps4 gamepads, fixed typos (#68)

Update: for flight testing in the CyberZoo

Fix color gain in mav_course_exercise.xml airframe (#72)

[flight_plan] Re-measured the autoland border. It is not safe to exit the red sqaure so it auto-lands to prevent damage. (#73)

[Camera] Vision runs faster is unnecessary frames are not grabbed. (#74)

Co-authored-by: Matteo Barbera <matteo.barbera97@gmail.com>
This commit is contained in:
Christophe De Wagter
2021-04-09 09:12:55 +02:00
committed by GitHub
parent 20900c46fb
commit 2c55ce3686
12 changed files with 713 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/bebop_course_orangeavoid.xml
+1
View File
@@ -9,6 +9,7 @@
<target name="ap" board="bebop">
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/>
<define name="MT9V117_TARGET_FPS" value="20"/>
<!-- Detect orange -->
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="30"/>
conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
+1
View File
@@ -8,6 +8,7 @@
<target name="ap" board="bebop">
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/>
<define name="MT9V117_TARGET_FPS" value="20"/>
<!-- Detect orange -->
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="30"/>
conf/airframes/tudelft/bebop_mav_course_exercise.xml
+272
View File
@@ -0,0 +1,272 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop_avoider">
<description>MAVLAB Course TUDelft
</description>
<firmware name="rotorcraft">
<target name="ap" board="bebop">
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000/log"/>
<define name="MT9V117_TARGET_FPS" value="20"/>
<!-- Detect orange -->
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="40"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MAX1" value="145"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MIN1" value="65"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MAX1" value="140"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MIN1" value="160"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MAX1" value="225"/>
</target>
<target name="nps" board="pc">
<module name="fdm" type="gazebo"/>
<define name="VIDEO_CAPTURE_PATH" value="/tmp/paparazzi/images"/>
<define name="FILE_LOGGER_PATH" value="/tmp/paparazzi/log"/>
<!-- Detect orange -->
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="41"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MAX1" value="183"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MIN1" value="53"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MAX1" value="121"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MIN1" value="134"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MAX1" value="249"/>
</target>
<define name="ARRIVED_AT_WAYPOINT" value="0.5"/><!-- Detect arrival at waypoint when within 0.5m -->
<!-- Subsystem section -->
<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
<module name="motor_mixing"/>
<module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/>
<module name="gps" type="datalink"/>
<module name="stabilization" type="indi_simple"/>
<module name="stabilization" type="rate_indi"/>
<module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
</module>
<module name="ins" type="extended"/>
<module name="logger_file"/>
<!-- Video/Camera modules -->
<module name="bebop_cam"/>
<!--module name="bebop_ae_awb">
<define name="CV_AE_AWB_AV" value="1" />
<define name="CV_AE_AWB_VERBOSE" value="0" />
</module-->
<module name="video_capture">
<define name="VIDEO_CAPTURE_CAMERA" value="front_camera"/>
<define name="VIDEO_CAPTURE_FPS" value="10"/>
</module>
<module name="cv_detect_color_object">
<define name="COLOR_OBJECT_DETECTOR_CAMERA1" value="front_camera"/>
<define name="COLOR_OBJECT_DETECTOR_FPS1" value="0"/>
<define name="COLOR_OBJECT_DETECTOR_DRAW1" value="1"/>
</module>
<module name="video_rtp_stream">
<define name="VIEWVIDEO_CAMERA" value="front_camera"/>
<define name="VIEWVIDEO_CAMERA2" value="bottom_camera"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="40"/>
</module>
</firmware>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="6000"/>
</commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="3000" neutral="3000" max="9800" />
<servo name="TOP_RIGHT" no="1" min="3000" neutral="3000" max="9800" />
<servo name="BOTTOM_RIGHT" no="2" min="3000" neutral="3000" max="9800" />
<servo name="BOTTOM_LEFT" no="3" min="3000" neutral="3000" max="9800" />
</servos>
<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_X"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<section name="BEBOP_FRONT_CAMERA" prefix="MT9F002_">
<define name="OUTPUT_HEIGHT" value="520" />
<define name="OUTPUT_WIDTH" value="240" />
<define name="OFFSET_X" value="0.09" />
<define name="OFFSET_Y" value="0.00" />
<define name="ZOOM" value="1.25"/>
<define name="GAIN_GREEN1" value="10.0"/>
<define name="GAIN_GREEN2" value="10.0"/>
<define name="GAIN_BLUE" value="12.5"/>
<define name="GAIN_RED" value="9.0"/>
<define name="TARGET_EXPOSURE" value="20" />
<define name="OUTPUT_SCALER" value="0.25"/>
<define name="TARGET_FPS" value="30" />
</section>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" 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="0." unit="deg"/>
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
</section>
<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
<!-- Use GPS altitude measurments and set the R gain -->
<define name="USE_GPS_ALT" value="1"/>
<define name="VFF_R_GPS" value="0.01"/>
</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_R" value="120" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>
</section>
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<!-- attitude reference generation model -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="200" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="300." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.0397"/>
<define name="G1_Q" value="0.0299"/>
<define name="G1_R" value="0.0014"/>
<define name="G2_R" value="0.1219"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="600.0"/>
<define name="REF_ERR_Q" value="600.0"/>
<define name="REF_ERR_R" value="600.0"/>
<define name="REF_RATE_P" value="28.0"/>
<define name="REF_RATE_Q" value="28.0"/>
<define name="REF_RATE_R" value="28.0"/>
<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="120.0" unit="deg/s"/>
<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="8.0"/>
<define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.1"/>
<define name="ACT_DYN_Q" value="0.1"/>
<define name="ACT_DYN_R" value="0.1"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="283"/>
<define name="HOVER_KD" value="82"/>
<define name="HOVER_KI" value="20"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.68"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.68" />
<define name="ADAPT_MIN_HOVER_THROTTLE" value="0.55" />
<define name="ADAPT_MAX_HOVER_THROTTLE" value="0.72" />
<!-- <define name="ADAPT_NOISE_FACTOR" value="0.8" /> -->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="REF_MAX_SPEED" value="0.5" unit="m/s"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="120"/>
<define name="DGAIN" value="230"/>
<define name="IGAIN" value="40"/>
</section>
<section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="1.0"/>
<define name="DESCEND_VSPEED" value="-0.75"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="bebop" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="MT9F002_SENSOR_RES_DIVIDER" value="4"/>
</section>
<include href="conf/simulator/gazebo/airframes/bebop.xml"/>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="10.8" unit="V"/>
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml
+4 -4
View File
@@ -1,6 +1,6 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="8" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
<header>
#include "subsystems/datalink/datalink.h"
#include "subsystems/electrical.h"
@@ -16,12 +16,12 @@
<waypoint name="GOAL" x="1.9" y="1.0"/>
<waypoint name="TRAJECTORY" x="1.9" y="1.0"/>
<waypoint lat="51.9905834" lon="4.3767710" name="_CZ1"/>
<waypoint lat="51.9906465" lon="4.3767025" name="_CZ2"/>
<waypoint lat="51.9906882" lon="4.376805" name="_CZ3"/>
<waypoint lat="51.9906440" lon="4.3767060" name="_CZ2"/>
<waypoint lat="51.9906860" lon="4.3768080" name="_CZ3"/>
<waypoint lat="51.9906238" lon="4.3768729" name="_CZ4"/>
<waypoint lat="51.9905860" lon="4.3767712" name="_OZ1"/>
<waypoint lat="51.9906456" lon="4.3767088" name="_OZ2"/>
<waypoint lat="51.9906430" lon="4.3767110" name="_OZ2"/>
<waypoint lat="51.9906830" lon="4.3768048" name="_OZ3"/>
<waypoint lat="51.9906239" lon="4.3768684" name="_OZ4"/>
</waypoints>
conf/flight_plans/tudelft/course_orangeavoid_cyberzoo_guided.xml
+5 -5
View File
@@ -1,6 +1,6 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="8" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
<header>
#include "subsystems/datalink/datalink.h"
#include "subsystems/electrical.h"
@@ -24,10 +24,10 @@ inline void setGuided(void){
<waypoint name="TD" x="0.8" y="-1.7"/>
<waypoint name="GOAL" x="1.9" y="1.0"/>
<waypoint name="TRAJECTORY" x="1.9" y="1.0"/>
<waypoint lat="51.9905874" lon="4.3767766" name="_CZ1"/>
<waypoint lat="51.990644" lon="4.376721" name="_CZ2"/>
<waypoint lat="51.990676" lon="4.376805" name="_CZ3"/>
<waypoint lat="51.9906213" lon="4.3768628" name="_CZ4"/>
<waypoint lat="51.9905834" lon="4.3767710" name="_CZ1"/>
<waypoint lat="51.9906440" lon="4.3767060" name="_CZ2"/>
<waypoint lat="51.9906860" lon="4.3768080" name="_CZ3"/>
<waypoint lat="51.9906238" lon="4.3768729" name="_CZ4"/>
<waypoint lat="51.990595" lon="4.376779" name="_OZ1"/>
<waypoint lat="51.990640" lon="4.376734" name="_OZ2"/>
<waypoint lat="51.990667" lon="4.376804" name="_OZ3"/>
conf/joystick/ps4_gamepad.xml
+104
View File
@@ -0,0 +1,104 @@
<!-- PlayStation 4 gamepad
It has 6 axes:
axis 0: LeftStickHorizontal
axis 1: LeftStickVertical
axis 2: L2 (left trigger). Also triggers button 6
axis 3: RightStickHorizontal
axis 4: RightStickVertical
axis 5: R2 (right trigger). Also triggers button 7
It has 13 buttons.
b0 - X (blue)
b1 - Circle (red)
b2 - Triangle (green)
b3 - Square (pink)
b4 - L1 (left shoulder button)
b5 - R1 (right shoulder button)
b6 - L2 (left trigger). Also triggers axis 2
b7 - R2 (right trigger). Also triggers axis 5
b8 - Share (left of touchscreen)
b9 - Options (right of touchscreen)
b10 - PS (below touchscreen)
b11 - L3 (press left stick)
b12 - R3 (press right stick)
and the DPad as a hat
You can use the Hat<Position>(<hat_name>) function to trigger events,
where <Position> is one of
Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown
so e.g. HatDown(dpad)
-->
<joystick>
<input>
<axis index="0" name="LeftStickHorizontal"/>
<axis index="1" name="LeftStickVertical"/>
<axis index="2" name="L2" trim="127"/>
<axis index="3" name="RightStickHorizontal"/>
<axis index="4" name="RightStickVertical"/>
<axis index="5" name="R2" trim="127"/>
<button index="0" name="X"/>
<button index="1" name="Circle"/>
<button index="2" name="Triangle"/>
<button index="3" name="Square"/>
<button index="4" name="L1"/>
<button index="5" name="R1"/>
<button index="6" name="L2"/>
<button index="7" name="R2"/>
<button index="8" name="Share"/>
<button index="9" name="Options"/>
<button index="10" name="PS"/>
<button index="11" name="L3"/>
<button index="12" name="R3"/>
<hat index="0" name="dpad"/>
</input>
<variables>
<!-- manual by default and when pressing X, AUTO1 on Square, AUTO2 on Triangle -->
<var name="mode" default="0"/>
<set var="mode" value="0" on_event="X"/>
<set var="mode" value="1" on_event="Square"/>
<set var="mode" value="2" on_event="Triangle"/>
</variables>
<messages period="0.05">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="mode" value="mode"/>
<field name="throttle" value="Fit(-LeftStickVertical,-127,127,0,127)"/>
<field name="roll" value="RightStickHorizontal"/>
<field name="pitch" value="RightStickVertical"/>
<field name="yaw" value="LeftStickHorizontal"/>
</message>
<!-- go to "Start Engine" block if in AUTO2 and pressing Options button -->
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && Options">
<field name="block_id" value="IndexOfBlock('Start Engine')"/>
</message>
<!-- go to "Takeoff" block if in AUTO2 and pressing up on dpad -->
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && HatUp(dpad)">
<field name="block_id" value="IndexOfBlock('Takeoff')"/>
</message>
<!-- go to "land here" block if in AUTO2 and pressing down on dpad -->
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && HatDown(dpad)">
<field name="block_id" value="IndexOfBlock('land here')"/>
</message>
<!-- resurrect throttle if throttle is 0 and R1 button is pressed -->
<message class="ground" name="DL_SETTING" on_event="(LeftStickVertical > 120) && R1">
<field name="index" value="IndexOfSetting('autopilot.kill_throttle')"/>
<field name="value" value="0"/>
</message>
<!-- kill throttle on L1 button -->
<message class="ground" name="DL_SETTING" on_event="L1">
<field name="index" value="IndexOfSetting('autopilot.kill_throttle')"/>
<field name="value" value="1"/>
</message>
</messages>
</joystick>
conf/joystick/ps4_gamepad_yawontriggers.xml
+104
View File
@@ -0,0 +1,104 @@
<!-- PlayStation 4 gamepad
It has 6 axes:
axis 0: LeftStickHorizontal
axis 1: LeftStickVertical
axis 2: L2 (left trigger). Also triggers button 6
axis 3: RightStickHorizontal
axis 4: RightStickVertical
axis 5: R2 (right trigger). Also triggers button 7
It has 13 buttons.
b0 - X (blue)
b1 - Circle (red)
b2 - Triangle (green)
b3 - Square (pink)
b4 - L1 (left shoulder button)
b5 - R1 (right shoulder button)
b6 - L2 (left trigger). Also triggers axis 2
b7 - R2 (right trigger). Also triggers axis 5
b8 - Share (left of touchscreen)
b9 - Options (right of touchscreen)
b10 - PS (below touchscreen)
b11 - L3 (press left stick)
b12 - R3 (press right stick)
and the DPad as a hat
You can use the Hat<Position>(<hat_name>) function to trigger events,
where <Position> is one of
Centered/Up/Right/Down/Left/RightUp/RightDown/LeftUp/LeftDown
so e.g. HatDown(dpad)
-->
<joystick>
<input>
<axis index="0" name="LeftStickHorizontal"/>
<axis index="1" name="LeftStickVertical"/>
<axis index="2" name="L2" trim="127"/>
<axis index="3" name="RightStickHorizontal"/>
<axis index="4" name="RightStickVertical"/>
<axis index="5" name="R2" trim="127"/>
<button index="0" name="X"/>
<button index="1" name="Circle"/>
<button index="2" name="Triangle"/>
<button index="3" name="Square"/>
<button index="4" name="L1"/>
<button index="5" name="R1"/>
<button index="6" name="L2"/>
<button index="7" name="R2"/>
<button index="8" name="Share"/>
<button index="9" name="Options"/>
<button index="10" name="PS"/>
<button index="11" name="L3"/>
<button index="12" name="R3"/>
<hat index="0" name="dpad"/>
</input>
<variables>
<!-- manual by default and when pressing X, AUTO1 on Square, AUTO2 on Triangle -->
<var name="mode" default="0"/>
<set var="mode" value="0" on_event="X"/>
<set var="mode" value="1" on_event="Square"/>
<set var="mode" value="2" on_event="Triangle"/>
</variables>
<messages period="0.05">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="mode" value="mode"/>
<field name="throttle" value="Fit(-LeftStickVertical,-127,127,0,127)"/>
<field name="roll" value="RightStickHorizontal"/>
<field name="pitch" value="RightStickVertical"/>
<field name="yaw" value="Fit(R2-L2,-127,127,-127,127)"/>
</message>
<!-- go to "Start Engine" block if in AUTO2 and pressing Options button -->
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && Options">
<field name="block_id" value="IndexOfBlock('Start Engine')"/>
</message>
<!-- go to "Takeoff" block if in AUTO2 and pressing up on dpad -->
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && HatUp(dpad)">
<field name="block_id" value="IndexOfBlock('Takeoff')"/>
</message>
<!-- go to "land here" block if in AUTO2 and pressing down on dpad -->
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && HatDown(dpad)">
<field name="block_id" value="IndexOfBlock('land here')"/>
</message>
<!-- resurrect throttle if throttle is 0 and R1 button is pressed -->
<message class="ground" name="DL_SETTING" on_event="(LeftStickVertical > 120) && R1">
<field name="index" value="IndexOfSetting('autopilot.kill_throttle')"/>
<field name="value" value="0"/>
</message>
<!-- kill throttle on L1 button -->
<message class="ground" name="DL_SETTING" on_event="L1">
<field name="index" value="IndexOfSetting('autopilot.kill_throttle')"/>
<field name="value" value="1"/>
</message>
</messages>
</joystick>
conf/joystick/sm600.xml
+1 -1
View File
@@ -36,7 +36,7 @@ The basis of steering is the standard signs of aerospace convention
<messages period="0.05">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="throttle" value="Fit(LeftStickVertical,-90,127,0,127)"/>
<field name="throttle" value="Fit(LeftStickVertical,0,127,0,127)"/>
<field name="roll" value="RightStickHorizontal"/>
<field name="pitch" value="-RightStickVertical"/>
<field name="yaw" value="LeftStickHorizontal"/>
conf/modules/mav_course_exercise.xml
+11
View File
@@ -0,0 +1,11 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="mav_course_exercise">
<doc>
<description>
Exercise module for Autonomous Flight of MAV Course
</description>
</doc>
<depends>cv_detect_color_object</depends>
<!-- TODO Specify header, init, periodic, makefile sections -->
</module>
doc/sphinx/source/tutorials/intermediate/create_joystick.rst
+7 -7
View File
@@ -9,7 +9,7 @@ New joystick configurations can be added in the ``paparazzi/conf/joystick`` dire
Profile a joystick
==================
Test if your joystick is recognized: plug your joystick then run ``dmesg``. The message is different for every device, but the last lines should look like these::
Test if your joystick is recognized: plug your joystick then run ``dmesg``. The message is different for every device, but the last lines should look like these:
[49174.642275] usb 1-1: new low-speed USB device number 8 using xhci_hcd
[49174.812307] usb 1-1: New USB device found, idVendor=046d, idProduct=c214, bcdDevice= 2.05
@@ -20,7 +20,7 @@ Test if your joystick is recognized: plug your joystick then run ``dmesg``. The
[49174.823608] hid-generic 0003:046D:C214.000B: input,hidraw4: USB HID v1.10 Joystick [Logitech Logitech Attack 3] on usb-0000:00:14.0-1/input0
Launch ``./sw/ground_segment/joystick/test_stick``. It will display joystick informations, then print current status::
Launch ``./sw/ground_segment/joystick/test_stick``. It will display joystick information, then print current status:
Available button: 5 (0x5)
Available hats: 0 (0x0)
@@ -94,7 +94,7 @@ Create a new file for your joystick in the ``paparazzi/conf/joystick`` directory
Inputs
------
Edit the *input* section according to your info given by *test_stick*. There are 3 kind of inputs :
Edit the *input* section according to your info given by *test_stick*. There are 3 kind of inputs:
- *axis*: "analog" stick that range from a min to a max value,
- *hat*: tiny stick or arrows that can have 8 directions (up, down, left, right, up-left, ...),
@@ -102,7 +102,7 @@ Edit the *input* section according to your info given by *test_stick*. There are
*name* and *index* attributes are mandatory for all.
Axis has 4 optionnal attributes:
Axis has 4 optional attributes:
- *deadband*: input values within the deadband output 0. Range in [0, 127].
- *exponent*: gives precise control around center values, and greater speed at high values. Range in [0, 1.0]. 0 has no effect, 1.0 has maximum effect.
@@ -124,7 +124,7 @@ The *period* attribute on the *messages* section is the period in seconds at whi
In this section, you define which messages will be sent, the value of each field, and the conditions required to send the message.
The *message* tag has two required attributes: the *name* and *class* of the message, and two optionnal attributes : *send_always* and *on_event*.
The *message* tag has two required attributes: the *name* and *class* of the message, and two optional attributes : *send_always* and *on_event*.
*send_always* is a boolean that default to *false*. If set to *true*, messages will keep be sent at the *period* rate. If set to *false*, message will be sent only when one of its field change value.
@@ -137,12 +137,12 @@ In the message node, all fields must be specified except the *ac_id* field, that
*value* is a "C like" expression made of axis and variables names, operators, and a set of utily functions.
Thoses functions are:
Those functions are:
- ``Scale(toto, min, max)`` : scale toto from default min/max values [-128, 127] to [*min*, *max*]
- ``Fit(x, min_in, max_in, min_out, max_out)`` : scale *x* from *min_in* *max_in* to *min_out*, *max_out*
- ``Bound(x, min, max)`` : bound x between *min* and *max*
- ``PprzMode(x)`` : scale input value to [0;1;2]. usefull for RC mode.
- ``PprzMode(x)`` : scale input value to [0;1;2]. useful for RC mode.
- ``JoystickID()`` : return the joystick ID.
- ``IndexOfEnum(NAME)`` : return the index of the enum member *NAME*
- ``IndexOfSetting('setting_name')`` : return the index of the setting *setting_name*.
sw/airborne/modules/mav_course_exercise/mav_exercise.c
+176
View File
@@ -0,0 +1,176 @@
/*
* Copyright (C) 2021 Matteo Barbera <matteo.barbera97@gmail.com>
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
#include "mav_exercise.h"
#include "subsystems/abi.h"
#include "firmwares/rotorcraft/navigation.h"
#include "state.h"
#include "autopilot_static.h"
#include <stdio.h>
#define NAV_C // needed to get the nav functions like Inside...
#include "generated/flight_plan.h"
#define PRINT(string, ...) fprintf(stderr, "[mav_exercise->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
uint8_t increase_nav_heading(float incrementDegrees);
uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters);
uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor);
enum navigation_state_t {
SAFE,
OBSTACLE_FOUND,
OUT_OF_BOUNDS,
HOLD
};
// define and initialise global variables
float oa_color_count_frac = 0.18f;
enum navigation_state_t navigation_state = SAFE;
int32_t color_count = 0; // orange color count from color filter for obstacle detection
int16_t obstacle_free_confidence = 0; // a measure of how certain we are that the way ahead is safe.
float moveDistance = 2; // waypoint displacement [m]
float oob_haeding_increment = 5.f; // heading angle increment if out of bounds [deg]
const int16_t max_trajectory_confidence = 5; // number of consecutive negative object detections to be sure we are obstacle free
// needed to receive output from a separate module running on a parallel process
#ifndef ORANGE_AVOIDER_VISUAL_DETECTION_ID
#define ORANGE_AVOIDER_VISUAL_DETECTION_ID ABI_BROADCAST
#endif
static abi_event color_detection_ev;
static void color_detection_cb(uint8_t __attribute__((unused)) sender_id,
int16_t __attribute__((unused)) pixel_x, int16_t __attribute__((unused)) pixel_y,
int16_t __attribute__((unused)) pixel_width,
int16_t __attribute__((unused)) pixel_height,
int32_t quality, int16_t __attribute__((unused)) extra) {
color_count = quality;
}
void mav_exercise_init(void) {
// bind our colorfilter callbacks to receive the color filter outputs
AbiBindMsgVISUAL_DETECTION(ORANGE_AVOIDER_VISUAL_DETECTION_ID, &color_detection_ev, color_detection_cb);
}
void mav_exercise_periodic(void) {
// only evaluate our state machine if we are flying
if (!autopilot_in_flight()) {
return;
}
// compute current color thresholds
// front_camera defined in airframe xml, with the video_capture module
int32_t color_count_threshold = oa_color_count_frac * front_camera.output_size.w * front_camera.output_size.h;
PRINT("Color_count: %d threshold: %d state: %d \n", color_count, color_count_threshold, navigation_state);
// update our safe confidence using color threshold
if (color_count < color_count_threshold) {
obstacle_free_confidence++;
} else {
obstacle_free_confidence -= 2; // be more cautious with positive obstacle detections
}
// bound obstacle_free_confidence
Bound(obstacle_free_confidence, 0, max_trajectory_confidence);
switch (navigation_state) {
case SAFE:
moveWaypointForward(WP_TRAJECTORY, 1.5f * moveDistance);
if (!InsideObstacleZone(WaypointX(WP_TRAJECTORY), WaypointY(WP_TRAJECTORY))) {
navigation_state = OUT_OF_BOUNDS;
} else if (obstacle_free_confidence == 0) {
navigation_state = OBSTACLE_FOUND;
} else {
moveWaypointForward(WP_GOAL, moveDistance);
}
break;
case OBSTACLE_FOUND:
// TODO Change behavior
// stop as soon as obstacle is found
waypoint_move_here_2d(WP_GOAL);
waypoint_move_here_2d(WP_TRAJECTORY);
navigation_state = HOLD;
break;
case OUT_OF_BOUNDS:
// stop
waypoint_move_here_2d(WP_GOAL);
waypoint_move_here_2d(WP_TRAJECTORY);
increase_nav_heading(oob_haeding_increment);
moveWaypointForward(WP_TRAJECTORY, 1.5f);
if (InsideObstacleZone(WaypointX(WP_TRAJECTORY), WaypointY(WP_TRAJECTORY))) {
// add offset to head back into arena
increase_nav_heading(oob_haeding_increment);
navigation_state = SAFE;
}
break;
case HOLD:
default:
break;
}
}
/*
* Increases the NAV heading. Assumes heading is an INT32_ANGLE. It is bound in this function.
*/
uint8_t increase_nav_heading(float incrementDegrees) {
float new_heading = stateGetNedToBodyEulers_f()->psi + RadOfDeg(incrementDegrees);
// normalize heading to [-pi, pi]
FLOAT_ANGLE_NORMALIZE(new_heading);
// set heading
nav_heading = ANGLE_BFP_OF_REAL(new_heading);
return false;
}
/*
* Calculates coordinates of a distance of 'distanceMeters' forward w.r.t. current position and heading
*/
static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters) {
float heading = stateGetNedToBodyEulers_f()->psi;
// Now determine where to place the waypoint you want to go to
new_coor->x = stateGetPositionEnu_i()->x + POS_BFP_OF_REAL(sinf(heading) * (distanceMeters));
new_coor->y = stateGetPositionEnu_i()->y + POS_BFP_OF_REAL(cosf(heading) * (distanceMeters));
return false;
}
/*
* Sets waypoint 'waypoint' to the coordinates of 'new_coor'
*/
uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor) {
waypoint_move_xy_i(waypoint, new_coor->x, new_coor->y);
return false;
}
/*
* Calculates coordinates of distance forward and sets waypoint 'waypoint' to those coordinates
*/
uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters) {
struct EnuCoor_i new_coor;
calculateForwards(&new_coor, distanceMeters);
moveWaypoint(waypoint, &new_coor);
return false;
}
sw/airborne/modules/mav_course_exercise/mav_exercise.h
+27
View File
@@ -0,0 +1,27 @@
/*
* Copyright (C) 2021 Matteo Barbera <matteo.barbera97@gmail.com>
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
#ifndef PAPARAZZI_MAV_EXERCISE_H
#define PAPARAZZI_MAV_EXERCISE_H
extern void mav_exercise_init(void);
extern void mav_exercise_periodic(void);
#endif //PAPARAZZI_MAV_EXERCISE_H