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 26 Packages Projects Releases Wiki Activity

Gazebo in tudelft conf, with orange avoid example (#2149)

Browse Source 
* Gazebo Cyberzoo and OrangeAvoid in tudelft conf, Simulate when no RC, call gzclient as tool
* [orange_avoider] color setting

* docs+remove modules section
This commit is contained in:
kevindehecker
2017-11-09 19:45:26 +01:00
committed by Christophe De Wagter
parent 62bb3cafaa
commit 6fafcaaae9
14 changed files with 2065 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/ardrone2_gazebo_cyberzoo.xml
+220
View File
@@ -0,0 +1,220 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="ardrone2">
<firmware name="rotorcraft">
<target name="nps" board="pc">
<module name="fdm" type="gazebo"/>
</target>
<define name="USE_SONAR" value="TRUE"/>
<!-- Subsystem section -->
<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
<module name="motor_mixing"/>
<module name="actuators" type="ardrone2"/>
<module name="imu" type="ardrone2"/>
<!-- gps: "ublox" or change to "sirf" for usage with parrot flight recorder -->
<module name="gps" type="ublox"/>
<module name="stabilization" type="int_quat"/>
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins" type="extended"/>
<module name="bat_voltage_ardrone2"/>
<!-- remove the gps_ubx_ucenter module if you use the sirf gps (flight recorder) -->
<module name="gps" type="ubx_ucenter"/>
<module name="send_imu_mag_current"/>
<module name="air_data"/>
<module name="geo_mag"/>
<!--module name="logger_file"/-->
<module name="video_thread">
</module>
<module name="cv_colorfilter">
<define name="COLORFILTER_CAMERA" value="front_camera"/>
</module>
<module name="orange_avoider" />
<module name="video_rtp_stream">
<define name="VIEWVIDEO_CAMERA" value="front_camera"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="4"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="60"/>
</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="3000"/>
</commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/>
<servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/>
<servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/>
<servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/>
</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"/>
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
<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>
<!-- Gazebo Simulation values -->
<section name="COLORFILTER" prefix="ORANGE_AVOIDER_">
<define name="LUM_MIN" value="41"/>
<define name="LIM_MAX" value="183"/>
<define name="CB_MIN" value="53"/>
<define name="CB_MAX" value="121"/>
<define name="CR_MIN" value="134"/>
<define name="CR_MAX" value="249"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- Accelero -->
<define name="ACCEL_X_NEUTRAL" value="2048"/>
<define name="ACCEL_Y_NEUTRAL" value="2048"/>
<define name="ACCEL_Z_NEUTRAL" value="2048"/>
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="-180"/>
<define name="MAG_X_SENS" value="16." integer="16"/>
<define name="MAG_Y_SENS" value="16." integer="16"/>
<define name="MAG_Z_SENS" value="16." integer="16"/>
<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
<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"/>
</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 -->
<!-- Toulouse -->
<define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/>
<!-- Delft -->
<!--define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/ -->
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="600" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="200" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="300." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="850"/>
<define name="PHI_DGAIN" value="425"/>
<define name="PHI_IGAIN" value="0"/>
<define name="THETA_PGAIN" value="850"/>
<define name="THETA_DGAIN" value="425"/>
<define name="THETA_IGAIN" value="0"/>
<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="700"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="100"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="283"/>
<define name="HOVER_KD" value="82"/>
<define name="HOVER_KI" value="13"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="79"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="30"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="GAZEBO_WORLD" value='"ardrone_cyberzoo.world"'/>
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="double[]"/>
<define name="ACTUATOR_TORQUES" value="0.155, -0.155, 0.155, -0.155" type="double[]"/>
<define name="JSBSIM_MODEL" value="simple_ardrone2" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_ardrone2.h" type="string"/>
<define name="BYPASS_INS" value="1"/>
<define name="BYPASS_AHRS" value="1"/>
<define name="SIMULATE_VIDEO" value="1"/>
</section>
<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"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/airframes/tudelft/bebop_orangeavoid_course2017.xml
+12 -2
View File
@@ -45,9 +45,9 @@
</firmware>
<modules main_freq="512">
<module name="geo_mag"/>
<!--module name="geo_mag"/-->
<module name="air_data"/>
<module name="send_imu_mag_current"/>
<!--module name="send_imu_mag_current"/-->
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
</module>
@@ -105,6 +105,16 @@
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>
<!-- Cyberzoo bebop1 values -->
<section name="COLORFILTER" prefix="ORANGE_AVOIDER_">
<define name="LUM_MIN" value="20"/>
<define name="LIM_MAX" value="255"/>
<define name="CB_MIN" value="75"/>
<define name="CB_MAX" value="145"/>
<define name="CR_MIN" value="167"/>
<define name="CR_MAX" value="255"/>
</section>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
conf/flight_plans/tudelft/course2017_avoid_orange_cyberzoo.xml
+3 -3
View File
@@ -39,10 +39,10 @@
</sectors>
<exceptions>
<!-- RC lost -->
<exception cond="((radio_control.status == RC_REALLY_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"/>
(autopilot_in_flight() == true) )" deroute="Standby"/-->
<!-- Datalink lost (constant RPM descent) -->
<exception cond="((datalink_time > 2) &&
!(IndexOfBlock('Holding point') > nav_block) &&
@@ -57,7 +57,7 @@
<exception cond="((GetPosAlt() > 3.5) &&
!(IndexOfBlock('Holding point') > nav_block) &&
!(nav_block >= IndexOfBlock('Land here')) &&
(autopilot_in_flight == true) )" deroute="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) &&
conf/modules/orange_avoider.xml
+7 -1
View File
@@ -7,6 +7,12 @@
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.
</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"/>
</doc>
<depends>cv_colorfilter.xml</depends>
<header>
@@ -14,7 +20,7 @@
</header>
<init fun="orange_avoider_init()"/>
<periodic fun="orange_avoider_periodic()" freq="4"/>
<makefile target="ap">
<makefile >
<file name="orange_avoider.c"/>
</makefile>
</module>
conf/simulator/gazebo/models/cyberzoo/cyberzoo.sdf
+103
View File
@@ -0,0 +1,103 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="cyberzoo">
<static>true</static>
<pose>0 0 0 0 0 0</pose>
<link name="floor">
<pose>0 0 0.01 0 0 0</pose>
<visual name="visual">
<geometry>
<box>
<size>10 10 0.02</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0.2 0.8 0.2 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>10 10 0.02</size>
</box>
</geometry>
</collision>
</link>
<link name="north_wall">
<pose>0 5 5 0 0 0</pose>
<visual name="visual">
<geometry>
<box>
<size>10 0.2 10</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0 0 0 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>10 0.2 10</size>
</box>
</geometry>
</collision>
</link>
<link name="east_wall">
<pose>5 0 5 0 0 0</pose>
<visual name="visual">
<geometry>
<box>
<size>0.2 10 10</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0 0 0 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>0.2 10 10</size>
</box>
</geometry>
</collision>
</link>
<link name="south_wall">
<pose>0 -5 6.25 0 0 0</pose>
<visual name="visual">
<geometry>
<box>
<size>10 0.2 7.5</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0 0 0 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<collision name="collision">
<geometry>
<box>
<size>10 0.2 7.5</size>
</box>
</geometry>
</collision>
</link>
</model>
</sdf>
conf/simulator/gazebo/models/cyberzoo/model.config
+15
View File
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>Cyberzoo</name>
<version>1.0</version>
<sdf version='1.4'>cyberzoo.sdf</sdf>
<author>
<name>Tom van Dijk</name>
<email>tomvand@users.noreply.github.com</email>
</author>
<description>
Cyberzoo coarse model
</description>
</model>
conf/simulator/gazebo/models/orange_pole/model.config
+15
View File
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>Orange Pole</name>
<version>1.0</version>
<sdf version='1.4'>orange_pole.sdf</sdf>
<author>
<name>Tom van Dijk</name>
<email>tomvand@users.noreply.github.com</email>
</author>
<description>
Orange pole as used in the TU Delft Cyberzoo.
</description>
</model>
conf/simulator/gazebo/models/orange_pole/orange_pole.sdf
+43
View File
@@ -0,0 +1,43 @@
<?xml version='1.0'?>
<sdf version="1.4">
<model name="orange_pole">
<pose>0 0 1.0 0 0 0</pose>
<static>true</static>
<link name="link">
<inertial>
<mass>1.0</mass>
<inertia> <!-- interias are tricky to compute -->
<!-- http://answers.gazebosim.org/question/4372/the-inertia-matrix-explained/ -->
<ixx>0.083</ixx> <!-- for a box: ixx = 0.083 * mass * (y*y + z*z) -->
<ixy>0.0</ixy> <!-- for a box: ixy = 0 -->
<ixz>0.0</ixz> <!-- for a box: ixz = 0 -->
<iyy>0.083</iyy> <!-- for a box: iyy = 0.083 * mass * (x*x + z*z) -->
<iyz>0.0</iyz> <!-- for a box: iyz = 0 -->
<izz>0.083</izz> <!-- for a box: izz = 0.083 * mass * (x*x + y*y) -->
</inertia>
</inertial>
<collision name="collision">
<geometry>
<cylinder>
<radius>0.3</radius>
<length>2.0</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>0.2</radius>
<length>2.0</length>
</cylinder>
</geometry>
<material>
<ambient>1 0.55 0 1</ambient>
<diffuse>1 0.55 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
</sdf>
conf/simulator/gazebo/world/ardrone_cyberzoo.world
+1588
View File
File diff suppressed because it is too large Load Diff
conf/userconf/tudelft/conf.xml
+11
View File
@@ -429,6 +429,17 @@
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml modules/radio_control_superbitrf_rc.xml modules/gps_ubx_ucenter.xml"
gui_color="green"
/>
<aircraft
name="ardrone2_flightplan_guided_gazebo"
ac_id="235"
airframe="airframes/examples/ardrone2_gazebo.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_guided_flightplan.xml"
settings="settings/rotorcraft_basic.xml settings/nps.xml"
settings_modules="modules/video_rtp_stream.xml modules/cv_colorfilter.xml [modules/geo_mag.xml] modules/air_data.xml modules/gps_ubx_ucenter.xml [modules/ins_extended.xml] modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="red"
/>
<aircraft
name="autonomous_race"
ac_id="8"
conf/userconf/tudelft/course2017_conf.xml
+14 -2
View File
@@ -1,14 +1,26 @@
<conf>
<aircraft
name="bebop_avoid"
name="bebop_orange_avoid"
ac_id="42"
airframe="airframes/tudelft/bebop_orangeavoid_course2017.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/course2017_avoid_orange_cyberzoo.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/video_rtp_stream.xml modules/cv_colorfilter.xml modules/air_data.xml modules/geo_mag.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
settings_modules="modules/video_rtp_stream.xml modules/cv_colorfilter.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="b740eb6e6f3843316778ffe3c7582ce0631e5411"
/>
<aircraft
name="simulate_orange_avoid"
ac_id="232"
airframe="airframes/examples/ardrone2_gazebo_cyberzoo.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/course2017_avoid_orange_cyberzoo.xml"
settings="settings/rotorcraft_basic.xml settings/nps.xml"
settings_modules="modules/video_rtp_stream.xml modules/cv_colorfilter.xml modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="red"
release="ea9d5a724642442f92503098a32e29a9d90b3385"
/>
</conf>
conf/userconf/tudelft/course2017_control_panel.xml
+1
View File
@@ -20,6 +20,7 @@
</program>
<program name="Environment Simulator" command="sw/simulator/gaia"/>
<program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
<program name="Gazebo" command="gzclient"/>
</section>
<section name="sessions">
<session name="Bebop basic">
sw/airborne/modules/computer_vision/lib/vision/image.c
+1 -1
View File
@@ -620,7 +620,7 @@ void image_gradient_pixel(struct image_t *img, struct point_t *loc, int method,
uint8_t add_ind = pixel_width - 1;
// check if all pixels will fall in the image:
if(loc->x >= 1 && loc->x < img->w-1 && loc->y >= 1 && loc->y < img->h - 1) {
if(loc->x >= 1 && (loc->x + 1) < img->w && loc->y >= 1 && (loc->y + 1) < img->h ) {
if(method == 0) {
// *************
sw/airborne/modules/orange_avoider/orange_avoider.c
+32 -6
View File
@@ -14,6 +14,7 @@
#include "modules/computer_vision/colorfilter.h"
#include "firmwares/rotorcraft/navigation.h"
#include "generated/flight_plan.h"
#include "generated/airframe.h"
#include "state.h"
#include <time.h>
#include <math.h>
@@ -30,6 +31,31 @@
#define VERBOSE_PRINT(...)
#endif
#ifndef ORANGE_AVOIDER_LUM_MIN
#define ORANGE_AVOIDER_LUM_MIN 41
#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;
@@ -42,12 +68,12 @@ float maxDistance = 2.25;
void orange_avoider_init()
{
// Initialise the variables of the colorfilter to accept orange
color_lum_min = 20;
color_lum_max = 255;
color_cb_min = 75;
color_cb_max = 145;
color_cr_min = 167;
color_cr_max = 255;
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();