root / quad2 / altitude_node / src / altitude_node.cpp @ 02975ec6
History | View | Annotate | Download (2.9 KB)
1 | 737bd480 | Tom Mullins | #include "ros/ros.h" |
---|---|---|---|
2 | #include "mikrokopter/FcDebug.h" |
||
3 | 7bc485ed | Tom Mullins | #include "std_msgs/Float64.h" |
4 | #include "std_msgs/Bool.h" |
||
5 | 737bd480 | Tom Mullins | #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 | 734b51b9 | Tom Mullins | 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 | 7bc485ed | Tom Mullins | void enable_cb(const std_msgs::Bool::ConstPtr& goal); |
18 | 737bd480 | Tom Mullins | |
19 | private:
|
||
20 | 7bc485ed | Tom Mullins | bool enabled;
|
21 | 02975ec6 | Tom Mullins | float last_height;
|
22 | 737bd480 | Tom Mullins | PID_control pid; |
23 | ros::NodeHandle n; |
||
24 | ros::Publisher pub; |
||
25 | ros::Subscriber height_sub; |
||
26 | ros::Subscriber goal_sub; |
||
27 | 734b51b9 | Tom Mullins | ros::Subscriber p_sub; |
28 | ros::Subscriber i_sub; |
||
29 | ros::Subscriber d_sub; |
||
30 | 7bc485ed | Tom Mullins | ros::Subscriber enable_sub; |
31 | 737bd480 | Tom Mullins | }; |
32 | |||
33 | 02975ec6 | Tom Mullins | AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false), last_height(0) |
34 | 737bd480 | Tom Mullins | { |
35 | 734b51b9 | Tom Mullins | 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 | 737bd480 | Tom Mullins | pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100); |
48 | 122a57d8 | Tom Mullins | |
49 | 02975ec6 | Tom Mullins | /*double default_goal;
|
50 | 734b51b9 | Tom Mullins | ros::param::param<double>("~default_goal", default_goal, 0);
|
51 | 02975ec6 | Tom Mullins | pid.change_goal(default_goal);*/
|
52 | 737bd480 | Tom Mullins | } |
53 | |||
54 | void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc) |
||
55 | { |
||
56 | 02975ec6 | Tom Mullins | last_height = fc->heightValue; |
57 | 7bc485ed | Tom Mullins | 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 | 737bd480 | Tom Mullins | } |
64 | |||
65 | void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal) |
||
66 | { |
||
67 | pid.change_goal(goal->data); |
||
68 | 734b51b9 | Tom Mullins | 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 | 737bd480 | Tom Mullins | } |
88 | |||
89 | 7bc485ed | Tom Mullins | void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg) |
90 | { |
||
91 | enabled = msg->data; |
||
92 | 98e35b23 | Alex Zirbel | if (enabled)
|
93 | { |
||
94 | 02975ec6 | Tom Mullins | pid.change_goal(last_height); |
95 | 98e35b23 | Alex Zirbel | ROS_INFO("Altitude Hold: Enabled.");
|
96 | 02975ec6 | Tom Mullins | ROS_INFO("Altitude: %f", last_height);
|
97 | 98e35b23 | Alex Zirbel | } |
98 | else
|
||
99 | { |
||
100 | ROS_INFO("Altitude Hold: Disabled.");
|
||
101 | } |
||
102 | 7bc485ed | Tom Mullins | } |
103 | |||
104 | 737bd480 | Tom Mullins | 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 | } |