Revision 02975ec6

View differences:

quad2/altitude_node/src/altitude_node.cpp
18 18

  
19 19
private:
20 20
  bool enabled;
21
  float last_height;
21 22
  PID_control pid;
22 23
  ros::NodeHandle n;
23 24
  ros::Publisher pub;
......
29 30
  ros::Subscriber enable_sub;
30 31
};
31 32

  
32
AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false)
33
AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false), last_height(0)
33 34
{
34 35
  height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100,
35 36
      &AltitudeNode::height_cb, this);
......
45 46
      &AltitudeNode::enable_cb, this);
46 47
  pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);
47 48
  
48
  double default_goal;
49
  /*double default_goal;
49 50
  ros::param::param<double>("~default_goal", default_goal, 0);
50
  pid.change_goal(default_goal);
51
  pid.change_goal(default_goal);*/
51 52
}
52 53

  
53 54
void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc)
54 55
{
56
  last_height = fc->heightValue;
55 57
  if (enabled)
56 58
  {
57 59
    std_msgs::Float64 msg;
......
89 91
  enabled = msg->data;
90 92
  if (enabled)
91 93
  {
94
    pid.change_goal(last_height);
92 95
    ROS_INFO("Altitude Hold: Enabled.");
96
    ROS_INFO("Altitude: %f", last_height);
93 97
  }
94 98
  else
95 99
  {

Also available in: Unified diff