Quadrotor

rosbuild_add_executable(node_control src/node_control.cpp src/nav_lib.cpp)

cmake_minimum_required(VERSION 2.4.6)

include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

rosbuild_init()

set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)

set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

#rosbuild_genmsg()

#rosbuild_gensrv()

rosbuild_add_executable(${PROJECT_NAME} src/altitude_node.cpp ../../control/pid_control.cpp)

include $(shell rospack find mk)/cmake.mk

/**

\mainpage

\htmlinclude manifest.html

\b altitude_node is ... 

<!-- 

Provide an overview of your package.

-->

\section codeapi Code API

<!--

Provide links to specific auto-generated API documentation within your

package that is of particular interest to a reader. Doxygen will

document pretty much every part of your code, so do your best here to

point the reader to the actual API.

If your codebase is fairly large or has different sets of APIs, you

should use the doxygen 'group' tag to keep these APIs together. For

example, the roscpp documentation has 'libros' group.

-->

*/

<package>

  <description brief="altitude_node">

    Publishes thrust settings to /mikrokopter/thrust based on height information

  </description>

  <author>Tom Mullins</author>

  <depend package="roscpp"/>

  <depend package="std_msgs"/>

  <depend package="mikrokopter"/>

</package>

#include "ros/ros.h"

#include "std_msgs/Float64.h"

#include "mikrokopter/FcDebug.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);

private:

  PID_control pid;

  ros::NodeHandle n;

  ros::Publisher pub;

  ros::Subscriber height_sub;

  ros::Subscriber goal_sub;

};

AltitudeNode::AltitudeNode() : pid(10, 0, 0, 0)

{

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

      100, &AltitudeNode::height_cb, this);

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

      100, &AltitudeNode::goal_cb, this);

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

}

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

{

  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);

}

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

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

  AltitudeNode an = AltitudeNode();

  an.main_loop();

  return 0;

}

    if (thrust < 0) thrust = 0;

    if (thrust > 1) thrust = 1;