Quadrotor

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 "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 enable_cb(const std_msgs::Bool::ConstPtr& goal);

private:

  bool enabled;

  PID_control pid;

  ros::NodeHandle n;

  ros::Publisher pub;

  ros::Subscriber height_sub;

  ros::Subscriber goal_sub;

  ros::Subscriber enable_sub;

};

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

{

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

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

      100, &AltitudeNode::enable_cb, this);

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

}

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

{

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

}

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

{

  enabled = msg->data;

}

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

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

  AltitudeNode an = AltitudeNode();

  an.main_loop();

  return 0;

}

cmake_minimum_required(VERSION 2.4.6)

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

# Set the build type.  Options are:

#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage

#  Debug          : w/ debug symbols, w/o optimization

#  Release        : w/o debug symbols, w/ optimization

#  RelWithDebInfo : w/ debug symbols, w/ optimization

#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries

#set(ROS_BUILD_TYPE RelWithDebInfo)

rosbuild_find_ros_package(rosserial_arduino)

rosbuild_find_ros_package(arduino)

set(ARDUINO_SDK_PATH ${arduino_PACKAGE_PATH}/../../arduino-1.0)

set(ARDUINO_SDK_VERSION 1.0)

include(${rosserial_arduino_PACKAGE_PATH}/cmake_scripts/rosserial.cmake)

set(FIRMWARE_NAME arduino)

#include_directories(${ARDUINO_SDK_PATH}/hardware/arduino/variants/leonardo)

include_directories(${ARDUINO_SDK_PATH}/hardware/arduino/variants/standard)

#set(${FIRMWARE_NAME}_BOARD leonardo)

set(${FIRMWARE_NAME}_BOARD uno)

set(${FIRMWARE_NAME}_SRCS src/main.cpp)

set(${FIRMWARE_NAME}_PORT /dev/ttyACM0)

generate_ros_firmware(${FIRMWARE_NAME})

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

Run "rosmake", and then "cd build && make upload" to upload it to the arduino. You also have to "rosrun rosserial_python serial_node.py ..." with some arguments for the port the arduino is connected to.

If you are using the micro:

In order to make this successfully compile for Leonardo, you will need to copy

ArduinoHardware.h to cd `rospack find rosserial_arduino`/src/ros_lib, and save

a backup of the one that is in that folder. The header uses pointers to

HardwareSerial objects, but for Leonardo it's called Serial_.

/**

\mainpage

\htmlinclude manifest.html

\b arduino 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="arduino">

     arduino

  </description>

  <author>Thomas Mullins</author>

  <license>BSD</license>

  <review status="unreviewed" notes=""/>

  <url>http://ros.org/wiki/arduino</url>

  <depend package="rosserial_arduino"/>

  <depend package="std_msgs"/>

</package>

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

/*

 *

 * This is a modified version of ArduinoHardware.h to use Serial_ instead of

 * HardwareSerial for the Leonardo

 *

 */

#ifndef ROS_ARDUINO_HARDWARE_H_

#define ROS_ARDUINO_HARDWARE_H_

#include "WProgram.h"

#include <HardwareSerial.h>

class ArduinoHardware {

  public:

    ArduinoHardware(Serial_* io , long baud= 57600){

      iostream = io;

      baud_ = baud;

    }

    ArduinoHardware()

    {

      iostream = &Serial;

      baud_ = 57600;

    }

    ArduinoHardware(ArduinoHardware& h){

      this->iostream = iostream;

      this->baud_ = h.baud_;

    }

    void setBaud(long baud){

      this->baud_= baud;

    }

    int getBaud(){return baud_;}

    void init(){

      iostream->begin(baud_);

    }

    int read(){return iostream->read();};

    void write(uint8_t* data, int length){

      for(int i=0; i<length; i++)

        iostream->write(data[i]);

    }

    unsigned long time(){return millis();}

  protected:

    Serial_* iostream;

    long baud_;

};

#endif

#include "ArduinoHardware.h" // modified version in this directory

#include <ros.h>

#include <std_msgs/UInt16.h>

ros::NodeHandle nh;

std_msgs::UInt16 distance;

ros::Publisher range_up("range_up", &distance);

ros::Publisher range_down("range_down", &distance);

void setup()

{

  nh.initNode();

  nh.advertise(range_up);

  nh.advertise(range_down);

}

void loop()

{

  distance.data = analogRead(0);

  range_up.publish(&distance);

  distance.data = analogRead(1);

  range_down.publish(&distance);

  nh.spinOnce();

}

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/joystick_node.cpp)

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

/**

\mainpage

\htmlinclude manifest.html

\b joystick_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="joystick_node">

    Publishes roll/pitch/yaw settings to /mikrokopter/velocity and /mikrokopter/yaw

  </description>

  <author>Tom Mullins</author>

  <depend package="roscpp"/>

  <depend package="std_msgs"/>

  <depend package="sensor_msgs"/>

  <depend package="mikrokopter"/>

</package>

#include "ros/ros.h"

#include "std_msgs/Float64.h"

#include "std_msgs/Bool.h"

#include "sensor_msgs/Joy.h"

#include "mikrokopter/Velocity2D.h"

// TODO

// publish to /altitude_node/enable based on a button

// publish velocity like in joystick_control.cpp

class JoystickNode

{

public:

  JoystickNode();

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

  void joy_cb(const sensor_msgs::Joy::ConstPtr& joy);

private:

  bool altitude_enabled;

  bool alt_en_pressed;

  ros::NodeHandle nh;

  ros::Subscriber joy_sub;

  ros::Publisher velocity_pub;

  ros::Publisher yaw_pub;

  ros::Publisher thrust_pub;

  ros::Publisher altitude_enable_pub;

};

JoystickNode::JoystickNode() : altitude_enabled(false), alt_en_pressed(false)

{

  joy_sub = nh.subscribe<sensor_msgs::Joy>("/joy", 100, &JoystickNode::joy_cb,

      this);

  velocity_pub = nh.advertise<mikrokopter::Velocity2D>("/mikrokopter/velocity",

      100);

  yaw_pub = nh.advertise<std_msgs::Float64>("/mikrokopter/yaw", 100);

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

  altitude_enable_pub = nh.advertise<std_msgs::Bool>("/altitude_node/enable",

      100);

}

void JoystickNode::joy_cb(const sensor_msgs::Joy::ConstPtr& joy)

{

  mikrokopter::Velocity2D vel;

  vel.forward = joy->axes[1];

  vel.lateral = -joy->axes[0];

  velocity_pub.publish(vel);

  std_msgs::Float64 yaw;

  yaw.data = -joy->axes[2];

  yaw_pub.publish(yaw);

  bool alt_en_down = joy->buttons[0];

  if (alt_en_down && !alt_en_pressed) // toggle altitude control

  {

    altitude_enabled = !altitude_enabled;

    std_msgs::Bool enable;

    enable.data = altitude_enabled;

    altitude_enable_pub.publish(enable);

  }

  alt_en_pressed = alt_en_down;

  if (!altitude_enabled)

  {

    std_msgs::Float64 thrust;

    thrust.data = (joy->axes[3]+1)/2;

    thrust_pub.publish(thrust);

  }

}

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

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

  JoystickNode jn = JoystickNode();

  jn.main_loop();

  return 0;

}

cmake_minimum_required(VERSION 2.4.6)

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

# Set the build type.  Options are:

#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage

#  Debug          : w/ debug symbols, w/o optimization

#  Release        : w/o debug symbols, w/ optimization

#  RelWithDebInfo : w/ debug symbols, w/ optimization

#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries

#set(ROS_BUILD_TYPE RelWithDebInfo)

rosbuild_init()

#set the default path for built executables to the "bin" directory

set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)

#set the default path for built libraries to the "lib" directory

set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

set(CONTROL_PATH ../../control)

#uncomment if you have defined messages

rosbuild_genmsg()

#uncomment if you have defined services

#rosbuild_gensrv()

include_directories(${CONTROL_PATH})

rosbuild_add_executable(${PROJECT_NAME} src/mikrokopter_node.cpp src/interface.cpp)

target_link_libraries(${PROJECT_NAME} serial)

rosbuild_add_executable(externctrl_client src/externctrl_client.cpp)

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

target_link_libraries(keyboard_control ncurses)

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

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

rosbuild_add_executable(CoordToPID src/CoordToPID.cpp src/nav_lib.cpp ${CONTROL_PATH}/pid_control.cpp)

rosbuild_add_executable(mk_wrapper src/mk_wrapper.cpp)

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

 /*!

  *  \file    interface.h

  *  \brief   Defines the MikrokopterInterface class

  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>

  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>

  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>

  *  \date    2011 October 25

  *  \details This class allows communication with and control of a Mikrokopter rotorcraft via ROS

  */

#ifndef _MIKROKOPTER_INTERFACE_H_

#define _MIKROKOPTER_INTERFACE_H_

#include <SerialStream.h>

using namespace LibSerial;

#include <string>

#include <ros/ros.h>

//Messages types:

//Command Output

#include <std_msgs/Empty.h>

#include <mikrokopter/PPM.h>

#include <mikrokopter/NcDebug.h>

#include <mikrokopter/FcDebug.h>

#include <mikrokopter/Version.h>

#include <mikrokopter/AnalogLabel.h>

#include <mikrokopter/Control.h>

#include <mikrokopter/Navi.h>

#include <mikrokopter/AngleOut.h>

namespace CA

{

  class MikrokopterInterface

  {

  public:

    MikrokopterInterface(ros::NodeHandle nh);

    virtual ~MikrokopterInterface();

    enum Address

    {

      BlankAddress  = 'a',

      FcAddress     = 'b',

      NcAddress     = 'c',

      Mk3MagAddress = 'd',

      BlCtrlAddress = 'f'

    };

  protected:

  private:

    // Member variables  

    SerialStream fd;

    std::vector<unsigned char> buffer;

    std::map<unsigned char, unsigned int> msg_len_table;

    ros::NodeHandle n;

    // Publishers

    ros::Publisher ppm_data_pub;

    ros::Publisher nc_debug_data_pub;

    ros::Publisher fc_debug_data_pub;

    ros::Publisher version_data_pub;

    ros::Publisher label_data_pub;

    ros::Publisher set_control_ack_pub;  

    ros::Publisher control_data_pub;

    ros::Publisher navi_data_pub;

    ros::Publisher angleout_data_pub;

    // Subscribers

    ros::Subscriber get_control_sub;

    ros::Subscriber set_control_sub;

    ros::Subscriber req_reset_sub;

    // Timers

    ros::Timer readSerialTimer;

    ros::Timer req_version_timer;

    ros::Timer req_analog_label_timer;

    ros::Timer req_nc_debug_timer;

    ros::Timer req_fc_debug_timer;

    ros::Timer req_navi_timer;

    ros::Timer req_external_control_timer;

    // Parameters

    double req_version_rate;

    int    req_version_addr;

    double req_analog_label_rate;

    int    req_analog_label_idx;

    double req_fc_debug_rate;

    double req_nc_debug_rate;

    double req_navi_rate;

    double req_reset_rate;

    double req_external_control_rate;

    //

    // Callback functions

    //

    void readSerialCallback(const ros::TimerEvent&);

    void reqVersionCallback(const ros::TimerEvent&);

    void reqAnalogLabelCallback(const ros::TimerEvent&);

    void reqNcDebugCallback(const ros::TimerEvent&);

    void reqFcDebugCallback(const ros::TimerEvent&);

    void reqNaviCallback(const ros::TimerEvent&);

    void reqResetCallback(const std_msgs::Empty& msg);

    void setControlCallback(const mikrokopter::Control::ConstPtr& msg);

    void getControlCallback(const ros::TimerEvent&);

    //

    // Message field extraction functions

    //

    void extract_nc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::NcDebug &debug_data);

    void extract_fc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::FcDebug &debug_data);

    void extract_navi_message(std::vector<unsigned char>& buffer, mikrokopter::Navi &msg);

    void extract_ppm_message(std::vector<unsigned char>& buffer, mikrokopter::PPM &message);

    void extract_label_message(std::vector<unsigned char>& buffer, mikrokopter::AnalogLabel &message);

    void extract_control_message(std::vector<unsigned char>& buffer, mikrokopter::Control &message);

    void extract_version_message(std::vector<unsigned char>& buffer, mikrokopter::Version &message);

    void extract_angleout_message(std::vector<unsigned char>& buffer, mikrokopter::AngleOut &msg);

    //

    // Message handling functions

    //

    void process_nc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_fc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_navi_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_ppm_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_label_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_control_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_version_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_angleout_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_external_control_ack_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    //

    // Utility functions

    //

    void redirectSerialTo(MikrokopterInterface::Address addr);

    void decode64(std::vector<unsigned char>& in_arr, int offset, unsigned char* out_arr);

    int calculateChecksum(std::vector<unsigned char>& buffer);

    int calculateChecksum(unsigned char* buffer, int len);

    bool isValidChecksum(std::vector<unsigned char>& buffer);

    void updateWithChecksum(unsigned char* buffer, int len);

    void createCommand(unsigned char *commandbuffer,int &msglen,const unsigned char cmd, const unsigned char address,const unsigned char *data,int len);

    mikrokopter::GpsPos processGpsPos(unsigned char* buffer);

    mikrokopter::GpsPosDev processGpsPosDev(unsigned char* buffer);

    uint8_t connectedToAddress;

  };

}

#endif

<launch>

  <node name="joy_node" pkg="joy" type="joy_node">

  </node>

  <node name="joystick_control" pkg="mikrokopter" type="joystick_control">

    <!-- rate in Hz to publish Control messages -->

    <param name="publish_rate" value="20" />

  </node>

</launch>

<launch>

  <node name="mikrokopter" pkg="mikrokopter" 	type="mikrokopter">

    <param name="port" value="/dev/ttyUSB0" />

    <param name="req_version_rate" value="0.0" />	

    <param name="req_version_addr" value="97" />

    <param name="req_analog_label_rate" value="0.0" />

    <param name="req_nc_debug_rate" value="0.0" />

    <param name="req_fc_debug_rate" value="10.0" />

    <param name="req_navi_rate" value="0.0" />

    <param name="req_external_control_rate" value="0.0" />

  </node>

</launch>

<launch>

  <include file="$(find mikrokopter)/launch/mikrokopter.launch" />

  <node name="mk_wrapper" pkg="mikrokopter" type="mk_wrapper">

    <!-- if no Control message is received in this time in ms, go dead-->

    <param name="max_quiet_time" value="200" />

  </node>

</launch>

/**

\mainpage

\htmlinclude manifest.html

\b mikrokopter 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="mikrokopter">

     mikrokopter

  </description>

  <author>Andrew Chambers/achamber@gmail.com, Justin Haines/jhaines@andrew.cmu.edu, Sebastian Scherer/basti@andrew.cmu.edu</author>

  <license>GPLv3</license>

  <review status="unreviewed" notes=""/>

  <url>https://aeroscoutsim.frc.cs.cmu.edu/redmine/projects/riverine/wiki/Mikrokopter</url>

  <depend package="std_msgs" />

  <depend package="sensor_msgs" />

  <depend package="roscpp" />

  <rosdep name="libserial" />

</package>

# Labels of the analog values in the Debug Data Struct 

Header header

uint8 slaveAddr

uint8 index

string label

Header header

uint8 slaveAddr

int16[2] angle

uint8[2] userParameter

uint8 calcState

uint8 orientation

# Feeds the ExternControl struct in the Mikrokopter

# struct str_ExternControl

# {

#  unsigned char Digital[2];

#  unsigned char RemoteTasten;

#  signed char   Nick;

#  signed char   Roll;

#  signed char   Gier;

#  unsigned char Gas;

#  signed char   Hight;

#  unsigned char free;

#  unsigned char Frame;

#  unsigned char Config;

# };

Header header

uint8[2] digital

uint8 remoteKey

int8 pitch

int8 roll

int8 yaw

uint8 thrust

int8 height

uint8 free

uint8 frame

uint8 config

Header header

float32 updateRate_hz

uint8 slaveAddr

uint8[2] status

int32 pitchAngle

int32 rollAngle

int32 accPitch

int32 accRoll

int32 gyroYaw

int32 heightValue

int32 accZ

int32 gas

int32 compassValue

int32 batteryVoltage

int32 receiverLevel

int32 gyroCompass

int32 engineFront

int32 engineBack

int32 engineLeft

int32 engineRight

int32 onesix

int32 oneseven

int32 oneeight

int32 onenine

int32 servo

int32 hovergas

int32 current

int32 capacity_mAh

int32 heightSetpoint

int32 twofive

int32 twosix

int32 compassSetpoint

int32 i2c_error

int32 bl_limit

int32 GPS_Pitch

int32 GPS_Roll

int32 longitude

int32 latitude

int32 altitude

uint8 status

uint16 distance

int16 bearing

Header header

float32 updateRate_hz

uint8 slaveAddr

uint8 version

GpsPos currentPosition

GpsPos targetPosition

GpsPosDev targetPositionDeviation

GpsPos homePosition

GpsPosDev homePositionDeviation

uint8 waypointIndex

uint8 waypointNumber

uint8 satsInUse

int16 altimeter

int16 variometer

uint16 flyingTime

uint8 uBat

uint16 groundSpeed

int16 heading

int16 compassHeading

int8 angleNick

int8 angleRoll

uint8 rcQuality

uint8 fcStatusFlags

uint8 ncFlags

uint8 errorCode

uint8 operatingRadius

int16 topSpeed

uint8 targetHoldTime

uint8 fcStatusFlags2

int16 setpointAltitude

uint8 gas

uint16 current

uint16 usedCapacity

Header header

float32 updateRate_hz

uint8 slaveAddr

uint8[2] status

int32 angleNick

int32 angleRoll

int32 accNick

int32 accRoll

int32 operatingRadius

int32 fcFlags

int32 ncFlags

int32 nickServo

int32 rollServo

int32 gpsData

int32 compassHeading

int32 gyroHeading

int32 spiError

int32 spiOkay

int32 i2cError

int32 i2cOkay

int32 poiIndex          # Kalman_K

int32 accSpeedN

int32 accSpeedE

int32 speedZ            # GPS ACC

int32 empty20

int32 nSpeed

int32 eSpeed

int32 empty23

int32 magnetX

int32 magnetY

int32 magnetZ

int32 distanceN

int32 distanceE

int32 gpsNick

int32 gpsRoll

int32 usedSats

Header header

int32[27] channels

Header header

int16[8] channels

bool	 valid_signal

Header header

float64 battery_volt

bool 	flying

bool 	motors_on

bool 	autonomous

Header header

float64[4] thrust

Header header

float64 forward

float64 lateral

# // bitmask for HardwareError[0]

# FC_ERROR0_GYRO_NICK 	0x01

# FC_ERROR0_GYRO_ROLL 	0x02

# FC_ERROR0_GYRO_YAW 		0x04

# FC_ERROR0_ACC_NICK 		0x08

# FC_ERROR0_ACC_ROLL 		0x10

# FC_ERROR0_ACC_TOP  		0x20

# FC_ERROR0_PRESSURE		0x40

# FC_ERROR0_CAREFREE		0x80

# // bitmask for HardwareError[1]

# FC_ERROR1_I2C   	 	  0x01

# FC_ERROR1_BL_MISSING 	0x02

# FC_ERROR1_SPI_RX	 	  0x04

# FC_ERROR1_PPM	 		    0x08

# FC_ERROR1_MIXER			  0x10

# FC_ERROR1_RES1			  0x20

# FC_ERROR1_RES2			  0x40

# FC_ERROR1_RES3			  0x80

Header header

uint8 slaveAddr

uint8 swMajor

uint8 swMinor

uint8 protoMajor

uint8 protoMinor

uint8 swPatch

uint8[5] hardwareError

libserial:

  ubuntu: libserial0 libserial-dev

  debian: libserial-dev

#include "nav_lib.h"

#include <geometry_msgs/Point.h>

#include <math.h>

#include "pid_control.h"

#include <time.h>

class CoordToPID

{

public:

  CoordToPID();

  void mainLoop();

  void pointCallback(const geometry_msgs::Point::ConstPtr& p);

private:

  MikrokopterControl control;

  PID_control pidanglecontrol;

  PID_control pidvelcontrol;

  ros::Publisher pub;

  ros::NodeHandle n;

  ros::Subscriber sub;

  int goaldistance;

};

CoordToPID::CoordToPID():pidanglecontrol(0.4,0.01,0.4,0),pidvelcontrol(0.4,0.01,0.4,1)

{

  //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);

  sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,

      &CoordToPID::pointCallback, this);

  //Sets Goal values for the PID	

  //Goal angle is 0 degrees

  float p = 0.4;

  float d = 0.01;

  float i = 0.4;

  PID_control pidanglecontrol = PID_control(p,d,i,0);

  //pdi values for velocity	

  p = 0.4;

  d = 0.01;

  i = 0.4;

  //Target distance to stay from target

  goaldistance = 100; //What units this in?

  PID_control pidvelcontrol = PID_control(p,d,i,goaldistance);

}

void CoordToPID::mainLoop()

{

  /*ros::Rate loop_rate(25);

  while(ros::ok())

  {

    ros::spinOnce();

    control.publish_on(pub);

    loop_rate.sleep();

  }*/

  control.main_loop();

}

void CoordToPID::pointCallback(const geometry_msgs::Point::ConstPtr& p)

{

  	//call on Priya's PID

	/* Initializes the PID controller 

	PID_control(int p_term, int d_term, int i_term, float goal); 

	Initializes the PID controller without goal term 

	PID_control(int p_term, int d_term, int i_term); 

	Reset the I term error and change the goal value 

	void change_goal(float goal);

	Given an input, get the PID controlled output 

	float pid(float input)*/

	// angle going CCW from forward in horizontal plane

  	float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct

 	 // should be from [-100, 100]

	float distance = sqrt(pow(p->x,2)+pow(p->y,2));

	//Updates pid with current value and gets pid output

	float pidangle = pidanglecontrol.pid(angle,(double)time(NULL));

	float pidvelocity = pidvelcontrol.pid(distance,(double)time(NULL));

    //ROS_INFO("angle %g, pidangle %g, distance %g, piddistance %g",angle,pidangle,distance,pidvelocity);	

    //sets pid output

	control.set_yaw(pidangle*100/M_PI);

	control.velocity_control(pidvelocity,0);

	//controls.set_thrust() //between [0,1]

}

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

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

  CoordToPID ctp = CoordToPID();

  ctp.mainLoop();

  return 0;

}

 /*!

  *  \file    externctrl_client.cpp

  *  \brief   A demonstration of the MikrokopterInterface class

  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>

  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>

  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>

  *  \date    2011 October 25

  *  \details This program sends commands to a Mikrokopter rotorcraft via ROS

  */

#include "ros/ros.h"

#include "mikrokopter/Control.h"

#include <cstdlib>

int main(int argc, char **argv)

{

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

  ros::NodeHandle n;

  ros::Publisher pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);

  mikrokopter::Control req;

  req.digital[0] = 0;

  req.digital[1] = 0;

  req.remoteKey = 0;

  req.pitch = 0;

  req.roll = 0;

  req.yaw = 0;

  req.thrust = 40;

  req.height = 0;

  req.free = 0;

  req.frame = 7;

  req.config = 1;

  ros::Rate loop_rate(25);

  int maxPt = 30;

  int minPt = -30;

  int step = 2;

  int val = 30;

  while (ros::ok())

  {

    if (val >= maxPt)

      step = -1*step;

    if (val <= minPt)

      step = -1*step;

    val += step;

    std::cout << val << std::endl;

    req.frame++;

    req.roll = val;

    pub.publish(req);

    ros::spinOnce();

    loop_rate.sleep();

  }

  return 0;

}

 /*!

  *  \file    interface.cpp

  *  \brief   Implements the MikrokopterInterface class

  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>

  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>

  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>

  *  \date    2011 October 25

  *  \details This class allows communication with and control of a Mikrokopter rotorcraft via ROS

  */

#include "mikrokopter/interface.h"

using namespace CA;

MikrokopterInterface::MikrokopterInterface(ros::NodeHandle nh):

    n(nh)

{

  //

  // Advertise all of the messages that this node might publish

  //

  ppm_data_pub = n.advertise<mikrokopter::PPM>("/mikrokopter/ppm", 100);

  fc_debug_data_pub = n.advertise<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100);

  nc_debug_data_pub = n.advertise<mikrokopter::NcDebug>("/mikrokopter/nc_debug", 100);

  navi_data_pub = n.advertise<mikrokopter::Navi>("/mikrokopter/navi", 100);

  version_data_pub = n.advertise<mikrokopter::Version>("/mikrokopter/version", 100);

  label_data_pub = n.advertise<mikrokopter::AnalogLabel>("/mikrokopter/analog_label", 100);

  set_control_ack_pub = n.advertise<mikrokopter::Control>("/mikrokopter/set_control_ack", 100);  

  control_data_pub = n.advertise<mikrokopter::Control>("/mikrokopter/extern_control", 100);

  angleout_data_pub = n.advertise<mikrokopter::AngleOut>("/mikrokopter/angle_out", 100);

  //

  // Subscribe to the external control and reset request topics

  //

  set_control_sub = n.subscribe("/mikrokopter/req_set_control", 100, &MikrokopterInterface::setControlCallback, this);

  req_reset_sub = n.subscribe("/mikrokopter/req_reset", 100, &MikrokopterInterface::reqResetCallback, this);

  //

  // Check to see if we should be sending periodic requests to the MK

  //

  if(n.getParam("/mikrokopter/req_version_rate",req_version_rate) && req_version_rate > 0.0)

  {

    if(!n.getParam("mikrokopter/req_version_addr",req_version_addr))

    {

      ROS_ERROR_STREAM("Must define req_version_addr");

      ros::shutdown();

    }

    req_version_timer = n.createTimer(ros::Duration(1/req_version_rate), &MikrokopterInterface::reqVersionCallback, this);

  }

  if(n.getParam("/mikrokopter/req_analog_label_rate",req_analog_label_rate) && req_analog_label_rate > 0.0)

  {

    req_analog_label_idx = 0;

    req_analog_label_timer = n.createTimer(ros::Duration(1/req_analog_label_rate), &MikrokopterInterface::reqAnalogLabelCallback, this);

  }

  if(n.getParam("/mikrokopter/req_nc_debug_rate",req_nc_debug_rate) && req_nc_debug_rate > 0.0)

  {

    req_nc_debug_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqNcDebugCallback, this);

  }

  if(n.getParam("/mikrokopter/req_fc_debug_rate",req_fc_debug_rate) && req_fc_debug_rate > 0.0)

  {

    req_fc_debug_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqFcDebugCallback, this);

  }

  if(n.getParam("/mikrokopter/req_navi_rate",req_navi_rate) && req_navi_rate > 0.0)

  {

    req_navi_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqNaviCallback, this);

  }

  if(n.getParam("/mikrokopter/req_external_control_rate",req_external_control_rate) && req_external_control_rate > 0.0)

  {

    req_external_control_timer = n.createTimer(ros::Duration(1/req_external_control_rate), &MikrokopterInterface::getControlCallback, this);

  }

  //

  // Set up the serial port and callback

  //

  readSerialTimer = n.createTimer(ros::Duration(0.02), &MikrokopterInterface::readSerialCallback, this);

  // Open serial port

  std::string portname;

  if(!n.getParam("/mikrokopter/port",portname))

  {

    ROS_ERROR_STREAM("Port not defined");

    ros::shutdown();

  }

  fd.Open(portname,std::ios::in|std::ios::out);

  if (!fd.good())

  {

    ROS_ERROR_STREAM( "Error : unable to open " << portname);

    ros::shutdown();

  }

  else

  {

    ROS_INFO_STREAM( "Opening port: " << portname );

  }

  fd.SetBaudRate(SerialStreamBuf::BAUD_57600);

  fd.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);

  fd.SetNumOfStopBits(1);

  fd.SetParity(SerialStreamBuf::PARITY_NONE);

  fd.SetFlowControl(SerialStreamBuf::FLOW_CONTROL_NONE);//NONE

  //

  // Create a lookup table mapping from message ID to message length

  //

  msg_len_table['A'] = 30;

  msg_len_table['B'] = 10;

  msg_len_table['V'] = 22;

  msg_len_table['D'] = 94;

  msg_len_table['G'] = 22;

  msg_len_table['k'] = 18;

  msg_len_table['P'] = 78;

  msg_len_table['O'] = 118;

  redirectSerialTo(NcAddress);

}

MikrokopterInterface::~MikrokopterInterface()

{

  fd.Close();

}

void MikrokopterInterface::redirectSerialTo(MikrokopterInterface::Address addr)

{

    //

    // Nothing to do if we are already connected to the right board

    //

    if(connectedToAddress == addr)

    {

      return;

    }

    if (addr == FcAddress)

    {

      unsigned char commandbuffer[512];

      int cmdlen = 0;

      unsigned char board[]={0};  // uart selector for FC

      createCommand(commandbuffer,cmdlen,'u',NcAddress,board,1);

      fd.write((const char*)commandbuffer,cmdlen);