Skip to content

Commit

Permalink
Merge pull request paparazzi#29 from knmcguire/imav2016_flightplan_na…
Browse files Browse the repository at this point in the history 
…vigation

[flight plan] added total navigation with forcefield disable through …
  • Loading branch information
@knmcguire
knmcguire committed Oct 19, 2016
2 parents e04da85 + 5fbf30f commit 7ef2f0c
Show file tree
Hide file tree
Showing 5 changed files with 93 additions and 77 deletions.
148 changes: 71 additions & 77 deletions conf/flight_plans/TUDELFT/tudelft_bebop_GUIDED_IMAV2016.xml
View file
Expand Up @@ -51,66 +51,31 @@
<call_once fun="front_cam_set_x_offset(896)"/>
<call_once fun="detector.front_white_building->active = true"/>
<call_once fun="object_state = 0"/>
<call fun="go_to_object(false)"/>
<!-- TODO -->
</block>

<block name="Search entrance">
<!-- TODO -->
</block>

<block name="Approach entrance">
<!-- TODO -->
<set var="max_pixel_building" value="1000"/> <!-- max amount of pixels that the building need to exceed (MAKE A SETTING FROM THIS)-->
<set var="approach_white_building" value="true"/>
<call fun="go_to_object(false)"/> <!-- go to the building until a pixel count is reached (CALIBRATE THIS!!)-->
<set var="approach_white_building" value="false"/>
<!-- TODO: now it goes untill it sees the bottom camera... how to stop this? -->
</block>

<block name="Fly through entrance">
<call_once fun="detector_disable_all()"/>
<call_once fun="front_cam_set_x_offset(896)"/>
<call_once fun="win_state = 0"/>
<set var="disable_sideways_forcefield" value="true"/>
<call fun="fly_through_window(SNAKE_WINDOW)"/>
<set var="disable_sideways_forcefield" value="false"/>
</block>

<block name="Find and land at red item">
<call_once fun="front_cam_set_x_offset(512)"/>
</block>

<block name="Find item">
<call_once fun="detector_disable_all()"/>
<call_once fun="detector.front_red_item->active = true"/>
<call_once fun="detector.bottom_red_item->active = true"/>
<call_once fun="object_state = 0"/>
<call fun="go_to_object(false)"/>
</block>

<!--<block name="Search item A">-->
<!--<call_once fun="front_cam_set_x_offset(512)"/>-->
<!--<call_once fun="detector_disable_all()"/>-->
<!--<call_once fun="detector.item_bottom->active = true"/>-->
<!--<call_once fun="detector.item_front->active = true"/>-->
<!--<call fun="Hover(NOM_FLIGHT_ALT)"/>-->
<!--<call fun="front_marker_heading_change()"/>-->
<!--</block>-->

<!--<block name="Approach item A">-->
<!--<call fun="Hover(NOM_FLIGHT_ALT)"/>-->
<!--<call fun="front_marker_approach()"/>-->
<!--<exception cond="marker_lost == true" deroute="Search item A"/>-->
<!--<exception cond="marker1.found_time > 1" deroute="Center item A"/>-->
<!--</block>-->

<!--<block name="Center item A">-->
<!--<call fun="Hover(NOM_FLIGHT_ALT)"/>-->
<!--<call fun="marker_center_land(0.1, 0, 0)"/>-->
<!--<exception cond="1 > marker1.found_time" deroute="Search item A"/>-->
<!--<exception cond="marker1.found_time > 4" deroute="Land item A"/>-->
<!--</block>-->

<!--<block name="Land item A">-->
<!--<call fun="marker_center_land(0.05, 0.4, 0.5)"/>-->
<!--<call_once fun="KillEngines()"/>-->
<!--&lt;!&ndash; TODO: exception conditions &ndash;&gt;-->
<!--</block>-->

<block name="Grab red item">
<call_once fun="close_gripper()"/>
<while cond="2 > block_time"/>
Expand All @@ -122,11 +87,14 @@
</block>


<block name="Turn to exit room 1">
<block name="Find exit room 1">
<!-- TODO: determine in what heading the door is and turn to it-->
<!-- TODO: center to middle of door and go through it-->
<!-- use either range sensors for this or gate detection-->
<!-- TODO: Move for an aditional 1 meter/-->
<call_once fun="detector_disable_all()"/>
<call_once fun="front_cam_set_x_offset(896)"/>
<call_once fun="win_state = 0"/>
<set var="disable_sideways_forcefield" value="true"/>
<call fun="fly_through_window(SNAKE_DOOR)"/>
<set var="disable_sideways_forcefield" value="false"/>
</block>

<block name="Navigate to room 2">
Expand All @@ -143,7 +111,9 @@
<call_once fun="detector_disable_all()"/>
<call_once fun="front_cam_set_x_offset(896)"/>
<call_once fun="win_state = 0"/>
<set var="disable_sideways_forcefield" value="true"/>
<call fun="fly_through_window(SNAKE_DOOR)"/>
<set var="disable_sideways_forcefield" value="false"/>
</block>

<block name="Find and hover above red bucket">
Expand All @@ -154,49 +124,37 @@
<call fun="go_to_object(false)"/>
</block>

<!--<block name="Search bucket A">-->
<!--<call_once fun="front_cam_set_x_offset(512)"/>-->
<!--<call_once fun="detector_disable_all()"/>-->
<!--<call_once fun="detector.bucket_bottom->active = true"/>-->
<!--<call_once fun="detector.bucket_front->active = true"/>-->
<!--<call fun="Hover(NOM_FLIGHT_ALT)"/>-->
<!--<call fun="front_marker_heading_change()"/>-->
<!--</block>-->

<!--<block name="Approach bucket A">-->
<!--<call fun="Hover(NOM_FLIGHT_ALT)"/>-->
<!--<call fun="front_marker_approach()"/>-->
<!--<exception cond="marker_lost == true" deroute="Search bucket A"/>-->
<!--</block>-->

<!--<block name="Center bucket A">-->
<!--<call fun="Hover(NOM_FLIGHT_ALT)"/>-->
<!--<call fun="marker_center_land(0.05, 0, 0)"/>-->
<!--<exception cond="0.1 > marker1.found_time" deroute="Search bucket A"/>-->
<!--</block>-->

<block name="Release red item">
<call_once fun="open_gripper()"/>
</block>

<block name="Turn to exit room 2">
<block name="Find exit room 2">
<!-- TODO: determine in what heading the door is and turn to it-->
<!-- TODO: center to middle of door and go through it-->
<!-- use either range sensors for this or gate detection-->
<!-- TODO: Move for an aditional 1 meter/-->
<call_once fun="detector_disable_all()"/>
<call_once fun="front_cam_set_x_offset(896)"/>
<call_once fun="win_state = 0"/>
<set var="disable_sideways_forcefield" value="true"/>
<call fun="fly_through_window(SNAKE_DOOR)"/>
<set var="disable_sideways_forcefield" value="false"/>
</block>

<block name="Navigate to room 3">
<!-- TODO: turn 180 degrees (face towards wall) and move LEFT for a meter-->
<!-- TODO: wall following for 2 meters untill door is found with gate detection-->
<!-- or wall following for constant velocity untill door is met-->
<call_once fun="guidance_h_set_guided_heading_relative(DegOfRad(180.0))"/>
<call_once fun="MoveRight(-0.2)"/>
<while cond = "!front_wall_detected"/> <!--wait till we see the wall-->
<set var="do_wall_following" value= "true"/> <!--follow wall at fixed distance-->
<while cond = "front_wall_detected"/> <!--wait till we see the next door-->
<set var="do_wall_following" value= "false"/>
<call_once fun="MoveRight(0.0)"/>
</block>

<block name="Move through door 3">
<call_once fun="detector_disable_all()"/>
<call_once fun="front_cam_set_x_offset(896)"/>
<call_once fun="win_state = 0"/>
<set var="disable_sideways_forcefield" value="true"/>
<call fun="fly_through_window(SNAKE_DOOR)"/>
<set var="disable_sideways_forcefield" value="false"/>
</block>

<block name="Find and land at blue item">
Expand All @@ -216,6 +174,36 @@
<!-- TODO -->
</block>

<block name="Find exit room second time">
<!-- TODO: determine in what heading the door is and turn to it-->
<call_once fun="detector_disable_all()"/>
<call_once fun="front_cam_set_x_offset(896)"/>
<call_once fun="win_state = 0"/>
<set var="disable_sideways_forcefield" value="true"/>
<call fun="fly_through_window(SNAKE_DOOR)"/>
<set var="disable_sideways_forcefield" value="false"/>
</block>

<block name="Navigate to room 2 second time">
<call_once fun="guidance_h_set_guided_heading_relative(DegOfRad(180.0))"/>
<call_once fun="MoveRight(0.2)"/>
<while cond = "!front_wall_detected"/> <!--wait till we see the wall-->
<set var="do_wall_following" value= "true"/> <!--follow wall at fixed distance-->
<while cond = "front_wall_detected"/> <!--wait till we see the next door-->
<set var="do_wall_following" value= "false"/>
<call_once fun="MoveRight(0.0)"/>
</block>

<block name="Move through door 2 second time">
<call_once fun="detector_disable_all()"/>
<call_once fun="front_cam_set_x_offset(896)"/>
<call_once fun="win_state = 0"/>
<set var="disable_sideways_forcefield" value="true"/>
<call fun="fly_through_window(SNAKE_DOOR)"/>
<set var="disable_sideways_forcefield" value="false"/>
</block>


<block name="Find and hover above blue bucket">
<call_once fun="detector_disable_all()"/>
<call_once fun="front_cam_set_x_offset(512)"/>
Expand All @@ -228,12 +216,18 @@
<!-- TODO -->
</block>

<block name="Search exit">
<!-- TODO -->
<block name="Find exit room 3">
<!-- TODO: determine in what heading the door is and turn to it-->
<call_once fun="detector_disable_all()"/>
<call_once fun="front_cam_set_x_offset(896)"/>
<call_once fun="win_state = 0"/>
<set var="disable_sideways_forcefield" value="true"/>
<call fun="fly_through_window(SNAKE_DOOR)"/>
<set var="disable_sideways_forcefield" value="false"/>
</block>

<block name="Approach exit">
<!-- TODO -->
<call_once fun="guidance_h_set_guided_heading_relative(DegOfRad(-45.0))"/> <!-- change heading to face the door approximatly -->
</block>

<block name="Fly through exit">
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/computer_vision/marker/detector.c
View file
Expand Up @@ -73,6 +73,7 @@ static struct Marker single_blob_finder(struct image_t *img, struct image_filter
fprintf(stderr, "[detector %i] largest blob size %i.\n", img->w, largest_size);

struct Marker marker;
marker.pixel_cnt = labels[largest_id].pixel_cnt;

if (largest_id >= 0 && largest_size > threshold) {
marker.pixel.x = labels[largest_id].x_sum / labels[largest_id].pixel_cnt * 2;
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/computer_vision/marker/detector.h
View file
Expand Up @@ -54,6 +54,7 @@ struct Marker {
struct point_t pixel;
struct NedCoor_f geo_location;
struct FloatVect3 geo_relative;
uint32_t pixel_cnt;
float found_time;
};

Expand Down
16 changes: 16 additions & 0 deletions sw/airborne/modules/flight_plan_guided/flight_plan_guided.c
View file
Expand Up @@ -52,12 +52,15 @@

float marker_err = 0;
bool marker_lost;
bool approach_white_building = false;
uint32_t max_pixel_building = 1000; //TODO: CALIBRATE THIS!

#include "subsystems/abi.h"
struct range_finders_ range_finders;
bool do_lr_avoidance = true;
bool do_wall_following = false;
bool front_wall_detected = false;
bool disable_sideways_forcefield = false;

#ifndef RANGE_SENSORS_ABI_ID
#define RANGE_SENSORS_ABI_ID ABI_BROADCAST
Expand Down Expand Up @@ -375,6 +378,15 @@ bool go_to_object(bool descent) {
break;
}

if(approach_white_building)
{
if(marker1.pixel_cnt > max_pixel_building) //TODO: calibrate this
{
return false; // if returned false it will stop the approach

}
}

object_state++; // Go to next state + switch fallthrough
case 3:
// Hover over marker
Expand Down Expand Up @@ -591,6 +603,10 @@ static void range_sensors_cb(uint8_t sender_id,

range_sensor_force_field(&vel_offset_body_x, &vel_offset_body_y, 500, 1000, 1600, 0.0f, 0.3f);

if(disable_sideways_forcefield) // disable forcefield for the side if the drone is going through a door for instance
{
vel_offset_body_y = 0.0f;
}
// calculate velocity offset for guidance
guidance_h_set_speed_offset(vel_offset_body_x, vel_offset_body_y);
}
4 changes: 4 additions & 0 deletions sw/airborne/modules/flight_plan_guided/flight_plan_guided.h
View file
Expand Up @@ -37,6 +37,9 @@ struct range_finders_ {

extern float marker_err;
extern bool marker_lost;
extern bool approach_white_building;
extern uint32_t max_pixel_building;

extern struct range_finders_ range_finders;

// Module functions
Expand Down Expand Up @@ -76,6 +79,7 @@ extern bool front_cam_set_x_offset(int offset);

extern bool front_wall_detected;
extern bool do_wall_following;
extern bool disable_sideways_forcefield;
extern bool range_sensor_wall_following_between_doors(float travel_time);

#endif

0 comments on commit 7ef2f0c

Please sign in to comment.