diff --git a/conf/airframes/tudelft/bebop_orangeavoid_course2017.xml b/conf/airframes/tudelft/bebop_orangeavoid_course2017.xml
new file mode 100644
index 0000000000..0e0a302f07
--- /dev/null
+++ b/conf/airframes/tudelft/bebop_orangeavoid_course2017.xml
@@ -0,0 +1,253 @@
+
+
+
+ Vision Course TUDelft V2017
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/tudelft/course2016_avoid_orange_cyberzoo.xml b/conf/flight_plans/tudelft/course2016_avoid_orange_cyberzoo.xml
deleted file mode 100644
index 2122747990..0000000000
--- a/conf/flight_plans/tudelft/course2016_avoid_orange_cyberzoo.xml
+++ /dev/null
@@ -1,124 +0,0 @@
-
-
-
-
-#include "autopilot.h"
-#include "subsystems/ahrs.h"
-#include "subsystems/electrical.h"
-#include "subsystems/datalink/datalink.h"
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/flight_plans/tudelft/course2017_avoid_orange_cyberzoo.xml b/conf/flight_plans/tudelft/course2017_avoid_orange_cyberzoo.xml
new file mode 100644
index 0000000000..73ba8f322c
--- /dev/null
+++ b/conf/flight_plans/tudelft/course2017_avoid_orange_cyberzoo.xml
@@ -0,0 +1,144 @@
+
+
+
+
+ #include "subsystems/datalink/datalink.h"
+ #include "subsystems/electrical.h"
+ #include "subsystems/radio_control.h"
+ #include "subsystems/ahrs.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/orange_avoider.xml b/conf/modules/orange_avoider.xml
index 1da1a70016..ac88015f03 100644
--- a/conf/modules/orange_avoider.xml
+++ b/conf/modules/orange_avoider.xml
@@ -5,7 +5,7 @@
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/course2016_avoid_orange_cyberzoo.xml you will avoid every obstacle that is orange.
+ 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.
cv_colorfilter.xml
diff --git a/conf/userconf/tudelft/course2017_conf.xml b/conf/userconf/tudelft/course2017_conf.xml
new file mode 100644
index 0000000000..e3cdce7994
--- /dev/null
+++ b/conf/userconf/tudelft/course2017_conf.xml
@@ -0,0 +1,14 @@
+
+
+
diff --git a/conf/userconf/tudelft/course2017_control_panel.xml b/conf/userconf/tudelft/course2017_control_panel.xml
new file mode 100644
index 0000000000..666d1cbcdd
--- /dev/null
+++ b/conf/userconf/tudelft/course2017_control_panel.xml
@@ -0,0 +1,63 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/orange_avoider/orange_avoider.c b/sw/airborne/modules/orange_avoider/orange_avoider.c
index f46928f18f..8cd9eaf771 100644
--- a/sw/airborne/modules/orange_avoider/orange_avoider.c
+++ b/sw/airborne/modules/orange_avoider/orange_avoider.c
@@ -13,76 +13,143 @@
#include "modules/orange_avoider/orange_avoider.h"
#include "modules/computer_vision/colorfilter.h"
#include "firmwares/rotorcraft/navigation.h"
+#include "generated/flight_plan.h"
#include "state.h"
#include
+#include
#include
#include
-uint8_t safeToGoForwards = false;
-int tresholdColorCount = 200;
-int32_t incrementForAvoidance;
+#define ORANGE_AVOIDER_VERBOSE TRUE
+
+#define PRINT(string,...) fprintf(stderr, "[orange_avoider->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
+#if ORANGE_AVOIDER_VERBOSE
+#define VERBOSE_PRINT PRINT
+#else
+#define VERBOSE_PRINT(...)
+#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;
+
+/*
+ * Initialisation function, setting the colour filter, random seed and incrementForAvoidance
+ */
void orange_avoider_init()
{
// Initialise the variables of the colorfilter to accept orange
- color_lum_min = 0;
- color_lum_max = 131;
- color_cb_min = 93;
- color_cb_max = 255;
- color_cr_min = 134;
- color_cr_max = 255;
+ color_lum_min = 20;
+ color_lum_max = 255;
+ color_cb_min = 75;
+ color_cb_max = 145;
+ color_cr_min = 167;
+ color_cr_max = 255;
// Initialise random values
srand(time(NULL));
chooseRandomIncrementAvoidance();
}
+
+/*
+ * Function that checks it is safe to move forwards, and then moves a waypoint forward or changes the heading
+ */
void orange_avoider_periodic()
{
// Check the amount of orange. If this is above a threshold
// you want to turn a certain amount of degrees
safeToGoForwards = color_count < tresholdColorCount;
- printf("Checking if this function is called %d threshold: %d now: %d \n", color_count, tresholdColorCount,
- safeToGoForwards);
+ 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;
+ } 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;
+ }
+ }
+ return;
}
-
-/**
+/*
* Increases the NAV heading. Assumes heading is an INT32_ANGLE. It is bound in this function.
*/
-uint8_t increase_nav_heading(int32_t *heading, int32_t increment)
+uint8_t increase_nav_heading(int32_t *heading, float incrementDegrees)
{
- *heading = *heading + increment;
+ 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(*heading); // HEADING HAS INT32_ANGLE_FRAC....
+ INT32_ANGLE_NORMALIZE(newHeading); // HEADING HAS INT32_ANGLE_FRAC....
+ *heading = newHeading;
+ VERBOSE_PRINT("Increasing heading to %f\n", DegOfRad(ANGLE_FLOAT_OF_BFP(*heading));
return false;
}
-uint8_t moveWaypointForwards(uint8_t waypoint, float distanceMeters)
+
+/*
+ * Calculates coordinates of a distance of 'distanceMeters' forward w.r.t. current position and heading
+ */
+static uint8_t calculateForwards(struct EnuCoor_i *new_coor, float distanceMeters)
+{
+ 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));
+ // 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));
+ 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));
+ return false;
+}
+
+/*
+ * Sets waypoint 'waypoint' to the coordinates of 'new_coor'
+ */
+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);
+ return false;
+}
+
+/*
+ * Calculates coordinates of distance forward and sets waypoint 'waypoint' to those coordinates
+ */
+uint8_t moveWaypointForward(uint8_t waypoint, float distanceMeters)
{
struct EnuCoor_i new_coor;
- struct EnuCoor_i *pos = stateGetPositionEnu_i(); // Get your current position
-
- // Calculate the sine and cosine of the heading the drone is keeping
- float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
- float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
-
- // 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.z = pos->z; // Keep the height the same
-
- // Set the waypoint to the calculated position
- waypoint_set_xy_i(waypoint, new_coor.x, new_coor.y);
-
+ calculateForwards(&new_coor, distanceMeters);
+ moveWaypoint(waypoint, &new_coor);
return false;
}
+/*
+ * Sets the variable 'incrementForAvoidance' randomly positive/negative
+ */
uint8_t chooseRandomIncrementAvoidance()
{
-
+ // Randomly choose CW or CCW avoiding direction
int r = rand() % 2;
if (r == 0) {
- incrementForAvoidance = 350;
+ incrementForAvoidance = 10.0;
+ VERBOSE_PRINT("Set avoidance increment to: %f\n", incrementForAvoidance);
} else {
- incrementForAvoidance = -350;
+ incrementForAvoidance = -10.0;
+ VERBOSE_PRINT("Set avoidance increment to: %f\n", incrementForAvoidance);
}
return false;
}
diff --git a/sw/airborne/modules/orange_avoider/orange_avoider.h b/sw/airborne/modules/orange_avoider/orange_avoider.h
index cc9f6faf9c..570a972027 100644
--- a/sw/airborne/modules/orange_avoider/orange_avoider.h
+++ b/sw/airborne/modules/orange_avoider/orange_avoider.h
@@ -13,13 +13,16 @@
#ifndef ORANGE_AVOIDER_H
#define ORANGE_AVOIDER_H
#include
+#include "math/pprz_geodetic_int.h"
extern uint8_t safeToGoForwards;
-extern int32_t incrementForAvoidance;
+extern float incrementForAvoidance;
+extern uint16_t trajectoryConfidence;
extern void orange_avoider_init(void);
extern void orange_avoider_periodic(void);
-extern uint8_t moveWaypointForwards(uint8_t waypoint, float distanceMeters);
-extern uint8_t increase_nav_heading(int32_t *heading, int32_t increment);
+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