Quadrotor

#include "ros/ros.h"

#include "mikrokopter/FcDebug.h"

#include "std_msgs/Float64.h"

#include "std_msgs/Bool.h"

#include "../../../control/pid_control.h"

class AltitudeNode

{

public:

  AltitudeNode();

  void main_loop() {ros::spin();}

  void height_cb(const mikrokopter::FcDebug::ConstPtr& fc);

  void goal_cb(const std_msgs::Float64::ConstPtr& goal);

  void p_cb(const std_msgs::Float64::ConstPtr& p);

  void i_cb(const std_msgs::Float64::ConstPtr& i);

  void d_cb(const std_msgs::Float64::ConstPtr& d);

  void enable_cb(const std_msgs::Bool::ConstPtr& goal);

private:

  bool enabled;

  float last_height;

  PID_control pid;

  ros::NodeHandle n;

  ros::Publisher pub;

  ros::Subscriber height_sub;

  ros::Subscriber goal_sub;

  ros::Subscriber p_sub;

  ros::Subscriber i_sub;

  ros::Subscriber d_sub;

  ros::Subscriber enable_sub;

};

AltitudeNode::AltitudeNode() : pid(0, 0, 0, 0), enabled(false), last_height(0)

{

  height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100,

      &AltitudeNode::height_cb, this);

  goal_sub = n.subscribe<std_msgs::Float64>("/altitude_node/height_goal", 100,

      &AltitudeNode::goal_cb, this);

  p_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/p", 100,

      &AltitudeNode::p_cb, this);

  i_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/i", 100,

      &AltitudeNode::i_cb, this);

  d_sub = n.subscribe<std_msgs::Float64>("/altitude_node/pid/d", 100,

      &AltitudeNode::d_cb, this);

  enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable", 100,

      &AltitudeNode::enable_cb, this);

  pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);

  /*double default_goal;

  ros::param::param<double>("~default_goal", default_goal, 0);

  pid.change_goal(default_goal);*/

}

void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc)

{

  last_height = fc->heightValue;

  if (enabled)

  {

    std_msgs::Float64 msg;

    msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec());

    pub.publish(msg);

  }

}

void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal)

{

  pid.change_goal(goal->data);

  ROS_INFO("Goal: %f", goal->data);

}

void AltitudeNode::p_cb(const std_msgs::Float64::ConstPtr& p)

{

  pid.change_p(p->data);

  ROS_INFO("K_p: %f", p->data);

}

void AltitudeNode::i_cb(const std_msgs::Float64::ConstPtr& i)

{

  pid.change_i(i->data);

  ROS_INFO("K_i: %f", i->data);

}

void AltitudeNode::d_cb(const std_msgs::Float64::ConstPtr& d)

{

  pid.change_d(d->data);

  ROS_INFO("K_d: %f", d->data);

}

void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg)

{

  enabled = msg->data;

  if (enabled)

  {

    pid.change_goal(last_height);

    ROS_INFO("Altitude Hold: Enabled.");

    ROS_INFO("Altitude: %f", last_height);

  }

  else

  {

    ROS_INFO("Altitude Hold: Disabled.");

  }

}

int main(int argc, char **argv) {

  ros::init(argc, argv, "altitude_node");

  AltitudeNode an = AltitudeNode();

  an.main_loop();

  return 0;

}