Commit 7e049134 authored by roangel's avatar roangel
Browse files

Timer based approach

parent 9ca1f746
......@@ -58,8 +58,8 @@
// parameters for take off and landing. Eventually will go in YAML file
#define TAKE_OFF_OFFSET 1 //in meters
#define LANDING_DISTANCE 0.15 //in meters
#define DURATION_TAKE_OFF 300 //100 is 1 second
#define DURATION_LANDING 300 //100 is 1 second
#define DURATION_TAKE_OFF 3 // seconds
#define DURATION_LANDING 3 // seconds
#define PI 3.141592653589
......@@ -250,10 +250,22 @@ void changeFlyingStateTo(int new_state)
flyingStatePublisher.publish(flying_state_msg);
}
int counter = 0;
bool finished_take_off = false;
bool finished_land = false;
ros::Timer timer_takeoff;
ros::Timer timer_land;
void takeOffTimerCallback(const ros::TimerEvent&)
{
finished_take_off = true;
}
void landTimerCallback(const ros::TimerEvent&)
{
finished_land = true;
}
//is called when new data from Vicon arrives
void viconCallback(const ViconData& viconData) {
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
......@@ -266,6 +278,8 @@ void viconCallback(const ViconData& viconData) {
coordinatesToLocal(local);
controllerCall.request.ownCrazyflie = local;
ros::NodeHandle nodeHandle("~");
switch(flying_state) //things here repeat every X ms, non-blocking stuff
{
case STATE_MOTORS_OFF: // here we will put the code of current disabled button
......@@ -280,15 +294,9 @@ void viconCallback(const ViconData& viconData) {
{
changed_state_flag = false;
takeOffCF(local);
counter = 0;
finished_take_off = false;
ROS_INFO("STATE_TAKE_OFF");
}
counter++;
if(counter >= DURATION_TAKE_OFF)
{
counter = 0;
finished_take_off = true;
timer_takeoff = nodeHandle.createTimer(ros::Duration(DURATION_TAKE_OFF), takeOffTimerCallback, true);
}
if(finished_take_off)
{
......@@ -310,15 +318,9 @@ void viconCallback(const ViconData& viconData) {
{
changed_state_flag = false;
landCF(local);
counter = 0;
finished_land = false;
ROS_INFO("STATE_LAND");
}
counter++;
if(counter >= DURATION_LANDING)
{
counter = 0;
finished_land = true;
timer_takeoff = nodeHandle.createTimer(ros::Duration(DURATION_LANDING), landTimerCallback, true);
}
if(finished_land)
{
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment