From 7e0491346587f9cb63754a915d5645befe8656b8 Mon Sep 17 00:00:00 2001
From: roangel <roangel@student.ethz.ch>
Date: Tue, 29 Aug 2017 16:44:37 +0200
Subject: [PATCH] Timer based approach
---
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 36 +++++++++++++------------
1 file changed, 19 insertions(+), 17 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index d25bda53..0d3fcdda 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -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)
{
--
GitLab