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

TU Delft MAVLab course 2019 (#2452)

Browse Source 
* Add video capture functionality to video_capture module

The video_capture module can now capture timestamped image-sequences
using the buttons or setting in the GCS. Images are stamped using
their capture pprz_ts, which can be synchronized with the csv file
logger (todo).

(video_capture) Add video capture functionality to video_capture module

(video_capture) name images with current timestamp

(video_capture) Log capture timestamp instead of current time

(video_capture) Minor XML fixes

(video_capture) Store pprz_ts-stamped images in boot-time-stamped folder

Stamping images with calender time does not make sense in simulation,
which has an independent clock that runs at a different rate, and
does not have any concept of a calender time.

* Minor usability tweaks to file logger

* XML changes and new gazebo worlds after collecting datasets

Flight plan for orange pole dataset

Add flightplan to fly towards textured panel

XML changes after collecting dataset

Revert video capture and stream to front camera

* FIX datalink timeout in NPS

* minor fix: make colorfilter's color_count volatile

Shared between video thread and autopilot thread, so should technically
be volatile (although it seemed to work already).

* updated orange avoider to module state machine and added simple guided mode example (#46)

* updated orange avoider to module state machine and added simple guided mode example

* Apply suggestions from code review

Co-Authored-By: kirkscheper <kirkscheper@gmail.com>

* add mutex and update guided to return to arena

* Minor fix: increase freq of WP_MOVED telemetry message

This makes the waypoint-based orange avoider example easier to
follow.

* minor readability tweak for file logger

* Enable video_capture by default, place before object detector

* Fix OpenCV inconsistencies in NPS, update to 3.4.5 (#47)

With this fix, the opencv example module will link the OpenCV lib from the bebop_opencv submodule rather than the locally installed one. This prevents inconsistent behavior between the real drone and the simulator; previously a bug in OpenCV caused U and V channels to be swapped depending on the installed version. The Bebop and sim now both use OpenCV 3.4.5.

Commits:

* FIX color loop error

Not exactly sure what went wrong, but this code works on both sim
and bebop without segfaulting. Note that the YUV channel order problem
still exists in this commit.

* YUV test function

* WIP SEGFAULT Try to upgrade opencv_bebop

This commit segfaults on the Bebop, caused by OpenCV's cvtColor
function (confirmed with printf and gdb). Might be caused by this:
https://github.com/opencv/opencv/issues/12176 .

* WIP Update opencv_bebop

* WIP get compilation working for nps

* WIP Clean up makefile, update conf LDFLAGS

* Restructure opencv_bebop makefile

Also update pprz because paths and ldflags have changed.

* Minor opencv_bebop makefile changes

* Fix YUV channel order

* FIX Consistent OpenCV behavior on drone and in NPS, OpenCV 3.4.5

With this fix, the opencv example module will link the OpenCV lib
from the bebop_opencv submodule rather than the locally installed one.
This prevents inconsistent behavior between the real drone and the simulator;
previously a bug in OpenCV caused U and V channels to be swapped depending
on the installed version. The Bebop and sim now both use OpenCV 3.4.5.

* update opencv_bebop

* Remove opencv demo module from course airframe

* Separate grayscale and color examples

* Add joystick xml for SM600

Joystick should be set in Reflex XTR mode. Tested on Ubuntu 16,
but not on 18 yet.

* Change sm600.xml to G3-G4.5 axis order

Reflex mode behaves differently between Ubuntu 16 and 18... :/

* Add OptiTrack distance measuring tool

Usage:
1. Connect through ethernet to OptiTrack pc.
2. In dist.py, change id number (line 19) to rigid body id to track.
3. Run dist.py
4. Results are not saved, copy manually.

dist.py was written by Shushuai Li.

* Made find_object_centroid documentation more comprehensive (#50)

* GPS coordinates of CyberZoo (#51)

* GPS coordinates of CyberZoo

Update GPS coordinates of the CyberZoo along the green carpet

* Update conf/flight_plans/tudelft/course2019_orangeavoid_cyberzoo.xml

* Update conf/flight_plans/tudelft/course2019_orangeavoid_cyberzoo.xml

* Fixed capitalisation in xbox controlled and added Turnigy controller (#52)

* Fixed capitalisation in xbox controlled and added Turnigy controller

 - Fixed capitalisation in conf/joystick/xbox_gamepad.xml

 - Added Turnigy controller conf/joystick/turnigy_evolution.xml

* Added a newbie friendly version of xbox_gamepad

The xbox controller both sticks have springs in both axis
and in the configuration of the xbox_gamepad.xml the yaw and throttle are on the same axis.
This configuration has the yaw on the triggers making for a more pleasant experience.

* FIX missing #endif in abi_sender_ids.h

* Update airframes to use bebop_cam module

TODO: retune, test and tag in cyberzoo

* Update opencv_bebop submodule to latest master

Also updated linked libraries in cv_opencvdemo module. Should
be re-tested on a clean system.

* Further cleanup for pull request

* Change sm600.xml throttle range

Throttle did not seem to behave correctly. Calibration only makes
things worse (incorrect ranges and lots of noise). The current xml
seems to work with an UNcalibrated joystick, with the trim sliders
in the middle (including throttle).

* Change default joystick to sm600.xml

* Revert sm600.xml range

* Re-tune camera

Camera settings were changed after bebop camera overhaul.

* Further tuning of green detector

* Tag aircraft after test flights

* FIX 'land here' incorrect capitalization

Changed back to lower case to maintain compatibility with other
flight plans that may not have been merged into master.

* FIX reduce WP_MOVED update rate

Avoidance code used waypoint_set_ functions instead of wp_move_,
which lacks the downlink message to update the wp position in the
GCS. Implemented the missing move functions and used these instead,
reduced the update rate of WP_MOVED to its original, low rate.

* FIX file_logger error message referring to video_capture

* FIX file_logger for fixed wing ac

* FIX sm600.xml missing newline

* WIP Cleanup opti_dist
This commit is contained in:
Tom van Dijk
2019-07-18 13:14:33 +02:00
committed by Gautier Hattenberger
parent 27057a37ab
commit 48e7b20789
38 changed files with 1957 additions and 394 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+1 -1
View File
@@ -189,7 +189,7 @@
<field name="pixel_y" type="int16_t">Center pixel Y</field>
<field name="pixel_width" type="int16_t">Width in pixels</field>
<field name="pixel_height" type="int16_t">Height in pixels</field>
<field name="quality" type="int16_t">Detection quality</field>
<field name="quality" type="int32_t">Detection quality</field>
<field name="extra" type="int16_t">Extra field for options like detector source etc</field>
</message>
conf/airframes/tudelft/bebop_course2018_orangeavoid.xml → conf/airframes/tudelft/bebop_course2019_orangeavoid.xml
+51 -40
View File
@@ -1,15 +1,38 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop_avoider">
<description>Vision Course TUDelft V2018
<description>Vision Course TUDelft V2019
</description>
<firmware name="rotorcraft">
<target name="ap" board="bebop"/>
<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"/>
<!-- Detect orange -->
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="30"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MAX1" value="170"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MIN1" value="70"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MAX1" value="130"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MIN1" value="150"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MAX1" value="190"/>
</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"/>
@@ -25,36 +48,31 @@
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
</module>
<module name="ins" type="extended"/>
<!-- <module name="guidance" type="indi"> <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN"
value="-500.0"/> <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
<define name="GUIDANCE_INDI_RC_DEBUG" value="TRUE"/> </module> -->
<!--module name="geo_mag"/-->
<module name="air_data"/>
<!--module name="send_imu_mag_current"/-->
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
</module>
<module name="logger_file"/>
<!-- Video/Camera modules -->
<module name="bebop_cam"/>
<!--module name="bebop_ae_awb">
<define name="CV_AE_AWB_VERBOSE" value="0" />
</module-->
<module name="cv_colorfilter">
<define name="COLORFILTER_CAMERA" value="front_camera" />
<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="orange_avoider">
<define name="ORANGE_AVOIDER_VISUAL_DETECTION_ID" value="COLOR_OBJECT_DETECTION1_ID"/>
</module>
<module name="orange_avoider" />
<module name="video_rtp_stream">
<define name="VIEWVIDEO_CAMERA" value="front_camera" />
<define name="VIEWVIDEO_FPS" value="18" />
<define name="VIEWVIDEO_STREAM_VIDEO" value="TRUE" />
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1" />
<define name="VIEWVIDEO_SHOT_PATH" value="/data/ftp/internal_000" />
<!-- <define name="VIEWVIDEO_WRITE_VIDEO" value="FALSE" />
<define name="VIEWVIDEO_VIDEO_FILE" value="orangeAvoider" /> -->
<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>
@@ -76,7 +94,6 @@
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<!--define name="REVERSE" value="TRUE"/-->
<define name="TYPE" value="QUAD_X"/>
</section>
@@ -92,18 +109,12 @@
<define name="OUTPUT_HEIGHT" value="520" />
<define name="OUTPUT_WIDTH" value="240" />
<define name="OFFSET_X" value="0.09" />
<define name="TARGET_EXPOSURE" value="20" />
<define name="ZOOM" value="1.25"/>
</section>
<!-- Cyberzoo bebop1 values -->
<section name="COLORFILTER" prefix="ORANGE_AVOIDER_">
<define name="LUM_MIN" value="20"/>
<define name="LUM_MAX" value="180"/>
<define name="CB_MIN" value="75"/>
<define name="CB_MAX" value="145"/>
<define name="CR_MIN" value="167"/>
<define name="CR_MAX" value="255"/>
<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="30" />
</section>
<section name="AIR_DATA" prefix="AIR_DATA_">
@@ -166,10 +177,10 @@
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
<define name="REF_OMEGA_R" value="200" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
<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_">
@@ -220,7 +231,7 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="REF_MAX_SPEED" value="2" unit="m/s"/>
<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"/>
conf/airframes/tudelft/bebop_course2019_orangeavoid_guided.xml
+289
View File
@@ -0,0 +1,289 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="bebop_avoider">
<description>Vision Course TUDelft V2019
</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"/>
<!-- Detect orange -->
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="30"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MAX1" value="170"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MIN1" value="70"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MAX1" value="130"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MIN1" value="150"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MAX1" value="190"/>
<!-- Detect green -->
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN2" value="60"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MAX2" value="130"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MIN2" value="75"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MAX2" value="110"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MIN2" value="120"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MAX2" value="140"/>
</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"/>
<!-- Detect green -->
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN2" value="0"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MAX2" value="255"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MIN2" value="0"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MAX2" value="110"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MIN2" value="0"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MAX2" value="130"/>
</target>
<!-- 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_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"/>
<define name="COLOR_OBJECT_DETECTOR_CAMERA2" value="front_camera"/>
<define name="COLOR_OBJECT_DETECTOR_FPS2" value="0"/>
<define name="COLOR_OBJECT_DETECTOR_DRAW2" value="1"/>
</module>
<module name="orange_avoider_guided">
<define name="ORANGE_AVOIDER_VISUAL_DETECTION_ID" value="COLOR_OBJECT_DETECTION1_ID"/>
<define name="FLOOR_VISUAL_DETECTION_ID" value="COLOR_OBJECT_DETECTION2_ID"/>
</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="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="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="450" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
</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/course2018_orangeavoid_cyberzoo.xml → conf/flight_plans/tudelft/course2019_orangeavoid_cyberzoo.xml
+29 -43
View File
@@ -1,14 +1,11 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="1.0" ground_alt="0" lat0="51.9906361" lon0="4.3767874" 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="8" name="Bebop avoid orange TU Delft Cyberzoo" security_height="0.4">
<header>
#include "subsystems/datalink/datalink.h"
#include "subsystems/electrical.h"
#include "subsystems/radio_control.h"
#include "subsystems/ahrs.h"
// Note: use 'git submodule update --init -- sw/ext/tudelft_gazebo_models/'
// to download the required models.
#define NPS_GAZEBO_WORLD "cyberzoo_orange_poles.world"
</header>
<waypoints>
@@ -18,14 +15,15 @@
<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.9905836" lon="4.3767729" name="_CZ1"/>
<waypoint lat="51.9906365" lon="4.3767138" name="_CZ2"/>
<waypoint lat="51.990680" lon="4.376805" name="_CZ3"/>
<waypoint lat="51.9906226" lon="4.3768699" name="_CZ4"/>
<waypoint lat="51.990594" lon="4.376776" name="_OZ1"/>
<waypoint lat="51.990636" lon="4.376733" name="_OZ2"/>
<waypoint lat="51.990671" lon="4.376806" name="_OZ3"/>
<waypoint lat="51.990624" lon="4.376852" name="_OZ4"/>
<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.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.9906830" lon="4.3768048" name="_OZ3"/>
<waypoint lat="51.9906239" lon="4.3768684" name="_OZ4"/>
</waypoints>
<sectors>
<sector color="red" name="CyberZoo">
@@ -45,37 +43,37 @@
<!-- RC lost -->
<!--exception cond="((radio_control.status == RC_REALLY_LOST) &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('Land here')) &&
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="Standby"/-->
<!-- Datalink lost (constant RPM descent) -->
<!-- Datalink lost (constant RPM descent) -->
<exception cond="((datalink_time > 5) &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('Land here')) &&
(autopilot_in_flight() == true) )" deroute="Land here"/>
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing XY -->
<exception cond="(!InsideCyberZoo(GetPosX(), GetPosY()) &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('Land here')) &&
(autopilot_in_flight() == true) )" deroute="Land here"/>
<!-- Geofencing Z 2.5 -->
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing Z 3.5 -->
<exception cond="((GetPosAlt() > 3.5) &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('Land here')) &&
(autopilot_in_flight() == true) )" deroute="Land here"/>
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing Z 4.5 (constant RPM descent)-->
<exception cond="((GetPosAlt() > 4.5) &&
!(IndexOfBlock('Holding point') > nav_block) &&
(autopilot_in_flight() == true) )" deroute="Landed"/>
<!-- Bat low -->
<!-- Bat low -->
<exception cond="(electrical.bat_low &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('Land here')) &&
(autopilot_in_flight() == true) )" deroute="Land here"/>
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Bat critical (constant RPM no stabilization)-->
<exception cond="(electrical.bat_critical &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('Land here')) &&
(autopilot_in_flight() == true) )" deroute="Land here"/>
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="land here"/>
</exceptions>
<blocks>
<block name="Wait GPS">
@@ -96,7 +94,7 @@
<call_once fun="NavResurrect()"/>
</block>
<block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > 1.0" deroute="Standby"/>
<exception cond="GetPosAlt() > 0.8" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
@@ -106,25 +104,13 @@
</block>
<block key="g" name="START" strip_button="Go" strip_icon="lookfore.png">
<call_once fun="NavSetWaypointHere(WP_GOAL)"/>
<set var="trajectoryConfidence" value="1"/>
</block>
<block name="StayGoal">
<stay wp="GOAL"/>
<exception cond="!InsideObstacleZone(WaypointX(WP_TRAJECTORY),WaypointY(WP_TRAJECTORY))" deroute="PrepareObstacleRun"/>
</block>
<block name="PrepareObstacleRun">
<while cond="!InsideObstacleZone(WaypointX(WP_TRAJECTORY),WaypointY(WP_TRAJECTORY))">
<call_once fun="increase_nav_heading(&nav_heading, 5)"/>
</while>
<block name="STOP">
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
<stay wp="STDBY"/>
</block>
<block name="GetIntoObstacleZone">
<while cond="!InsideObstacleZone(GetPosX(),GetPosY())">
<call_once fun="moveWaypointForward(WP_GOAL, 0.1)"/>
<go wp="GOAL"/>
</while>
<deroute block="START"/>
</block>
<block key="l" name="Land here" strip_button="Land Here" strip_icon="land-right.png">
<block key="l" name="land here" strip_button="land here" strip_icon="land-right.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/>
<go wp="TD"/>
<deroute block="Flare"/>
conf/flight_plans/tudelft/course2019_orangeavoid_cyberzoo_guided.xml
+145
View File
@@ -0,0 +1,145 @@
<!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">
<header>
#include "subsystems/datalink/datalink.h"
#include "subsystems/electrical.h"
#include "subsystems/radio_control.h"
#include "subsystems/ahrs.h"
#define NPS_GAZEBO_WORLD "cyberzoo_orange_poles.world"
#include "autopilot.h"
inline void setNav(void){
autopilot_mode_auto2 = AP_MODE_NAV;
autopilot_static_set_mode(AP_MODE_NAV);
}
inline void setGuided(void){
autopilot_mode_auto2 = AP_MODE_GUIDED;
autopilot_static_set_mode(AP_MODE_GUIDED);
}
</header>
<waypoints>
<waypoint lat="51.990631" lon="4.376796" name="HOME"/>
<waypoint name="CLIMB" x="1.9" y="1.0"/>
<waypoint name="STDBY" x="1.9" y="1.0"/>
<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.990595" lon="4.376779" name="_OZ1"/>
<waypoint lat="51.990640" lon="4.376734" name="_OZ2"/>
<waypoint lat="51.990667" lon="4.376804" name="_OZ3"/>
<waypoint lat="51.990623" lon="4.376850" name="_OZ4"/>
</waypoints>
<sectors>
<sector color="red" name="CyberZoo">
<corner name="_CZ1"/>
<corner name="_CZ2"/>
<corner name="_CZ3"/>
<corner name="_CZ4"/>
</sector>
<sector color="#FF9922" name="ObstacleZone">
<corner name="_OZ1"/>
<corner name="_OZ2"/>
<corner name="_OZ3"/>
<corner name="_OZ4"/>
</sector>
</sectors>
<exceptions>
<!-- RC lost -->
<!--exception cond="((radio_control.status == RC_REALLY_LOST) &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="Standby"/-->
<!-- Datalink lost (constant RPM descent) -->
<exception cond="((datalink_time > 5) &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing XY -->
<exception cond="(!InsideCyberZoo(GetPosX(), GetPosY()) &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing Z 2.5 -->
<exception cond="((GetPosAlt() > 3.5) &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Geofencing Z 4.5 (constant RPM descent)-->
<exception cond="((GetPosAlt() > 4.5) &&
!(IndexOfBlock('Holding point') > nav_block) &&
(autopilot_in_flight() == true) )" deroute="Landed"/>
<!-- Bat low -->
<exception cond="(electrical.bat_low &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="land here"/>
<!-- Bat critical (constant RPM no stabilization)-->
<exception cond="(electrical.bat_critical &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('land here')) &&
(autopilot_in_flight() == true) )" deroute="land here"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 2)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block key="r" name="Start Engine">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
<call_once fun="NavResurrect()"/>
</block>
<block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="GetPosAlt() > 0.8" deroute="Standby"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block key="s" name="Standby" strip_button="Standby" strip_icon="home.png">
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
<stay wp="STDBY"/>
</block>
<block key="g" name="START" strip_button="Go" strip_icon="lookfore.png">
<call_once fun="setGuided()"/>
<stay wp="STDBY"/>
</block>
<block name="STOP">
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
<call_once fun="setNav()"/>
<stay wp="STDBY"/>
</block>
<block key="l" name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call_once fun="setNav()"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
<go wp="TD"/>
<deroute block="Flare"/>
</block>
<block name="Land">
<call_once fun="setNav()"/>
<go wp="TD"/>
<deroute block="Flare"/>
</block>
<block name="Flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Landed"/>
<exception cond="0.10 > GetPosAlt()" deroute="Landed"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Landed">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/joystick/sm600.xml
+47
View File
@@ -0,0 +1,47 @@
<!--
SM600 6CH RC Transmitter shaped USB HID joystick Mode 2
If you want to use this file for the same product but mode 1 the axis index
numbers have to be re-arranged, the rest will be exactly the same.
This RC TX joystick has four axes,1 button, one dial, which are labeled on the RC TX joystick
NOTE: The in TX Simtype (Reflex,G3,APD,Phoenix) needs to be set to ** G3-G4.5 **
* The dial gives output on an **axis** type not button type
* The switch is as a button, only has 2 positions, not three :(
We will use it as mode switch none the less
switching between, MANUAL (or ATT for stabilized AC) and NAV
So it is a real 6CH USB joystick that just looks like a RC transmitter
If you want to fly your UAS via the joystick add this to your session:
/home/username/paparazzi/sw/ground_segment/joystick/input2ivy -d 0 -ac yourarfamename sm600.xml
Where -d 0 must be -d 1 if you have a laptop with accelometer installed
The basis of steering is the standard signs of aerospace convention
-->
<joystick>
<input>
<axis index="4" name="LeftStickHorizontal"/>
<axis index="2" name="LeftStickVertical"/>
<axis index="0" name="RightStickHorizontal"/>
<axis index="1" name="RightStickVertical"/>
<axis index="3" name="VRB"/>
<button index="0" name="VRA"/>
</input>
<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="roll" value="RightStickHorizontal"/>
<field name="pitch" value="-RightStickVertical"/>
<field name="yaw" value="LeftStickHorizontal"/>
<field name="mode" value="2-2*VRA"/>
</message>
</messages>
</joystick>
conf/joystick/turnigy_evolution.xml
+49
View File
@@ -0,0 +1,49 @@
<!--
Hobbyking 6CH RC Transmitter shaped USB HID joystick Mode 2
http://www.hobbyking.com/hobbyking/store/__20951__Hobbyking_6CH_RC_Flight_Simulator_System_Mode_2_.html
If you want to use this file for the same product but mode 1 the axis index
numbers have to be re-arranged, the rest will be exactly the same.
This RC TX joystick has four axes,2 buttons, one dial, which are labeled on the
RC TX joystick
The button and dial give their output on an **axis** type not button type
The button ""MIX" is not useful and hardwired to mixing and does NOT give
indepenadant output, better remove it from your RC joystick.
The 6CH Gyro switch only has 2 positions, not three :(
We will use it as mode switch none the less between, MANUAL (or ATT) and NAV
So it is a real 6CH USB joystick that just looks like a RC transmitter
If you want to fly your UAS via the joystick add thid to your session:
/home/username/paparazzi/sw/ground_segment/joystick/input2ivy -d 0 -ac yourarfamename hobbyking_usb_rc_6ch_rc_tx_joy_mode_2.xml
Where -d 0 must be -d 1 if you have a laptop with accelometer installed
The basis of steering is the standard signs of aerospace convention
-->
<joystick>
<input>
<axis index="0" name="roll"/>
<axis index="1" name="pitch"/>
<axis index="2" name="throttle"/>
<axis index="3" name="yaw"/>
<axis index="4" name="mode"/>
</input>
<messages period="0.0333333">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="throttle" value="Fit(throttle,-127,127,0,127)" />
<field name="roll" value="Fit(roll,-125,125,-127,127)" />
<field name="pitch" value="Fit(-pitch,-127,127,-127,127)" />
<field name="yaw" value="Fit(yaw,-127,127,-127,127)"/>
<field name="mode" value="Fit(mode,-126,126,0,127)"/>
</message>
</messages>
</joystick>
conf/joystick/xbox_gamepad_yawontriggers.xml
+100
View File
@@ -0,0 +1,100 @@
<!-- Generic X-Box gamepad
e.g. Logitech wireless gamepad F710
This xbox controller is setup for an easier control experience
The triggers control the yaw and the left stick only controls the throttle
This config is for Xinput mode. Make sure slider switch on back of controller is on X (not D)
Also make sure controller not in sports mode (mode light should be off)
Has six axes:
axis 0: LTS_H (left thumb stick horizontal) (or DPad horizontal in sports mode)
axis 1: LTS_V (left thumb stick vertical) (or DPad vertical in sports mode)
axis 2: LT (left trigger)
axis 3: RTS_H (right thumb stick horizontal)
axis 4: RTS_V (right thumb stick vertical)
axis 5: RT (right trigger)
It has 11 buttons.
b0 - A (green)
b1 - B (red)
b2 - X (blue)
b3 - Y (yellow)
b4 - LB (left shoulder button)
b5 - RB (right shoulder button)
b6 - back
b7 - start
b8 - ?
b9 - LSB (left stick button)
b10 - RSB (right stick button)
and the DPad as a hat (in normal mode)
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="yaw" limit="1.00" exponent="0.7" trim="0"/>
<axis index="1" name="throttle"/>
<axis index="2" name="LT" limit="1.00" trim="127"/>
<axis index="3" name="roll" limit="1.00" exponent="0.7" trim="0"/>
<axis index="4" name="pitch" limit="1.00" exponent="0.7" trim="0"/>
<axis index="5" name="RT" limit="1.00" trim="127"/>
<button index="0" name="A"/>
<button index="1" name="B"/>
<button index="2" name="X"/>
<button index="3" name="Y"/>
<button index="4" name="LB"/>
<button index="5" name="RB"/>
<button index="6" name="back"/>
<button index="7" name="start"/>
<button index="9" name="LSB"/>
<button index="10" name="RSB"/>
<hat index="0" name="dpad"/>
</input>
<variables>
<!-- manual by default and when pressing A, AUTO1 on B, AUTO2 on Y -->
<var name="mode" default="0"/>
<set var="mode" value="0" on_event="A"/>
<set var="mode" value="1" on_event="B"/>
<set var="mode" value="2" on_event="Y"/>
</variables>
<messages period="0.05">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="mode" value="mode"/>
<field name="throttle" value="Fit(-throttle,-127,127,0,127)"/> <!--To trimm for Parot Bebop set 0 to 47-->
<field name="roll" value="roll"/>
<field name="pitch" value="pitch"/>
<field name="yaw" value="Fit(RT-LT,-127,127,-127,127)"/>
</message>
<!-- go to "Start Engine" block if in AUTO2 and pressing start button -->
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && start">
<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>
<!-- go to "Standby" block if in AUTO2 and pressing down on dpad -->
<message class="ground" name="JUMP_TO_BLOCK" on_event="(mode > 1) && Y">
<field name="block_id" value="IndexOfBlock('Standby')"/>
</message>
</messages>
</joystick>
conf/modules/cv_colorfilter.xml
-1
View File
@@ -10,7 +10,6 @@
<settings>
<dl_settings>
<dl_settings NAME="ColorFilter">
<dl_setting var="listener->active" min="0" step="1" max="1" shortname="active" />
<dl_setting var="color_lum_min" min="0" step="1" max="255" shortname="y_min" />
<dl_setting var="color_lum_max" min="0" step="1" max="255" shortname="y_max" />
<dl_setting var="color_cb_min" min="0" step="1" max="255" shortname="u_min" />
conf/modules/cv_detect_color_object.xml
+63
View File
@@ -0,0 +1,63 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="cv_detect_color_object" dir="computer_vision">
<doc>
<description>Color Object Detector
Detects an object by a continuous color. Optionally draws on image.
</description>
<define name="COLOR_OBJECT_DETECTOR_CAMERA1" value="front_camera|bottom_camera" description="Video device to use"/>
<define name="COLOR_OBJECT_DETECTOR_FPS1" value="0" description="Desired FPS (0: camera rate)"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN1" value="0" description="Filter 1 min luminance"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MAX1" value="0" description="Filter 1 max luminance"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MIN1" value="0" description="Filter 1 min blue chroma"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MAX1" value="0" description="Filter 1 max blue chroma"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MIN1" value="0" description="Filter 1 min red chroma"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MAX1" value="0" description="Filter 1 max red chroma"/>
<define name="COLOR_OBJECT_DETECTOR_DRAW1" value="FALSE|TRUE" description="Whether or not to draw on image"/>
<define name="COLOR_OBJECT_DETECTOR_CAMERA2" value="front_camera|bottom_camera" description="Video device to use"/>
<define name="COLOR_OBJECT_DETECTOR_FPS2" value="0" description="Desired FPS (0: camera rate)"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MIN2" value="0" description="Filter 1 min luminance"/>
<define name="COLOR_OBJECT_DETECTOR_LUM_MAX2" value="0" description="Filter 2 max luminance"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MIN2" value="0" description="Filter 2 min blue chroma"/>
<define name="COLOR_OBJECT_DETECTOR_CB_MAX2" value="0" description="Filter 2 max blue chroma"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MIN2" value="0" description="Filter 2 min red chroma"/>
<define name="COLOR_OBJECT_DETECTOR_CR_MAX2" value="0" description="Filter 2 max red chroma"/>
<define name="COLOR_OBJECT_DETECTOR_DRAW2" value="FALSE|TRUE" description="Whether or not to draw on image"/>
</doc>
<settings>
<dl_settings>
<dl_settings name="ColorObjectDetector">
<dl_setting var="cod_lum_min1" min="0" step="1" max="255" shortname="y_min1"/>
<dl_setting var="cod_lum_max1" min="0" step="1" max="255" shortname="y_max1"/>
<dl_setting var="cod_cb_min1" min="0" step="1" max="255" shortname="u_min1"/>
<dl_setting var="cod_cb_max1" min="0" step="1" max="255" shortname="u_max1"/>
<dl_setting var="cod_cr_min1" min="0" step="1" max="255" shortname="v_min1"/>
<dl_setting var="cod_cr_max1" min="0" step="1" max="255" shortname="v_max1"/>
<dl_setting var="cod_draw1" min="0" step="1" max="1" values="False|True" shortname="draw 1" />
<dl_setting var="cod_lum_min2" min="0" step="1" max="255" shortname="y_min2"/>
<dl_setting var="cod_lum_max2" min="0" step="1" max="255" shortname="y_max2"/>
<dl_setting var="cod_cb_min2" min="0" step="1" max="255" shortname="u_min2"/>
<dl_setting var="cod_cb_max2" min="0" step="1" max="255" shortname="u_max2"/>
<dl_setting var="cod_cr_min2" min="0" step="1" max="255" shortname="v_min2"/>
<dl_setting var="cod_cr_max2" min="0" step="1" max="255" shortname="v_max2"/>
<dl_setting var="cod_draw2" min="0" step="1" max="1" values="False|True" shortname="draw 2" />
</dl_settings>
</dl_settings>
</settings>
<depends>video_thread</depends>
<header>
<file name="cv_detect_color_object.h"/>
</header>
<init fun="color_object_detector_init()"/>
<periodic fun="color_object_detector_periodic()" freq="50"/>
<makefile target="ap|nps">
<file name="cv_detect_color_object.c"/>
</makefile>
</module>
conf/modules/cv_opencvdemo.xml
+38 -20
View File
@@ -19,30 +19,48 @@
<file name="cv_opencvdemo.c"/>
<file name="opencv_example.cpp"/>
<file name="opencv_image_functions.cpp"/>
<flag name="CXXFLAGS" value="I$(PAPARAZZI_SRC)/sw/ext/opencv_bebop/install/include"/>
<flag name="LDFLAGS" value="L$(PAPARAZZI_HOME)/sw/ext/opencv_bebop/install/lib" />
<flag name="LDFLAGS" value="lopencv_world" />
<flag name="LDFLAGS" value="L$(PAPARAZZI_HOME)/sw/ext/opencv_bebop/install/share/OpenCV/3rdparty/lib" />
<flag name="LDFLAGS" value="ltegra_hal" />
<flag name="LDFLAGS" value="lzlib" />
<flag name="LDFLAGS" value="llibjpeg" />
<flag name="LDFLAGS" value="llibpng" />
<flag name="LDFLAGS" value="llibtiff" />
<flag name="LDFLAGS" value="lstdc++" />
<flag name="LDFLAGS" value="ldl" />
<flag name="LDFLAGS" value="lm" />
<flag name="LDFLAGS" value="lpthread" />
<flag name="LDFLAGS" value="lrt" />
<flag name="CXXFLAGS" value="I$(PAPARAZZI_SRC)/sw/ext/opencv_bebop/install_arm/include"/>
<flag name="LDFLAGS" value="L$(PAPARAZZI_HOME)/sw/ext/opencv_bebop/install_arm/lib"/>
<flag name="LDFLAGS" value="lopencv_world"/>
<flag name="LDFLAGS" value="L$(PAPARAZZI_HOME)/sw/ext/opencv_bebop/install_arm/share/OpenCV/3rdparty/lib"/>
<flag name="LDFLAGS" value="llibprotobuf"/>
<flag name="LDFLAGS" value="llibjpeg-turbo"/>
<flag name="LDFLAGS" value="llibpng"/>
<flag name="LDFLAGS" value="llibtiff"/>
<flag name="LDFLAGS" value="lzlib"/>
<flag name="LDFLAGS" value="lquirc"/>
<flag name="LDFLAGS" value="ltegra_hal"/>
<flag name="LDFLAGS" value="ldl"/>
<flag name="LDFLAGS" value="lm"/>
<flag name="LDFLAGS" value="lpthread"/>
<flag name="LDFLAGS" value="lrt"/>
</makefile>
<makefile target="nps">
<file name="cv_opencvdemo.c"/>
<file name="opencv_example.cpp"/>
<file name="opencv_image_functions.cpp"/>
<raw>
nps.CXXFLAGS += $(shell pkg-config opencv --cflags)
nps.LDFLAGS += -lopencv_imgproc -lopencv_highgui -lopencv_core
</raw>
<flag name="CXXFLAGS" value="I$(PAPARAZZI_SRC)/sw/ext/opencv_bebop/install_pc/include"/>
<flag name="LDFLAGS" value="L$(PAPARAZZI_HOME)/sw/ext/opencv_bebop/install_pc/lib"/>
<flag name="LDFLAGS" value="lopencv_world"/>
<flag name="LDFLAGS" value="L$(PAPARAZZI_HOME)/sw/ext/opencv_bebop/install_pc/share/OpenCV/3rdparty/lib"/>
<flag name="LDFLAGS" value="llibprotobuf"/>
<flag name="LDFLAGS" value="lquirc"/>
<flag name="LDFLAGS" value="L/usr/lib/x86_64-linux-gnu"/>
<flag name="LDFLAGS" value="ljpeg"/>
<flag name="LDFLAGS" value="lpng"/>
<flag name="LDFLAGS" value="ltiff"/>
<flag name="LDFLAGS" value="L/usr/lib/x86_64-linux-gnu/hdf5/serial"/>
<flag name="LDFLAGS" value="lhdf5"/>
<flag name="LDFLAGS" value="lpthread"/>
<flag name="LDFLAGS" value="lsz"/>
<flag name="LDFLAGS" value="lz"/>
<flag name="LDFLAGS" value="ldl"/>
<flag name="LDFLAGS" value="lm"/>
<flag name="LDFLAGS" value="lfreetype"/>
<flag name="LDFLAGS" value="lharfbuzz"/>
<flag name="LDFLAGS" value="lrt"/>
</makefile>
</module>
conf/modules/logger_file.xml
+1 -1
View File
@@ -12,7 +12,7 @@
<file name="file_logger.h" />
</header>
<periodic fun="file_logger_periodic()" start="file_logger_start()"
stop="file_logger_stop()" autorun="FALSE" />
stop="file_logger_stop()" autorun="FALSE" period="0.01" />
<makefile>
<file name="file_logger.c"/>
</makefile>
conf/modules/orange_avoider.xml
+18 -12
View File
@@ -3,25 +3,31 @@
<module name="orange_avoider">
<doc>
<description>
Avoid all objects that are orange!
This example module shows how you can use the camera stream and colorfilter to detect orange objects.
By adding this module to your flightplan and flying in the cyberzoo with the flightplan tudelft/course2017_avoid_orange_cyberzoo.xml you will avoid every obstacle that is orange.
This module is an example module for the course AE4317 Autonomous Flight of Micro Air Vehicles at the TU Delft.
This module is used in combination with a color filter (cv_detect_color_object) and the navigation mode of the autopilot.
The avoidance strategy is to simply count the total number of orange pixels. When above a certain percentage threshold,
(given by color_count_frac) we assume that there is an obstacle and we turn.
The color filter settings are set using the cv_detect_color_object. This module can run multiple filters simultaneously
so you have to define which filter to use with the ORANGE_AVOIDER_VISUAL_DETECTION_ID setting.
</description>
<define name="ORANGE_AVOIDER_LUM_MIN" value="0-255" description="Minimum Luminance To select pixel as object"/>
<define name="ORANGE_AVOIDER_LUM_MAX" value="0-255" description="Minimum Luminance To select pixel as object"/>
<define name="ORANGE_AVOIDER_CR_MIN" value="0-255" description="Minimum CR To select pixel as object"/>
<define name="ORANGE_AVOIDER_CR_MAX" value="0-255" description="Minimum CR To select pixel as object"/>
<define name="ORANGE_AVOIDER_CB_MIN" value="0-255" description="Minimum CB To select pixel as object"/>
<define name="ORANGE_AVOIDER_CB_MAX" value="0-255" description="Minimum CB To select pixel as object"/>
<define name="ORANGE_AVOIDER_VISUAL_DETECTION_ID" value="ABI_BROADCAST" description="which VISUAL_DETECTION message to recieve for orange pole detection."/>
</doc>
<depends>cv_colorfilter.xml</depends>
<settings>
<dl_settings>
<dl_settings name="OrangeAvoider">
<dl_setting var="oa_color_count_frac" min="0" step="0.01" max="0.5"/>
</dl_settings>
</dl_settings>
</settings>
<depends>cv_detect_color_object</depends>
<header>
<file name="orange_avoider.h"/>
</header>
<init fun="orange_avoider_init()"/>
<periodic fun="orange_avoider_periodic()" freq="4"/>
<makefile >
<makefile target="ap|nps">
<file name="orange_avoider.c"/>
</makefile>
</module>
conf/modules/orange_avoider_guided.xml
+46
View File
@@ -0,0 +1,46 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="orange_avoider_guided" dir="orange_avoider">
<doc>
<description>
This module is an example module for the course AE4317 Autonomous Flight of Micro Air Vehicles at the TU Delft.
This module is used in combination with a color filter (cv_detect_color_object) and the guided mode of the autopilot.
The avoidance strategy is to simply count the total number of orange pixels. When above a certain percentage threshold,
(given by color_count_frac) we assume that there is an obstacle and we turn.
The color filter settings are set using the cv_detect_color_object. This module can run multiple filters simultaneously
so you have to define which filter to use with the ORANGE_AVOIDER_VISUAL_DETECTION_ID setting.
This module differs from the simpler orange_avoider.xml in that this is flown in guided mode. This flight mode is
less dependent on a global positioning estimate as with the navigation mode. This module can be used with a simple
speed estimate rather than a global position.
Here we also need to use our onboard sensors to stay inside of the cyberzoo and not collide with the nets. For this
we employ a simple color detector, similar to the orange poles but for green to detect the floor. When the total amount
of green drops below a given threshold (given by floor_count_frac) we assume we are near the edge of the zoo and turn
around. The color detection is done by the cv_detect_color_object module, use the FLOOR_VISUAL_DETECTION_ID setting to
define which filter to use.
</description>
<define name="ORANGE_AVOIDER_VISUAL_DETECTION_ID" value="ABI_BROADCAST" description="which VISUAL_DETECTION message to recieve for orange pole detection."/>
<define name="FLOOR_VISUAL_DETECTION_ID" value="ABI_BROADCAST" description="which VISUAL_DETECTION message to recieve for floor detection."/>
</doc>
<settings>
<dl_settings>
<dl_settings name="OrangeAvoiderGuided">
<dl_setting var="oag_color_count_frac" min="0" step="0.01" max="0.5"/>
<dl_setting var="oag_floor_count_frac" min="0" step="0.01" max="0.5"/>
<dl_setting var="oag_max_speed" min="0" step="0.1" max="1"/>
<dl_setting var="oag_heading_rate" min="0" step="5" max="45" unit="rad" alt_unit="deg"/>
</dl_settings>
</dl_settings>
</settings>
<depends>cv_detect_color_object</depends>
<header>
<file name="orange_avoider_guided.h"/>
</header>
<init fun="orange_avoider_guided_init()"/>
<periodic fun="orange_avoider_guided_periodic()" freq="4"/>
<makefile target="ap|nps">
<file name="orange_avoider_guided.c"/>
</makefile>
</module>
conf/modules/video_capture.xml
+10 -4
View File
@@ -17,9 +17,15 @@
<settings>
<dl_settings>
<dl_settings name="video_capture">
<dl_setting var="video_capture_take_shot" min="0" step="1" max="1" shortname="capture_frame" module="computer_vision/video_capture">
<strip_button name="Video Save Image" icon="digital-camera.png" value="1" group="cv"/>
<dl_settings name="video">
<dl_setting var="video_capture_record_video" min="0" step="1" max="1" shortname="record_video"
module="computer_vision/video_capture">
<strip_button name="Start video capture" icon="dcstart.png" value="1" group="cv"/>
<strip_button name="Stop video capture" icon="dcstop.png" value="0" group="cv"/>
</dl_setting>
<dl_setting var="video_capture_take_shot" min="0" step="1" max="1" shortname="take_shot"
module="computer_vision/video_capture">
<strip_button name="Save Image" icon="digital-camera.png" value="1" group="cv"/>
</dl_setting>
</dl_settings>
</dl_settings>
@@ -33,7 +39,7 @@
<init fun="video_capture_init()"/>
<makefile target="ap">
<makefile target="ap|nps">
<file name="video_capture.c"/>
</makefile>
conf/userconf/tudelft/course2018_conf.xml
-14
View File
@@ -1,14 +0,0 @@
<conf>
<aircraft
name="bebop_orange_avoid"
ac_id="42"
airframe="airframes/tudelft/bebop_course2018_orangeavoid.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/course2018_orangeavoid_cyberzoo.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/video_rtp_stream.xml modules/cv_colorfilter.xml modules/bebop_cam.xml modules/air_data.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="white"
release="5e3b6247cc7e2770162bf215b8f427eac58fbdaf"
/>
</conf>
conf/userconf/tudelft/course2019_conf.xml
+26
View File
@@ -0,0 +1,26 @@
<conf>
<aircraft
name="bebop_orange_avoid"
ac_id="42"
airframe="airframes/tudelft/bebop_course2019_orangeavoid.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/course2019_orangeavoid_cyberzoo.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/video_rtp_stream.xml modules/orange_avoider.xml modules/cv_detect_color_object.xml modules/video_capture.xml modules/bebop_cam.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="white"
release="c3090b6692780d8b3ad4b32cb9cef06f4186360c"
/>
<aircraft
name="bebop_orange_avoid_guided"
ac_id="43"
airframe="airframes/tudelft/bebop_course2019_orangeavoid_guided.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/course2019_orangeavoid_cyberzoo_guided.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/video_rtp_stream.xml modules/orange_avoider_guided.xml modules/cv_detect_color_object.xml modules/video_capture.xml modules/bebop_cam.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="white"
release="c3090b6692780d8b3ad4b32cb9cef06f4186360c"
/>
</conf>
conf/userconf/tudelft/course2018_control_panel.xml → conf/userconf/tudelft/course2019_control_panel.xml
+5 -3
View File
@@ -23,7 +23,7 @@
<program name="Simulator" command="sw/simulator/pprzsim-launch"/>
<program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
<arg flag="sm600.xml"/>
</program>
<program name="Environment Simulator" command="sw/simulator/gaia"/>
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
@@ -37,6 +37,7 @@
</program>
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-ping_period" constant="100"/>
</program>
<program name="Server">
<arg flag="-n"/>
@@ -59,6 +60,7 @@
</program>
<program name="Data Link">
<arg flag="-udp"/>
<arg flag="-ping_period" constant="100"/>
</program>
<program name="Server">
<arg flag="-n"/>
@@ -74,7 +76,7 @@
</program>
<program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
<arg flag="sm600.xml"/>
<arg flag="-d 0"/>
</program>
<program name="Gazebo"/>
@@ -89,7 +91,7 @@
</program>
<program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
<arg flag="-ac" constant="@AIRCRAFT"/>
<arg flag="hobbyking.xml"/>
<arg flag="sm600.xml"/>
<arg flag="-d 0"/>
</program>
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy">
sw/airborne/modules/computer_vision/colorfilter.c
+3 -7
View File
@@ -51,15 +51,12 @@ uint8_t color_cr_min = 180;
uint8_t color_cr_max = 255;
// Result
int color_count = 0;
volatile int color_count = 0;
#include "subsystems/abi.h"
// Function
struct image_t *colorfilter_func(struct image_t *img);
struct image_t *colorfilter_func(struct image_t *img)
static struct image_t *colorfilter_func(struct image_t *img)
{
// Filter
color_count = image_yuv422_colorfilt(img, img,
@@ -68,7 +65,6 @@ struct image_t *colorfilter_func(struct image_t *img)
color_cr_min, color_cr_max
);
if (COLORFILTER_SEND_OBSTACLE) {
if (color_count > 20)
{
@@ -85,5 +81,5 @@ struct image_t *colorfilter_func(struct image_t *img)
void colorfilter_init(void)
{
listener = cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS);
cv_add_to_device(&COLORFILTER_CAMERA, colorfilter_func, COLORFILTER_FPS);
}
sw/airborne/modules/computer_vision/colorfilter.h
+1 -3
View File
@@ -41,8 +41,6 @@ extern uint8_t color_cb_max;
extern uint8_t color_cr_min;
extern uint8_t color_cr_max;
extern int color_count;
extern struct video_listener *listener;
extern volatile int color_count;
#endif /* COLORFILTER_CV_PLUGIN_H */
sw/airborne/modules/computer_vision/cv_detect_color_object.c
+276
View File
@@ -0,0 +1,276 @@
/*
* Copyright (C) 2019 Kirk Scheper <kirkscheper@gmail.com>
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/computer_vision/cv_detect_object.h
* Assumes the object consists of a continuous color and checks
* if you are over the defined object or not
*/
// Own header
#include "modules/computer_vision/cv_detect_color_object.h"
#include "modules/computer_vision/cv.h"
#include "subsystems/abi.h"
#include "std.h"
#include <stdio.h>
#include <stdbool.h>
#include <math.h>
#include "pthread.h"
#define PRINT(string,...) fprintf(stderr, "[object_detector->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
#if OBJECT_DETECTOR_VERBOSE
#define VERBOSE_PRINT PRINT
#else
#define VERBOSE_PRINT(...)
#endif
static pthread_mutex_t mutex;
#ifndef COLOR_OBJECT_DETECTOR_FPS1
#define COLOR_OBJECT_DETECTOR_FPS1 0 ///< Default FPS (zero means run at camera fps)
#endif
#ifndef COLOR_OBJECT_DETECTOR_FPS2
#define COLOR_OBJECT_DETECTOR_FPS2 0 ///< Default FPS (zero means run at camera fps)
#endif
// Filter Settings
uint8_t cod_lum_min1 = 0;
uint8_t cod_lum_max1 = 0;
uint8_t cod_cb_min1 = 0;
uint8_t cod_cb_max1 = 0;
uint8_t cod_cr_min1 = 0;
uint8_t cod_cr_max1 = 0;
uint8_t cod_lum_min2 = 0;
uint8_t cod_lum_max2 = 0;
uint8_t cod_cb_min2 = 0;
uint8_t cod_cb_max2 = 0;
uint8_t cod_cr_min2 = 0;
uint8_t cod_cr_max2 = 0;
bool cod_draw1 = false;
bool cod_draw2 = false;
// define global variables
struct color_object_t {
int32_t x_c;
int32_t y_c;
uint32_t color_count;
bool updated;
};
struct color_object_t global_filters[2];
// Function
uint32_t find_object_centroid(struct image_t *img, int32_t* p_xc, int32_t* p_yc, bool draw,
uint8_t lum_min, uint8_t lum_max,
uint8_t cb_min, uint8_t cb_max,
uint8_t cr_min, uint8_t cr_max);
/*
* object_detector
* @param img - input image to process
* @param filter - which detection filter to process
* @return img
*/
static struct image_t *object_detector(struct image_t *img, uint8_t filter)
{
uint8_t lum_min, lum_max;
uint8_t cb_min, cb_max;
uint8_t cr_min, cr_max;
bool draw;
switch (filter){
case 1:
lum_min = cod_lum_min1;
lum_max = cod_lum_max1;
cb_min = cod_cb_min1;
cb_max = cod_cb_max1;
cr_min = cod_cr_min1;
cr_max = cod_cr_max1;
draw = cod_draw1;
break;
case 2:
lum_min = cod_lum_min2;
lum_max = cod_lum_max2;
cb_min = cod_cb_min2;
cb_max = cod_cb_max2;
cr_min = cod_cr_min2;
cr_max = cod_cr_max2;
draw = cod_draw2;
break;
default:
return img;
};
int32_t x_c, y_c;
// Filter and find centroid
uint32_t count = find_object_centroid(img, &x_c, &y_c, draw, lum_min, lum_max, cb_min, cb_max, cr_min, cr_max);
VERBOSE_PRINT("Color count %d: %u, threshold %u, x_c %d, y_c %d\n", camera, object_count, count_threshold, x_c, y_c);
VERBOSE_PRINT("centroid %d: (%d, %d) r: %4.2f a: %4.2f\n", camera, x_c, y_c,
hypotf(x_c, y_c) / hypotf(img->w * 0.5, img->h * 0.5), RadOfDeg(atan2f(y_c, x_c)));
pthread_mutex_lock(&mutex);
global_filters[filter-1].color_count = count;
global_filters[filter-1].x_c = x_c;
global_filters[filter-1].y_c = y_c;
global_filters[filter-1].updated = true;
pthread_mutex_unlock(&mutex);
return img;
}
struct image_t *object_detector1(struct image_t *img);
struct image_t *object_detector1(struct image_t *img)
{
return object_detector(img, 1);
}
struct image_t *object_detector2(struct image_t *img);
struct image_t *object_detector2(struct image_t *img)
{
return object_detector(img, 2);
}
void color_object_detector_init(void)
{
memset(global_filters, 0, 2*sizeof(struct color_object_t));
pthread_mutex_init(&mutex, NULL);
#ifdef COLOR_OBJECT_DETECTOR_CAMERA1
#ifdef COLOR_OBJECT_DETECTOR_LUM_MIN1
cod_lum_min1 = COLOR_OBJECT_DETECTOR_LUM_MIN1;
cod_lum_max1 = COLOR_OBJECT_DETECTOR_LUM_MAX1;
cod_cb_min1 = COLOR_OBJECT_DETECTOR_CB_MIN1;
cod_cb_max1 = COLOR_OBJECT_DETECTOR_CB_MAX1;
cod_cr_min1 = COLOR_OBJECT_DETECTOR_CR_MIN1;
cod_cr_max1 = COLOR_OBJECT_DETECTOR_CR_MAX1;
#endif
#ifdef COLOR_OBJECT_DETECTOR_DRAW1
cod_draw1 = COLOR_OBJECT_DETECTOR_DRAW1;
#endif
cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA1, object_detector1, COLOR_OBJECT_DETECTOR_FPS1);
#endif
#ifdef COLOR_OBJECT_DETECTOR_CAMERA2
#ifdef COLOR_OBJECT_DETECTOR_LUM_MIN2
cod_lum_min2 = COLOR_OBJECT_DETECTOR_LUM_MIN2;
cod_lum_max2 = COLOR_OBJECT_DETECTOR_LUM_MAX2;
cod_cb_min2 = COLOR_OBJECT_DETECTOR_CB_MIN2;
cod_cb_max2 = COLOR_OBJECT_DETECTOR_CB_MAX2;
cod_cr_min2 = COLOR_OBJECT_DETECTOR_CR_MIN2;
cod_cr_max2 = COLOR_OBJECT_DETECTOR_CR_MAX2;
#endif
#ifdef COLOR_OBJECT_DETECTOR_DRAW2
cod_draw2 = COLOR_OBJECT_DETECTOR_DRAW2;
#endif
cv_add_to_device(&COLOR_OBJECT_DETECTOR_CAMERA2, object_detector2, COLOR_OBJECT_DETECTOR_FPS2);
#endif
}
/*
* find_object_centroid
*
* Finds the centroid of pixels in an image within filter bounds.
* Also returns the amount of pixels that satisfy these filter bounds.
*
* @param img - input image to process formatted as YUV422.
* @param p_xc - x coordinate of the centroid of color object
* @param p_yc - y coordinate of the centroid of color object
* @param lum_min - minimum y value for the filter in YCbCr colorspace
* @param lum_max - maximum y value for the filter in YCbCr colorspace
* @param cb_min - minimum cb value for the filter in YCbCr colorspace
* @param cb_max - maximum cb value for the filter in YCbCr colorspace
* @param cr_min - minimum cr value for the filter in YCbCr colorspace
* @param cr_max - maximum cr value for the filter in YCbCr colorspace
* @param draw - whether or not to draw on image
* @return number of pixels of image within the filter bounds.
*/
uint32_t find_object_centroid(struct image_t *img, int32_t* p_xc, int32_t* p_yc, bool draw,
uint8_t lum_min, uint8_t lum_max,
uint8_t cb_min, uint8_t cb_max,
uint8_t cr_min, uint8_t cr_max)
{
uint32_t cnt = 0;
uint32_t tot_x = 0;
uint32_t tot_y = 0;
uint8_t *buffer = img->buf;
// Go through all the pixels
for (uint16_t y = 0; y < img->h; y++) {
for (uint16_t x = 0; x < img->w; x ++) {
// Check if the color is inside the specified values
uint8_t *yp, *up, *vp;
if (x % 2 == 0) {
// Even x
up = &buffer[y * 2 * img->w + 2 * x]; // U
yp = &buffer[y * 2 * img->w + 2 * x + 1]; // Y1
vp = &buffer[y * 2 * img->w + 2 * x + 2]; // V
//yp = &buffer[y * 2 * img->w + 2 * x + 3]; // Y2
} else {
// Uneven x
up = &buffer[y * 2 * img->w + 2 * x - 2]; // U
//yp = &buffer[y * 2 * img->w + 2 * x - 1]; // Y1
vp = &buffer[y * 2 * img->w + 2 * x]; // V
yp = &buffer[y * 2 * img->w + 2 * x + 1]; // Y2
}
if ( (*yp >= lum_min) && (*yp <= lum_max) &&
(*up >= cb_min ) && (*up <= cb_max ) &&
(*vp >= cr_min ) && (*vp <= cr_max )) {
cnt ++;
tot_x += x;
tot_y += y;
if (draw){
*yp = 255; // make pixel brighter in image
}
}
}
}
if (cnt > 0) {
*p_xc = (int32_t)roundf(tot_x / ((float) cnt) - img->w * 0.5f);
*p_yc = (int32_t)roundf(img->h * 0.5f - tot_y / ((float) cnt));
} else {
*p_xc = 0;
*p_yc = 0;
}
return cnt;
}
void color_object_detector_periodic(void)
{
static struct color_object_t local_filters[2];
pthread_mutex_lock(&mutex);
memcpy(local_filters, global_filters, 2*sizeof(struct color_object_t));
pthread_mutex_unlock(&mutex);
if(local_filters[0].updated){
AbiSendMsgVISUAL_DETECTION(COLOR_OBJECT_DETECTION1_ID, local_filters[0].x_c, local_filters[0].y_c,
0, 0, local_filters[0].color_count, 0);
local_filters[0].updated = false;
}
if(local_filters[1].updated){
AbiSendMsgVISUAL_DETECTION(COLOR_OBJECT_DETECTION2_ID, local_filters[1].x_c, local_filters[1].y_c,
0, 0, local_filters[1].color_count, 1);
local_filters[1].updated = false;
}
}
sw/airborne/modules/computer_vision/cv_detect_color_object.h
+56
View File
@@ -0,0 +1,56 @@
/*
* Copyright (C) 2019 Kirk Scheper <kirkscheper@gmail.com>
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/computer_vision/cv_detect_color_object.h
* Assumes the color_object consists of a continuous color and checks
* if you are over the defined color_object or not
*/
#ifndef COLOR_OBJECT_DETECTOR_CV_H
#define COLOR_OBJECT_DETECTOR_CV_H
#include <stdint.h>
#include <stdbool.h>
// Module settings
extern uint8_t cod_lum_min1;
extern uint8_t cod_lum_max1;
extern uint8_t cod_cb_min1;
extern uint8_t cod_cb_max1;
extern uint8_t cod_cr_min1;
extern uint8_t cod_cr_max1;
extern uint8_t cod_lum_min2;
extern uint8_t cod_lum_max2;
extern uint8_t cod_cb_min2;
extern uint8_t cod_cb_max2;
extern uint8_t cod_cr_min2;
extern uint8_t cod_cr_max2;
extern bool cod_draw1;
extern bool cod_draw2;
// Module functions
extern void color_object_detector_init(void);
extern void color_object_detector_periodic(void);
#endif /* COLOR_OBJECT_DETECTOR_CV_H */
sw/airborne/modules/computer_vision/opencv_example.cpp
+13 -9
View File
@@ -34,26 +34,30 @@ using namespace std;
using namespace cv;
#include "opencv_image_functions.h"
int opencv_example(char *img, int width, int height)
{
// Create a new image, using the original bebop image.
Mat M(height, width, CV_8UC2, img);
Mat image;
// If you want a color image, uncomment this line
// cvtColor(M, image, CV_YUV2BGR_Y422);
// For a grayscale image, use this one
#if OPENCVDEMO_GRAYSCALE
// Grayscale image example
cvtColor(M, image, CV_YUV2GRAY_Y422);
// Blur it, because we can
// blur(image, image, Size(5, 5));
// Canny edges, only works with grayscale image
int edgeThresh = 35;
Canny(image, image, edgeThresh, edgeThresh * 3);
// Convert back to YUV422, and put it in place of the original image
grayscale_opencv_to_yuv422(image, img, width, height);
// colorrgb_opencv_to_yuv422(image, img, width, height);
#else // OPENCVDEMO_GRAYSCALE
// Color image example
// Convert the image to an OpenCV Mat
cvtColor(M, image, CV_YUV2BGR_Y422);
// Blur it, because we can
blur(image, image, Size(5, 5));
// Convert back to YUV422 and put it in place of the original image
colorbgr_opencv_to_yuv422(image, img, width, height);
#endif // OPENCVDEMO_GRAYSCALE
return 0;
}
sw/airborne/modules/computer_vision/opencv_image_functions.cpp
+11 -21
View File
@@ -42,31 +42,21 @@ void coloryuv_opencv_to_yuv422(Mat image, char *img, int width, int height)
int nRows = image.rows;
int nCols = image.cols;
// If the image is one block in memory we can iterate over it all at once!
if (image.isContinuous()) {
nCols *= nRows;
nRows = 1;
}
// Iterate over the image, setting only the Y value
// and setting U and V to 127
int i, j;
uchar *p;
int index_img = 0;
for (i = 0; i < nRows; ++i) {
p = image.ptr<uchar>(i);
for (j = 0; j < nCols; j += 6) {
img[index_img++] = p[j + 1]; //U
img[index_img++] = p[j];//Y
img[index_img++] = p[j + 2]; //V
img[index_img++] = p[j + 3]; //Y
int byte_index = 0;
for(int r = 0; r < nRows; ++r) {
for(int c = 0; c < nCols; ++c) {
Vec3b yuv = image.at<Vec3b>(r, c);
if((byte_index % 4) == 0) {
img[byte_index++] = yuv.val[1]; // U
} else {
img[byte_index++] = yuv.val[2]; // V
}
img[byte_index++] = yuv.val[0]; // Y
}
}
}
void colorrgb_opencv_to_yuv422(Mat image, char *img, int width, int height)
void colorbgr_opencv_to_yuv422(Mat image, char *img, int width, int height)
{
// Convert to YUV color space
cvtColor(image, image, COLOR_BGR2YUV);
sw/airborne/modules/computer_vision/opencv_image_functions.h
+1 -1
View File
@@ -36,7 +36,7 @@
* Note that the rgb function first converts to YUV, and then to YUV422 making
* this function slower than coloryuv_opencv_to_yuv422.
*/
void colorrgb_opencv_to_yuv422(cv::Mat image, char *img, int width, int height);
void colorbgr_opencv_to_yuv422(cv::Mat image, char *img, int width, int height);
/**
* Converts cv::Mat with three channels YUV to a YUV422 image.
sw/airborne/modules/computer_vision/video_capture.c
+55 -41
View File
@@ -26,6 +26,7 @@
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <time.h>
#include "modules/computer_vision/video_capture.h"
#include "modules/computer_vision/cv.h"
@@ -52,9 +53,13 @@
PRINT_CONFIG_VAR(VIDEO_CAPTURE_FPS)
// Module settings
bool video_capture_take_shot = false;
bool video_capture_take_shot = false; // Capture single images
bool video_capture_record_video = false; // Capture video
int video_capture_index = 0;
// Save directory
static char save_dir[256];
// Forward function declarations
struct image_t *video_capture_func(struct image_t *img);
void video_capture_save(struct image_t *img);
@@ -62,13 +67,17 @@ void video_capture_save(struct image_t *img);
void video_capture_init(void)
{
// Create the images directory
char save_name[128];
sprintf(save_name, "mkdir -p %s", STRINGIFY(VIDEO_CAPTURE_PATH));
if (system(save_name) != 0) {
printf("[video_capture] Could not create images directory %s.\n", STRINGIFY(VIDEO_CAPTURE_PATH));
return;
}
// Create images directory with timestamp
struct timeval tv;
struct tm *tm;
gettimeofday(&tv, NULL);
tm = localtime(&tv.tv_sec);
sprintf(save_dir, "%s/%04d%02d%02d-%02d%02d%02d", STRINGIFY(VIDEO_CAPTURE_PATH),
tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday,
tm->tm_hour, tm->tm_min, tm->tm_sec);
// Folder creation delayed until capture starts, see video_capture_save.
// This prevents empty folders if nothing is actually recorded.
// Add function to computer vision pipeline
cv_add_to_device(&VIDEO_CAPTURE_CAMERA, video_capture_func, VIDEO_CAPTURE_FPS);
@@ -78,7 +87,7 @@ void video_capture_init(void)
struct image_t *video_capture_func(struct image_t *img)
{
// If take_shot bool is set, save the image
if (video_capture_take_shot) {
if (video_capture_take_shot || video_capture_record_video) {
video_capture_save(img);
video_capture_take_shot = false;
}
@@ -94,48 +103,53 @@ void video_capture_shoot(void)
video_capture_take_shot = true;
}
void video_capture_start_capture(void) {
video_capture_record_video = true;
}
void video_capture_stop_capture(void) {
video_capture_record_video = false;
}
void video_capture_save(struct image_t *img)
{
// Declare storage for image location
char save_name[128];
// Simple shot counter to find first available image location
for (/* no init */; video_capture_index < 9999; ++video_capture_index) {
// Generate image location
sprintf(save_name, "%s/img_%05d.jpg", STRINGIFY(VIDEO_CAPTURE_PATH), video_capture_index);
// Continue with next number if file exists already
if (access(save_name, F_OK) != -1) {
continue;
// Create output folder if necessary
if (access(save_dir, F_OK)) {
char save_dir_cmd[256];
sprintf(save_dir_cmd, "mkdir -p %s", save_dir);
if (system(save_dir_cmd) != 0) {
printf("[video_capture] Could not create images directory %s.\n", save_dir);
return;
}
}
printf("[video_capture] Saving image to %s.\n", save_name);
// Declare storage for image location
char save_name[256];
// Create jpg image from raw frame
struct image_t img_jpeg;
image_create(&img_jpeg, img->w, img->h, IMAGE_JPEG);
jpeg_encode_image(img, &img_jpeg, VIDEO_CAPTURE_JPEG_QUALITY, true);
// Generate image filename from image timestamp
sprintf(save_name, "%s/%u.jpg", save_dir, img->pprz_ts);
printf("[video_capture] Saving image to %s.\n", save_name);
// Create jpg image from raw frame
struct image_t img_jpeg;
image_create(&img_jpeg, img->w, img->h, IMAGE_JPEG);
jpeg_encode_image(img, &img_jpeg, VIDEO_CAPTURE_JPEG_QUALITY, true);
#if JPEG_WITH_EXIF_HEADER
write_exif_jpeg(save_name, img_jpeg.buf, img_jpeg.buf_size, img_jpeg.w, img_jpeg.h);
write_exif_jpeg(save_name, img_jpeg.buf, img_jpeg.buf_size, img_jpeg.w, img_jpeg.h);
#else
// Open file
FILE *fp = fopen(save_name, "w");
if (fp == NULL) {
printf("[video_capture] Could not write shot %s.\n", save_name);
break;
}
// Open file
FILE *fp = fopen(save_name, "w");
if (fp == NULL) {
printf("[video_capture] Could not write shot %s.\n", save_name);
return;
}
// Save it to the file and close it
fwrite(img_jpeg.buf, sizeof(uint8_t), img_jpeg.buf_size, fp);
fclose(fp);
// Save it to the file and close it
fwrite(img_jpeg.buf, sizeof(uint8_t), img_jpeg.buf_size, fp);
fclose(fp);
#endif
// Free image
image_free(&img_jpeg);
// End loop here
break;
}
// Free image
image_free(&img_jpeg);
}
sw/airborne/modules/computer_vision/video_capture.h
+4 -1
View File
@@ -30,9 +30,12 @@
// Module settings
extern bool video_capture_take_shot;
extern bool video_capture_record_video;
// Module functions
extern void video_capture_init(void);
extern void video_capture_shoot(void);
extern void video_capture_shoot(void); // Capture single image
extern void video_capture_start_capture(void); // Start video capture
extern void video_capture_stop_capture(void); // Stop video capture
#endif /* VIDEO_CAPTURE_H_ */
sw/airborne/modules/loggers/file_logger.c
+72 -70
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
* 2019 Tom van Dijk <tomvand@users.noreply.github.com>
*
* This file is part of paparazzi.
*
@@ -29,9 +30,12 @@
#include <stdio.h>
#include <sys/stat.h>
#include <time.h>
#include <unistd.h>
#include "std.h"
#include "subsystems/imu.h"
#include "mcu_periph/sys_time.h"
#include "state.h"
#include "generated/airframe.h"
#ifdef COMMAND_THRUST
#include "firmwares/rotorcraft/stabilization.h"
#else
@@ -39,7 +43,6 @@
#include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
#endif
#include "state.h"
/** Set the default File logger path to the USB drive */
#ifndef FILE_LOGGER_PATH
@@ -49,24 +52,74 @@
/** The file pointer */
static FILE *file_logger = NULL;
/** Logging functions */
/** Write CSV header
* Write column names at the top of the CSV file. Make sure that the columns
* match those in file_logger_write_row! Don't forget the \n at the end of the
* line.
* @param file Log file pointer
*/
static void file_logger_write_header(FILE *file) {
fprintf(file, "time,");
fprintf(file, "pos_x,pos_y,pos_z,");
fprintf(file, "vel_x,vel_y,vel_z,");
fprintf(file, "att_phi,att_theta,att_psi,");
fprintf(file, "rate_p,rate_q,rate_r,");
#ifdef COMMAND_THRUST
fprintf(file, "cmd_thrust,cmd_roll,cmd_pitch,cmd_yaw\n");
#else
fprintf(file, "h_ctl_aileron_setpoint,h_ctl_elevator_setpoint\n");
#endif
}
/** Write CSV row
* Write values at this timestamp to log file. Make sure that the printf's match
* the column headers of file_logger_write_header! Don't forget the \n at the
* end of the line.
* @param file Log file pointer
*/
static void file_logger_write_row(FILE *file) {
struct NedCoor_f *pos = stateGetPositionNed_f();
struct NedCoor_f *vel = stateGetSpeedNed_f();
struct FloatEulers *att = stateGetNedToBodyEulers_f();
struct FloatRates *rates = stateGetBodyRates_f();
fprintf(file, "%f,", get_sys_time_float());
fprintf(file, "%f,%f,%f,", pos->x, pos->y, pos->z);
fprintf(file, "%f,%f,%f,", vel->x, vel->y, vel->z);
fprintf(file, "%f,%f,%f,", att->phi, att->theta, att->psi);
fprintf(file, "%f,%f,%f,", rates->p, rates->q, rates->r);
#ifdef COMMAND_THRUST
fprintf(file, "%d,%d,%d,%d\n",
stabilization_cmd[COMMAND_THRUST], stabilization_cmd[COMMAND_ROLL],
stabilization_cmd[COMMAND_PITCH], stabilization_cmd[COMMAND_YAW]);
#else
fprintf(file, "%d,%d\n", h_ctl_aileron_setpoint, h_ctl_elevator_setpoint);
#endif
}
/** Start the file logger and open a new file */
void file_logger_start(void)
{
// check if log path exists
struct stat s;
int err = stat(STRINGIFY(FILE_LOGGER_PATH), &s);
if(err < 0) {
// try to make the directory
mkdir(STRINGIFY(FILE_LOGGER_PATH), 0666);
// Create output folder if necessary
if (access(STRINGIFY(FILE_LOGGER_PATH), F_OK)) {
char save_dir_cmd[256];
sprintf(save_dir_cmd, "mkdir -p %s", STRINGIFY(FILE_LOGGER_PATH));
if (system(save_dir_cmd) != 0) {
printf("[file_logger] Could not create log file directory %s.\n", STRINGIFY(FILE_LOGGER_PATH));
return;
}
}
// Get current date/time, format is YYYY-MM-DD.HH:mm:ss
// Get current date/time for filename
char date_time[80];
time_t now = time(0);
struct tm tstruct;
tstruct = *localtime(&now);
strftime(date_time, sizeof(date_time), "%Y-%m-%d_%X", &tstruct);
strftime(date_time, sizeof(date_time), "%Y%m%d-%H%M%S", &tstruct);
uint32_t counter = 0;
char filename[512];
@@ -81,19 +134,14 @@ void file_logger_start(void)
}
file_logger = fopen(filename, "w");
if (file_logger != NULL) {
fprintf(
file_logger,
//rotorcraft uses COMMAND_THRUST, fixedwing COMMAND_THROTTLE at this time
#ifdef COMMAND_THRUST
"counter,gyro_unscaled_p,gyro_unscaled_q,gyro_unscaled_r,accel_unscaled_x,accel_unscaled_y,accel_unscaled_z,mag_unscaled_x,mag_unscaled_y,mag_unscaled_z,COMMAND_THRUST,COMMAND_ROLL,COMMAND_PITCH,COMMAND_YAW,qi,qx,qy,qz\n"
#else
"counter,gyro_unscaled_p,gyro_unscaled_q,gyro_unscaled_r,accel_unscaled_x,accel_unscaled_y,accel_unscaled_z,mag_unscaled_x,mag_unscaled_y,mag_unscaled_z, h_ctl_aileron_setpoint, h_ctl_elevator_setpoint, qi,qx,qy,qz\n"
#endif
);
if(!file_logger) {
printf("[file_logger] ERROR opening log file %s!\n", filename);
return;
}
printf("[file_logger] Start logging to %s...\n", filename);
file_logger_write_header(file_logger);
}
/** Stop the logger an nicely close the file */
@@ -106,56 +154,10 @@ void file_logger_stop(void)
}
/** Log the values to a csv file */
/** Change the Variable that you are interested in here */
void file_logger_periodic(void)
{
if (file_logger == NULL) {
return;
}
static uint32_t counter;
struct Int32Quat *quat = stateGetNedToBodyQuat_i();
#ifdef COMMAND_THRUST //For example rotorcraft
fprintf(file_logger, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
counter,
imu.gyro_unscaled.p,
imu.gyro_unscaled.q,
imu.gyro_unscaled.r,
imu.accel_unscaled.x,
imu.accel_unscaled.y,
imu.accel_unscaled.z,
imu.mag_unscaled.x,
imu.mag_unscaled.y,
imu.mag_unscaled.z,
stabilization_cmd[COMMAND_THRUST],
stabilization_cmd[COMMAND_ROLL],
stabilization_cmd[COMMAND_PITCH],
stabilization_cmd[COMMAND_YAW],
quat->qi,
quat->qx,
quat->qy,
quat->qz
);
#else
fprintf(file_logger, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
counter,
imu.gyro_unscaled.p,
imu.gyro_unscaled.q,
imu.gyro_unscaled.r,
imu.accel_unscaled.x,
imu.accel_unscaled.y,
imu.accel_unscaled.z,
imu.mag_unscaled.x,
imu.mag_unscaled.y,
imu.mag_unscaled.z,
h_ctl_aileron_setpoint,
h_ctl_elevator_setpoint,
quat->qi,
quat->qx,
quat->qy,
quat->qz
);
#endif
counter++;
file_logger_write_row(file_logger);
}
sw/airborne/modules/orange_avoider/orange_avoider.c
+144 -87
View File
@@ -8,19 +8,25 @@
* @file "modules/orange_avoider/orange_avoider.c"
* @author Roland Meertens
* Example on how to use the colours detected to avoid orange pole in the cyberzoo
* This module is an example module for the course AE4317 Autonomous Flight of Micro Air Vehicles at the TU Delft.
* This module is used in combination with a color filter (cv_detect_color_object) and the navigation mode of the autopilot.
* The avoidance strategy is to simply count the total number of orange pixels. When above a certain percentage threshold,
* (given by color_count_frac) we assume that there is an obstacle and we turn.
*
* The color filter settings are set using the cv_detect_color_object. This module can run multiple filters simultaneously
* so you have to define which filter to use with the ORANGE_AVOIDER_VISUAL_DETECTION_ID setting.
*/
#include "modules/orange_avoider/orange_avoider.h"
#include "modules/computer_vision/colorfilter.h"
#include "firmwares/rotorcraft/navigation.h"
#include "generated/flight_plan.h"
#include "generated/airframe.h"
#include "state.h"
#include "subsystems/abi.h"
#include <time.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#define NAV_C // needed to get the nav funcitons like Inside...
#include "generated/flight_plan.h"
#define ORANGE_AVOIDER_VERBOSE TRUE
@@ -31,79 +37,131 @@
#define VERBOSE_PRINT(...)
#endif
#ifndef ORANGE_AVOIDER_LUM_MIN
#define ORANGE_AVOIDER_LUM_MIN 41
uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters);
uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor);
uint8_t increase_nav_heading(float incrementDegrees);
uint8_t chooseRandomIncrementAvoidance(void);
enum navigation_state_t {
SAFE,
OBSTACLE_FOUND,
SEARCH_FOR_SAFE_HEADING,
OUT_OF_BOUNDS
};
// define settings
float oa_color_count_frac = 0.18f;
// define and initialise global variables
enum navigation_state_t navigation_state = SEARCH_FOR_SAFE_HEADING;
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 heading_increment = 5.f; // heading angle increment [deg]
float maxDistance = 2.25; // max waypoint displacement [m]
const int16_t max_trajectory_confidence = 5; // number of consecutive negative object detections to be sure we are obstacle free
#ifndef ORANGE_AVOIDER_VISUAL_DETECTION_ID
#define ORANGE_AVOIDER_VISUAL_DETECTION_ID ABI_BROADCAST
#endif
#ifndef ORANGE_AVOIDER_LUM_MAX
#define ORANGE_AVOIDER_LUM_MAX 183
#endif
#ifndef ORANGE_AVOIDER_CB_MIN
#define ORANGE_AVOIDER_CB_MIN 53
#endif
#ifndef ORANGE_AVOIDER_CB_MAX
#define ORANGE_AVOIDER_CB_MAX 121
#endif
#ifndef ORANGE_AVOIDER_CR_MIN
#define ORANGE_AVOIDER_CR_MIN 134
#endif
#ifndef ORANGE_AVOIDER_CR_MAX
#define ORANGE_AVOIDER_CR_MAX 249
#endif
uint8_t safeToGoForwards = false;
int tresholdColorCount = 0.05 * 124800; // 520 x 240 = 124.800 total pixels
float incrementForAvoidance;
uint16_t trajectoryConfidence = 1;
float maxDistance = 2.25;
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;
}
/*
* Initialisation function, setting the colour filter, random seed and incrementForAvoidance
* Initialisation function, setting the colour filter, random seed and heading_increment
*/
void orange_avoider_init()
void orange_avoider_init(void)
{
// Initialise the variables of the colorfilter to accept orange
color_lum_min = ORANGE_AVOIDER_LUM_MIN;
color_lum_max = ORANGE_AVOIDER_LUM_MAX;
color_cb_min = ORANGE_AVOIDER_CB_MIN;
color_cb_max = ORANGE_AVOIDER_CB_MAX;
color_cr_min = ORANGE_AVOIDER_CR_MIN;
color_cr_max = ORANGE_AVOIDER_CR_MAX;
// Initialise random values
srand(time(NULL));
chooseRandomIncrementAvoidance();
// bind our colorfilter callbacks to receive the color filter outputs
AbiBindMsgVISUAL_DETECTION(ORANGE_AVOIDER_VISUAL_DETECTION_ID, &color_detection_ev, color_detection_cb);
}
/*
* Function that checks it is safe to move forwards, and then moves a waypoint forward or changes the heading
*/
void orange_avoider_periodic()
void orange_avoider_periodic(void)
{
// Check the amount of orange. If this is above a threshold
// you want to turn a certain amount of degrees
safeToGoForwards = color_count < tresholdColorCount;
VERBOSE_PRINT("Color_count: %d threshold: %d safe: %d \n", color_count, tresholdColorCount, safeToGoForwards);
float moveDistance = fmin(maxDistance, 0.05 * trajectoryConfidence);
if (safeToGoForwards) {
moveWaypointForward(WP_GOAL, moveDistance);
moveWaypointForward(WP_TRAJECTORY, 1.25 * moveDistance);
nav_set_heading_towards_waypoint(WP_GOAL);
chooseRandomIncrementAvoidance();
trajectoryConfidence += 1;
// only evaluate our state machine if we are flying
if(!autopilot_in_flight()){
return;
}
// compute current color thresholds
int32_t color_count_threshold = oa_color_count_frac * front_camera.output_size.w * front_camera.output_size.h;
VERBOSE_PRINT("Color_count: %d threshold: %d state: %d \n", color_count, color_count, navigation_state);
// update our safe confidence using color threshold
if(color_count < color_count_threshold){
obstacle_free_confidence++;
} else {
waypoint_set_here_2d(WP_GOAL);
waypoint_set_here_2d(WP_TRAJECTORY);
increase_nav_heading(&nav_heading, incrementForAvoidance);
if (trajectoryConfidence > 5) {
trajectoryConfidence -= 4;
} else {
trajectoryConfidence = 1;
}
obstacle_free_confidence -= 2; // be more cautious with positive obstacle detections
}
// bound obstacle_free_confidence
Bound(obstacle_free_confidence, 0, max_trajectory_confidence);
float moveDistance = fminf(maxDistance, 0.2f * obstacle_free_confidence);
switch (navigation_state){
case SAFE:
// Move waypoint forward
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:
// stop
waypoint_move_here_2d(WP_GOAL);
waypoint_move_here_2d(WP_TRAJECTORY);
// randomly select new search direction
chooseRandomIncrementAvoidance();
navigation_state = SEARCH_FOR_SAFE_HEADING;
break;
case SEARCH_FOR_SAFE_HEADING:
increase_nav_heading(heading_increment);
// make sure we have a couple of good readings before declaring the way safe
if (obstacle_free_confidence >= 2){
navigation_state = SAFE;
}
break;
case OUT_OF_BOUNDS:
increase_nav_heading(heading_increment);
moveWaypointForward(WP_TRAJECTORY, 1.5f);
if (InsideObstacleZone(WaypointX(WP_TRAJECTORY),WaypointY(WP_TRAJECTORY))){
// add offset to head back into arena
increase_nav_heading(heading_increment);
// reset safe counter
obstacle_free_confidence = 0;
// ensure direction is safe before continuing
navigation_state = SEARCH_FOR_SAFE_HEADING;
}
break;
default:
break;
}
return;
}
@@ -111,14 +169,17 @@ void orange_avoider_periodic()
/*
* Increases the NAV heading. Assumes heading is an INT32_ANGLE. It is bound in this function.
*/
uint8_t increase_nav_heading(int32_t *heading, float incrementDegrees)
uint8_t increase_nav_heading(float incrementDegrees)
{
struct Int32Eulers *eulerAngles = stateGetNedToBodyEulers_i();
int32_t newHeading = eulerAngles->psi + ANGLE_BFP_OF_REAL(RadOfDeg(incrementDegrees));
// Check if your turn made it go out of bounds...
INT32_ANGLE_NORMALIZE(newHeading); // HEADING HAS INT32_ANGLE_FRAC....
*heading = newHeading;
VERBOSE_PRINT("Increasing heading to %f\n", DegOfRad(ANGLE_FLOAT_OF_BFP(*heading)));
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);
VERBOSE_PRINT("Increasing heading to %f\n", DegOfRad(new_heading));
return false;
}
@@ -127,17 +188,14 @@ uint8_t increase_nav_heading(int32_t *heading, float incrementDegrees)
*/
static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
{
struct EnuCoor_i *pos = stateGetPositionEnu_i(); // Get your current position
struct Int32Eulers *eulerAngles = stateGetNedToBodyEulers_i();
// Calculate the sine and cosine of the heading the drone is keeping
float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(eulerAngles->psi));
float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(eulerAngles->psi));
float heading = stateGetNedToBodyEulers_f()->psi;
// Now determine where to place the waypoint you want to go to
new_coor->x = pos->x + POS_BFP_OF_REAL(sin_heading * (distanceMeters));
new_coor->y = pos->y + POS_BFP_OF_REAL(cos_heading * (distanceMeters));
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));
VERBOSE_PRINT("Calculated %f m forward position. x: %f y: %f based on pos(%f, %f) and heading(%f)\n", distanceMeters,
POS_FLOAT_OF_BFP(new_coor->x), POS_FLOAT_OF_BFP(new_coor->y), POS_FLOAT_OF_BFP(pos->x), POS_FLOAT_OF_BFP(pos->y),
DegOfRad(ANGLE_FLOAT_OF_BFP(eulerAngles->psi)) );
POS_FLOAT_OF_BFP(new_coor->x), POS_FLOAT_OF_BFP(new_coor->y),
stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, DegOfRad(heading));
return false;
}
@@ -148,7 +206,7 @@ uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor)
{
VERBOSE_PRINT("Moving waypoint %d to x:%f y:%f\n", waypoint, POS_FLOAT_OF_BFP(new_coor->x),
POS_FLOAT_OF_BFP(new_coor->y));
waypoint_set_xy_i(waypoint, new_coor->x, new_coor->y);
waypoint_move_xy_i(waypoint, new_coor->x, new_coor->y);
return false;
}
@@ -164,18 +222,17 @@ uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
}
/*
* Sets the variable 'incrementForAvoidance' randomly positive/negative
* Sets the variable 'heading_increment' randomly positive/negative
*/
uint8_t chooseRandomIncrementAvoidance()
uint8_t chooseRandomIncrementAvoidance(void)
{
// Randomly choose CW or CCW avoiding direction
int r = rand() % 2;
if (r == 0) {
incrementForAvoidance = 10.0;
VERBOSE_PRINT("Set avoidance increment to: %f\n", incrementForAvoidance);
if (rand() % 2 == 0) {
heading_increment = 5.f;
VERBOSE_PRINT("Set avoidance increment to: %f\n", heading_increment);
} else {
incrementForAvoidance = -10.0;
VERBOSE_PRINT("Set avoidance increment to: %f\n", incrementForAvoidance);
heading_increment = -5.f;
VERBOSE_PRINT("Set avoidance increment to: %f\n", heading_increment);
}
return false;
}
sw/airborne/modules/orange_avoider/orange_avoider.h
+4 -9
View File
@@ -12,18 +12,13 @@
#ifndef ORANGE_AVOIDER_H
#define ORANGE_AVOIDER_H
#include <inttypes.h>
#include "math/pprz_geodetic_int.h"
extern uint8_t safeToGoForwards;
extern float incrementForAvoidance;
extern uint16_t trajectoryConfidence;
// settings
extern float oa_color_count_frac;
// functions
extern void orange_avoider_init(void);
extern void orange_avoider_periodic(void);
extern uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters);
extern uint8_t moveWaypoint(uint8_t waypoint, struct EnuCoor_i *new_coor);
extern uint8_t increase_nav_heading(int32_t *heading, float incrementDegrees);
extern uint8_t chooseRandomIncrementAvoidance(void);
#endif
sw/airborne/modules/orange_avoider/orange_avoider_guided.c
+219
View File
@@ -0,0 +1,219 @@
/*
* Copyright (C) Kirk Scheper <kirkscheper@gmail.com>
*
* This file is part of paparazzi
*
*/
/**
* @file "modules/orange_avoider/orange_avoider_guided.c"
* @author Kirk Scheper
* This module is an example module for the course AE4317 Autonomous Flight of Micro Air Vehicles at the TU Delft.
* This module is used in combination with a color filter (cv_detect_color_object) and the guided mode of the autopilot.
* The avoidance strategy is to simply count the total number of orange pixels. When above a certain percentage threshold,
* (given by color_count_frac) we assume that there is an obstacle and we turn.
*
* The color filter settings are set using the cv_detect_color_object. This module can run multiple filters simultaneously
* so you have to define which filter to use with the ORANGE_AVOIDER_VISUAL_DETECTION_ID setting.
* This module differs from the simpler orange_avoider.xml in that this is flown in guided mode. This flight mode is
* less dependent on a global positioning estimate as witht the navigation mode. This module can be used with a simple
* speed estimate rather than a global position.
*
* Here we also need to use our onboard sensors to stay inside of the cyberzoo and not collide with the nets. For this
* we employ a simple color detector, similar to the orange poles but for green to detect the floor. When the total amount
* of green drops below a given threshold (given by floor_count_frac) we assume we are near the edge of the zoo and turn
* around. The color detection is done by the cv_detect_color_object module, use the FLOOR_VISUAL_DETECTION_ID setting to
* define which filter to use.
*/
#include "modules/orange_avoider/orange_avoider_guided.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "generated/airframe.h"
#include "state.h"
#include "subsystems/abi.h"
#include <stdio.h>
#include <time.h>
#define ORANGE_AVOIDER_VERBOSE TRUE
#define PRINT(string,...) fprintf(stderr, "[orange_avoider_guided->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
#if ORANGE_AVOIDER_VERBOSE
#define VERBOSE_PRINT PRINT
#else
#define VERBOSE_PRINT(...)
#endif
uint8_t chooseRandomIncrementAvoidance(void);
enum navigation_state_t {
SAFE,
OBSTACLE_FOUND,
SEARCH_FOR_SAFE_HEADING,
OUT_OF_BOUNDS,
REENTER_ARENA
};
// define settings
float oag_color_count_frac = 0.18f; // obstacle detection threshold as a fraction of total of image
float oag_floor_count_frac = 0.05f; // floor detection threshold as a fraction of total of image
float oag_max_speed = 0.5f; // max flight speed [m/s]
float oag_heading_rate = RadOfDeg(20.f); // heading change setpoint for avoidance [rad/s]
// define and initialise global variables
enum navigation_state_t navigation_state = SEARCH_FOR_SAFE_HEADING; // current state in state machine
int32_t color_count = 0; // orange color count from color filter for obstacle detection
int32_t floor_count = 0; // green color count from color filter for floor detection
int32_t floor_centroid = 0; // floor detector centroid in y direction (along the horizon)
float avoidance_heading_direction = 0; // heading change direction for avoidance [rad/s]
int16_t obstacle_free_confidence = 0; // a measure of how certain we are that the way ahead if safe.
const int16_t max_trajectory_confidence = 5; // number of consecutive negative object detections to be sure we are obstacle free
// This call back will be used to receive the color count from the orange detector
#ifndef ORANGE_AVOIDER_VISUAL_DETECTION_ID
#error This module requires two color filters, as such you have to define ORANGE_AVOIDER_VISUAL_DETECTION_ID to the orange filter
#error Please define ORANGE_AVOIDER_VISUAL_DETECTION_ID to be COLOR_OBJECT_DETECTION1_ID or COLOR_OBJECT_DETECTION2_ID in your airframe
#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;
}
#ifndef FLOOR_VISUAL_DETECTION_ID
#error This module requires two color filters, as such you have to define FLOOR_VISUAL_DETECTION_ID to the orange filter
#error Please define FLOOR_VISUAL_DETECTION_ID to be COLOR_OBJECT_DETECTION1_ID or COLOR_OBJECT_DETECTION2_ID in your airframe
#endif
static abi_event floor_detection_ev;
static void floor_detection_cb(uint8_t __attribute__((unused)) sender_id,
int16_t __attribute__((unused)) pixel_x, int16_t pixel_y,
int16_t __attribute__((unused)) pixel_width, int16_t __attribute__((unused)) pixel_height,
int32_t quality, int16_t __attribute__((unused)) extra)
{
floor_count = quality;
floor_centroid = pixel_y;
}
/*
* Initialisation function
*/
void orange_avoider_guided_init(void)
{
// Initialise random values
srand(time(NULL));
chooseRandomIncrementAvoidance();
// bind our colorfilter callbacks to receive the color filter outputs
AbiBindMsgVISUAL_DETECTION(ORANGE_AVOIDER_VISUAL_DETECTION_ID, &color_detection_ev, color_detection_cb);
AbiBindMsgVISUAL_DETECTION(FLOOR_VISUAL_DETECTION_ID, &floor_detection_ev, floor_detection_cb);
}
/*
* Function that checks it is safe to move forwards, and then sets a forward velocity setpoint or changes the heading
*/
void orange_avoider_guided_periodic(void)
{
// Only run the mudule if we are in the correct flight mode
if (guidance_h.mode != GUIDANCE_H_MODE_GUIDED) {
navigation_state = SEARCH_FOR_SAFE_HEADING;
obstacle_free_confidence = 0;
return;
}
// compute current color thresholds
int32_t color_count_threshold = oag_color_count_frac * front_camera.output_size.w * front_camera.output_size.h;
int32_t floor_count_threshold = oag_floor_count_frac * front_camera.output_size.w * front_camera.output_size.h;
float floor_centroid_frac = floor_centroid / (float)front_camera.output_size.h / 2.f;
VERBOSE_PRINT("Color_count: %d threshold: %d state: %d \n", color_count, color_count_threshold, navigation_state);
VERBOSE_PRINT("Floor count: %d, threshold: %d\n", floor_count, floor_count_threshold);
VERBOSE_PRINT("Floor centroid: %f\n", floor_centroid_frac);
// 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);
float speed_sp = fminf(oag_max_speed, 0.2f * obstacle_free_confidence);
switch (navigation_state){
case SAFE:
if (floor_count < floor_count_threshold || fabsf(floor_centroid_frac) > 0.12){
navigation_state = OUT_OF_BOUNDS;
} else if (obstacle_free_confidence == 0){
navigation_state = OBSTACLE_FOUND;
} else {
guidance_h_set_guided_body_vel(speed_sp, 0);
}
break;
case OBSTACLE_FOUND:
// stop
guidance_h_set_guided_body_vel(0, 0);
// randomly select new search direction
chooseRandomIncrementAvoidance();
navigation_state = SEARCH_FOR_SAFE_HEADING;
break;
case SEARCH_FOR_SAFE_HEADING:
guidance_h_set_guided_heading_rate(avoidance_heading_direction * oag_heading_rate);
// make sure we have a couple of good readings before declaring the way safe
if (obstacle_free_confidence >= 2){
guidance_h_set_guided_heading(stateGetNedToBodyEulers_f()->psi);
navigation_state = SAFE;
}
break;
case OUT_OF_BOUNDS:
// stop
guidance_h_set_guided_body_vel(0, 0);
// start turn back into arena
guidance_h_set_guided_heading_rate(avoidance_heading_direction * RadOfDeg(15));
navigation_state = REENTER_ARENA;
break;
case REENTER_ARENA:
// force floor center to opposite side of turn to head back into arena
if (floor_count >= floor_count_threshold && avoidance_heading_direction * floor_centroid_frac >= 0.f){
// return to heading mode
guidance_h_set_guided_heading(stateGetNedToBodyEulers_f()->psi);
// reset safe counter
obstacle_free_confidence = 0;
// ensure direction is safe before continuing
navigation_state = SAFE;
}
break;
default:
break;
}
return;
}
/*
* Sets the variable 'incrementForAvoidance' randomly positive/negative
*/
uint8_t chooseRandomIncrementAvoidance(void)
{
// Randomly choose CW or CCW avoiding direction
if (rand() % 2 == 0) {
avoidance_heading_direction = 1.f;
VERBOSE_PRINT("Set avoidance increment to: %f\n", avoidance_heading_direction * oag_heading_rate);
} else {
avoidance_heading_direction = -1.f;
VERBOSE_PRINT("Set avoidance increment to: %f\n", avoidance_heading_direction * oag_heading_rate);
}
return false;
}
sw/airborne/modules/orange_avoider/orange_avoider_guided.h
+41
View File
@@ -0,0 +1,41 @@
/*
* Copyright (C) Kirk Scheper <kirkscheper@gmail.com>
*
* This file is part of paparazzi
*
*/
/**
* @file "modules/orange_avoider/orange_avoider_guided.c"
* @author Kirk Scheper
* This module is an example module for the course AE4317 Autonomous Flight of Micro Air Vehicles at the TU Delft.
* This module is used in combination with a color filter (cv_detect_color_object) and the guided mode of the autopilot.
* The avoidance strategy is to simply count the total number of orange pixels. When above a certain percentage threshold,
* (given by color_count_frac) we assume that there is an obstacle and we turn.
*
* The color filter settings are set using the cv_detect_color_object. This module can run multiple filters simultaneously
* so you have to define which filter to use with the ORANGE_AVOIDER_VISUAL_DETECTION_ID setting.
* This module differs from the simpler orange_avoider.xml in that this is flown in guided mode. This flight mode is
* less dependent on a global positioning estimate as witht the navigation mode. This module can be used with a simple
* speed estimate rather than a global position.
*
* Here we also need to use our onboard sensors to stay inside of the cyberzoo and not collide with the nets. For this
* we employ a simple color detector, similar to the orange poles but for green to detect the floor. When the total amount
* of green drops below a given threshold (given by floor_count_frac) we assume we are near the edge of the zoo and turn
* around. The color detection is done by the cv_detect_color_object module, use the FLOOR_VISUAL_DETECTION_ID setting to
* define which filter to use.
*/
#ifndef ORANGE_AVOIDER_GUIDED_H
#define ORANGE_AVOIDER_GUIDED_H
// settings
extern float oag_color_count_frac; // obstacle detection threshold as a fraction of total of image
extern float oag_floor_count_frac; // floor detection threshold as a fraction of total of image
extern float oag_max_speed; // max flight speed [m/s]
extern float oag_heading_rate; // heading rate setpoint [rad/s]
extern void orange_avoider_guided_init(void);
extern void orange_avoider_guided_periodic(void);
#endif
sw/airborne/subsystems/abi_sender_ids.h
+11 -1
View File
@@ -429,11 +429,21 @@
#define RELATIVE_LOCALIZATION_ID 1
#endif
#ifndef DETECT_GATE_ABI_ID
#define DETECT_GATE_ABI_ID 33
#endif
/*
* VISUAL_DETECTION communication (message 27)
*/
#ifndef COLOR_OBJECT_DETECTION1_ID
#define COLOR_OBJECT_DETECTION1_ID 1
#endif
#ifndef COLOR_OBJECT_DETECTION2_ID
#define COLOR_OBJECT_DETECTION2_ID 2
#endif
/*
* JOYSTICK message (used for payload or control, but not as a RC)
*/
sw/airborne/subsystems/navigation/waypoints.c
+28
View File
@@ -147,6 +147,15 @@ void waypoint_set_xy_i(uint8_t wp_id, int32_t x, int32_t y)
}
}
void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y)
{
if (wp_id < nb_waypoint) {
waypoint_set_xy_i(wp_id, x, y);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &x, &y,
&(waypoints[wp_id].enu_i.z));
}
}
void waypoint_set_alt_i(uint8_t wp_id, int32_t alt)
{
if (wp_id < nb_waypoint) {
@@ -234,6 +243,25 @@ void waypoint_set_here_2d(uint8_t wp_id)
}
}
void waypoint_move_here_2d(uint8_t wp_id)
{
if (wp_id >= nb_waypoint) {
return;
}
if (waypoint_is_global(wp_id)) {
/* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */
struct LlaCoor_i *lla = &(waypoints[wp_id].lla);
int32_t hmsl = lla->alt - state.ned_origin_i.lla.alt + state.ned_origin_i.hmsl;
DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id,
&lla->lat, &lla->lon, &hmsl);
} else {
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
&waypoints[wp_id].enu_i.x,
&waypoints[wp_id].enu_i.y,
&waypoints[wp_id].enu_i.z);
}
}
void waypoint_globalize(uint8_t wp_id)
{
if (state.ned_initialized_i) {
sw/airborne/subsystems/navigation/waypoints.h
+3 -3
View File
@@ -108,11 +108,11 @@ extern void waypoint_position_copy(uint8_t wp_dest, uint8_t wp_src);
/*
* Move waypoints.
* Basically sets the coordinats and sends the WP_MOVED telemetry message as ack.
* @todo keep this here?
* Basically sets the coordinates and sends the WP_MOVED telemetry message as ack.
*/
extern void waypoint_move_here_2d(uint8_t wp_id);
extern void waypoint_move_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos);
extern void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y);
extern void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla);
sw/ext/opencv_bebop
+1 -1
Submodule sw/ext/opencv_bebop updated: 4c08e44fdd...b2264ae3ee
sw/ext/tudelft_gazebo_models
+1 -1
Submodule sw/ext/tudelft_gazebo_models updated: 53bb8f82d2...1606051925
sw/tools/opti_dist/dist.py
Executable
+95
View File
@@ -0,0 +1,95 @@
#!/usr/bin/env python3
import time
import sys
import os
import math
import argparse
import matplotlib.pyplot as plt
# if PAPARAZZI_HOME not set, then assume the tree containing this
# file is a reasonable substitute
PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../../')))
sys.path.append(PPRZ_HOME + "/sw/ground_segment/python/natnet3.x")
from NatNetClient import NatNetClient
# This is a callback function that gets connected to the NatNet client. It is called once per rigid body per frame
def receiveRigidBodyFrame(id, position, rotation):
# print( "Received frame for rigid body", id )
global pos_x, pos_y, pos_z
global track_id
if track_id and id != track_id:
return
pos_x = position[0]
pos_y = position[1]
pos_z = position[2]
def main(args):
global track_id
track_id = args.id
global pos_x, pos_y, pos_z
pos_x, pos_y, pos_z = 0.0, 0.0, 0.0
fig = plt.figure()
plt.axis([-6, 6, -6, 6])
# This will create a new NatNet client
streamingClient = NatNetClient(
server=args.server,
multicast=args.multicast,
commandPort=args.commandPort,
dataPort=args.dataPort,
rigidBodyListListener=receiveRigidBodyFrame)
# Start up the streaming client now that the callbacks are set up.
# This will run perpetually, and operate on a separate thread.
streamingClient.run()
time.sleep(2)
print('Start tracking')
if args.outputfile:
file = open(args.outputfile, 'w')
file.write('timestamp, x, y, z\n')
old_z = pos_z
old_x = pos_x
distance = 0
start_time = time.time()
pre_time = time.time()
while plt.fignum_exists(fig.number):
if args.outputfile:
data = '{}, {}, {}, {}\n'.format(int((time.time() - start_time) * 1000), pos_x, pos_y, pos_z)
file.write(data)
h = math.hypot(pos_z - old_z, pos_x - old_x)
if h > 0.20:
distance += h
old_z = pos_z
old_x = pos_x
if time.time() - pre_time > 0.5:
print("distance:%3.4f m; time_step:%d" % (distance, int((time.time() - start_time) * 2)))
pre_time = time.time()
plt.plot(pos_z, pos_x, 'ro')
plt.draw()
plt.pause(0.001)
time.sleep(0.01)
streamingClient.stop()
if args.outputfile:
file.close()
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--server', default="169.254.201.120")
parser.add_argument('--multicast', default="239.255.42.99")
parser.add_argument('--commandPort', type=int, default=1510)
parser.add_argument('--dataPort', type=int, default=1511)
parser.add_argument('--id', type=int, default=None)
parser.add_argument('--outputfile', type=str, default=None)
args = parser.parse_args()
main(args)