Statistics
| Branch: | Revision:

root / quad2 / altitude_node / src / altitude_node.cpp @ 02975ec6

History | View | Annotate | Download (2.9 KB)

1
#include "ros/ros.h"
2
#include "mikrokopter/FcDebug.h"
3
#include "std_msgs/Float64.h"
4
#include "std_msgs/Bool.h"
5
#include "../../../control/pid_control.h"
6

    
7
class AltitudeNode
8
{
9
public:
10
  AltitudeNode();
11
  void main_loop() {ros::spin();}
12
  void height_cb(const mikrokopter::FcDebug::ConstPtr& fc);
13
  void goal_cb(const std_msgs::Float64::ConstPtr& goal);
14
  void p_cb(const std_msgs::Float64::ConstPtr& p);
15
  void i_cb(const std_msgs::Float64::ConstPtr& i);
16
  void d_cb(const std_msgs::Float64::ConstPtr& d);
17
  void enable_cb(const std_msgs::Bool::ConstPtr& goal);
18

    
19
private:
20
  bool enabled;
21
  float last_height;
22
  PID_control pid;
23
  ros::NodeHandle n;
24
  ros::Publisher pub;
25
  ros::Subscriber height_sub;
26
  ros::Subscriber goal_sub;
27
  ros::Subscriber p_sub;
28
  ros::Subscriber i_sub;
29
  ros::Subscriber d_sub;
30
  ros::Subscriber enable_sub;
31
};
32

    
33
AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false), last_height(0)
34
{
35
  height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100,
36
      &AltitudeNode::height_cb, this);
37
  goal_sub = n.subscribe<std_msgs::Float64>("/altitude_node/height_goal", 100,
38
      &AltitudeNode::goal_cb, this);
39
  p_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/p", 100,
40
      &AltitudeNode::p_cb, this);
41
  i_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/i", 100,
42
      &AltitudeNode::i_cb, this);
43
  d_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/d", 100,
44
      &AltitudeNode::d_cb, this);
45
  enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable", 100,
46
      &AltitudeNode::enable_cb, this);
47
  pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);
48
  
49
  /*double default_goal;
50
  ros::param::param<double>("~default_goal", default_goal, 0);
51
  pid.change_goal(default_goal);*/
52
}
53

    
54
void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc)
55
{
56
  last_height = fc->heightValue;
57
  if (enabled)
58
  {
59
    std_msgs::Float64 msg;
60
    msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec());
61
    pub.publish(msg);
62
  }
63
}
64

    
65
void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal)
66
{
67
  pid.change_goal(goal->data);
68
  ROS_INFO("Goal: %f", goal->data);
69
}
70

    
71
void AltitudeNode::p_cb(const std_msgs::Float64::ConstPtr& p)
72
{
73
  pid.change_p(p->data);
74
  ROS_INFO("K_p: %f", p->data);
75
}
76

    
77
void AltitudeNode::i_cb(const std_msgs::Float64::ConstPtr& i)
78
{
79
  pid.change_i(i->data);
80
  ROS_INFO("K_i: %f", i->data);
81
}
82

    
83
void AltitudeNode::d_cb(const std_msgs::Float64::ConstPtr& d)
84
{
85
  pid.change_d(d->data);
86
  ROS_INFO("K_d: %f", d->data);
87
}
88

    
89
void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg)
90
{
91
  enabled = msg->data;
92
  if (enabled)
93
  {
94
    pid.change_goal(last_height);
95
    ROS_INFO("Altitude Hold: Enabled.");
96
    ROS_INFO("Altitude: %f", last_height);
97
  }
98
  else
99
  {
100
    ROS_INFO("Altitude Hold: Disabled.");
101
  }
102
}
103

    
104
int main(int argc, char **argv) {
105
  ros::init(argc, argv, "altitude_node");
106
  AltitudeNode an = AltitudeNode();
107
  an.main_loop();
108
  return 0;
109
}