Revision 98e35b23

View differences:

mikrokopter/altitude_node/CMakeLists.txt
1
cmake_minimum_required(VERSION 2.4.6)
2
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3

  
4
rosbuild_init()
5

  
6
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
7
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
8

  
9
#rosbuild_genmsg()
10
#rosbuild_gensrv()
11

  
12
rosbuild_add_executable(${PROJECT_NAME} src/altitude_node.cpp ../../control/pid_control.cpp)
mikrokopter/altitude_node/Makefile
1
include $(shell rospack find mk)/cmake.mk
mikrokopter/altitude_node/mainpage.dox
1
/**
2
\mainpage
3
\htmlinclude manifest.html
4

  
5
\b altitude_node is ... 
6

  
7
<!-- 
8
Provide an overview of your package.
9
-->
10

  
11

  
12
\section codeapi Code API
13

  
14
<!--
15
Provide links to specific auto-generated API documentation within your
16
package that is of particular interest to a reader. Doxygen will
17
document pretty much every part of your code, so do your best here to
18
point the reader to the actual API.
19

  
20
If your codebase is fairly large or has different sets of APIs, you
21
should use the doxygen 'group' tag to keep these APIs together. For
22
example, the roscpp documentation has 'libros' group.
23
-->
24

  
25

  
26
*/
mikrokopter/altitude_node/manifest.xml
1
<package>
2

  
3
  <description brief="altitude_node">
4
    Publishes thrust settings to /mikrokopter/thrust based on height information
5
  </description>
6
  <author>Tom Mullins</author>
7
  
8
  <depend package="roscpp"/>
9
  <depend package="std_msgs"/>
10
  <depend package="mikrokopter"/>
11

  
12
</package>
13

  
14

  
mikrokopter/altitude_node/src/altitude_node.cpp
1
#include "ros/ros.h"
2
#include "mikrokopter/FcDebug.h"
3
#include "std_msgs/Float64.h"
4
#include "std_msgs/Bool.h"
5
#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
  void enable_cb(const std_msgs::Bool::ConstPtr& goal);
15

  
16
private:
17
  bool enabled;
18
  PID_control pid;
19
  ros::NodeHandle n;
20
  ros::Publisher pub;
21
  ros::Subscriber height_sub;
22
  ros::Subscriber goal_sub;
23
  ros::Subscriber enable_sub;
24
};
25

  
26
AltitudeNode::AltitudeNode() : pid(10, 0, 0, 0), enabled(false)
27
{
28
  height_sub = n.subscribe<mikrokopter::FcDebug>("/mikrokopter/fc_debug",
29
      100, &AltitudeNode::height_cb, this);
30
  goal_sub = n.subscribe<std_msgs::Float64>("/mikrokopter/height_goal",
31
      100, &AltitudeNode::goal_cb, this);
32
  enable_sub = n.subscribe<std_msgs::Bool>("/altitude_node/enable",
33
      100, &AltitudeNode::enable_cb, this);
34
  pub = n.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);
35
}
36

  
37
void AltitudeNode::height_cb(const mikrokopter::FcDebug::ConstPtr& fc)
38
{
39
  if (enabled)
40
  {
41
    std_msgs::Float64 msg;
42
    msg.data = pid.pid(fc->heightValue, fc->header.stamp.toNSec());
43
    pub.publish(msg);
44
  }
45
}
46

  
47
void AltitudeNode::goal_cb(const std_msgs::Float64::ConstPtr& goal)
48
{
49
  pid.change_goal(goal->data);
50
}
51

  
52
void AltitudeNode::enable_cb(const std_msgs::Bool::ConstPtr& msg)
53
{
54
  enabled = msg->data;
55
}
56

  
57
int main(int argc, char **argv) {
58
  ros::init(argc, argv, "altitude_node");
59
  AltitudeNode an = AltitudeNode();
60
  an.main_loop();
61
  return 0;
62
}
mikrokopter/arduino/CMakeLists.txt
1
cmake_minimum_required(VERSION 2.4.6)
2
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3

  
4
# Set the build type.  Options are:
5
#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
6
#  Debug          : w/ debug symbols, w/o optimization
7
#  Release        : w/o debug symbols, w/ optimization
8
#  RelWithDebInfo : w/ debug symbols, w/ optimization
9
#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
10
#set(ROS_BUILD_TYPE RelWithDebInfo)
11

  
12
rosbuild_find_ros_package(rosserial_arduino)
13
rosbuild_find_ros_package(arduino)
14
set(ARDUINO_SDK_PATH ${arduino_PACKAGE_PATH}/../../arduino-1.0)
15
set(ARDUINO_SDK_VERSION 1.0)
16
include(${rosserial_arduino_PACKAGE_PATH}/cmake_scripts/rosserial.cmake)
17

  
18
set(FIRMWARE_NAME arduino)
19

  
20
#include_directories(${ARDUINO_SDK_PATH}/hardware/arduino/variants/leonardo)
21
include_directories(${ARDUINO_SDK_PATH}/hardware/arduino/variants/standard)
22

  
23
#set(${FIRMWARE_NAME}_BOARD leonardo)
24
set(${FIRMWARE_NAME}_BOARD uno)
25
set(${FIRMWARE_NAME}_SRCS src/main.cpp)
26
set(${FIRMWARE_NAME}_PORT /dev/ttyACM0)
27
generate_ros_firmware(${FIRMWARE_NAME})
mikrokopter/arduino/Makefile
1
include $(shell rospack find mk)/cmake.mk
mikrokopter/arduino/README
1
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.
2

  
3

  
4

  
5
If you are using the micro:
6
In order to make this successfully compile for Leonardo, you will need to copy
7
ArduinoHardware.h to cd `rospack find rosserial_arduino`/src/ros_lib, and save
8
a backup of the one that is in that folder. The header uses pointers to
9
HardwareSerial objects, but for Leonardo it's called Serial_.
mikrokopter/arduino/mainpage.dox
1
/**
2
\mainpage
3
\htmlinclude manifest.html
4

  
5
\b arduino is ... 
6

  
7
<!-- 
8
Provide an overview of your package.
9
-->
10

  
11

  
12
\section codeapi Code API
13

  
14
<!--
15
Provide links to specific auto-generated API documentation within your
16
package that is of particular interest to a reader. Doxygen will
17
document pretty much every part of your code, so do your best here to
18
point the reader to the actual API.
19

  
20
If your codebase is fairly large or has different sets of APIs, you
21
should use the doxygen 'group' tag to keep these APIs together. For
22
example, the roscpp documentation has 'libros' group.
23
-->
24

  
25

  
26
*/
mikrokopter/arduino/manifest.xml
1
<package>
2
  <description brief="arduino">
3

  
4
     arduino
5

  
6
  </description>
7
  <author>Thomas Mullins</author>
8
  <license>BSD</license>
9
  <review status="unreviewed" notes=""/>
10
  <url>http://ros.org/wiki/arduino</url>
11
  <depend package="rosserial_arduino"/>
12
  <depend package="std_msgs"/>
13

  
14
</package>
15

  
16

  
mikrokopter/arduino/src/ArduinoHardware.h-replacement
1
/* 
2
 * Software License Agreement (BSD License)
3
 *
4
 * Copyright (c) 2011, Willow Garage, Inc.
5
 * All rights reserved.
6
 *
7
 * Redistribution and use in source and binary forms, with or without
8
 * modification, are permitted provided that the following conditions
9
 * are met:
10
 *
11
 *  * Redistributions of source code must retain the above copyright
12
 *    notice, this list of conditions and the following disclaimer.
13
 *  * Redistributions in binary form must reproduce the above
14
 *    copyright notice, this list of conditions and the following
15
 *    disclaimer in the documentation and/or other materials provided
16
 *    with the distribution.
17
 *  * Neither the name of Willow Garage, Inc. nor the names of its
18
 *    contributors may be used to endorse or promote prducts derived
19
 *    from this software without specific prior written permission.
20
 *
21
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32
 * POSSIBILITY OF SUCH DAMAGE.
33
 */
34

  
35
/*
36
 *
37
 * This is a modified version of ArduinoHardware.h to use Serial_ instead of
38
 * HardwareSerial for the Leonardo
39
 *
40
 */
41

  
42
#ifndef ROS_ARDUINO_HARDWARE_H_
43
#define ROS_ARDUINO_HARDWARE_H_
44

  
45
#include "WProgram.h"
46
#include <HardwareSerial.h>
47

  
48
class ArduinoHardware {
49
  public:
50
    ArduinoHardware(Serial_* io , long baud= 57600){
51
      iostream = io;
52
      baud_ = baud;
53
    }
54
    ArduinoHardware()
55
    {
56
      iostream = &Serial;
57
      baud_ = 57600;
58
    }
59
    ArduinoHardware(ArduinoHardware& h){
60
      this->iostream = iostream;
61
      this->baud_ = h.baud_;
62
    }
63
  
64
    void setBaud(long baud){
65
      this->baud_= baud;
66
    }
67
  
68
    int getBaud(){return baud_;}
69

  
70
    void init(){
71
      iostream->begin(baud_);
72
    }
73

  
74
    int read(){return iostream->read();};
75
    void write(uint8_t* data, int length){
76
      for(int i=0; i<length; i++)
77
        iostream->write(data[i]);
78
    }
79

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

  
82
  protected:
83
    Serial_* iostream;
84
    long baud_;
85
};
86

  
87
#endif
mikrokopter/arduino/src/main.cpp
1
#include "ArduinoHardware.h" // modified version in this directory
2
#include <ros.h>
3
#include <std_msgs/UInt16.h>
4

  
5
ros::NodeHandle nh;
6

  
7
std_msgs::UInt16 distance;
8
ros::Publisher range_up("range_up", &distance);
9
ros::Publisher range_down("range_down", &distance);
10

  
11
void setup()
12
{
13
  nh.initNode();
14
  nh.advertise(range_up);
15
  nh.advertise(range_down);
16
}
17

  
18
void loop()
19
{
20
  distance.data = analogRead(0);
21
  range_up.publish(&distance);
22
  distance.data = analogRead(1);
23
  range_down.publish(&distance);
24
  nh.spinOnce();
25
}
mikrokopter/joystick_node/CMakeLists.txt
1
cmake_minimum_required(VERSION 2.4.6)
2
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3

  
4
rosbuild_init()
5

  
6
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
7
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
8

  
9
#rosbuild_genmsg()
10
#rosbuild_gensrv()
11

  
12
rosbuild_add_executable(${PROJECT_NAME} src/joystick_node.cpp)
mikrokopter/joystick_node/Makefile
1
include $(shell rospack find mk)/cmake.mk
mikrokopter/joystick_node/mainpage.dox
1
/**
2
\mainpage
3
\htmlinclude manifest.html
4

  
5
\b joystick_node is ... 
6

  
7
<!-- 
8
Provide an overview of your package.
9
-->
10

  
11

  
12
\section codeapi Code API
13

  
14
<!--
15
Provide links to specific auto-generated API documentation within your
16
package that is of particular interest to a reader. Doxygen will
17
document pretty much every part of your code, so do your best here to
18
point the reader to the actual API.
19

  
20
If your codebase is fairly large or has different sets of APIs, you
21
should use the doxygen 'group' tag to keep these APIs together. For
22
example, the roscpp documentation has 'libros' group.
23
-->
24

  
25

  
26
*/
mikrokopter/joystick_node/manifest.xml
1
<package>
2

  
3
  <description brief="joystick_node">
4
    Publishes roll/pitch/yaw settings to /mikrokopter/velocity and /mikrokopter/yaw
5
  </description>
6
  <author>Tom Mullins</author>
7
  
8
  <depend package="roscpp"/>
9
  <depend package="std_msgs"/>
10
  <depend package="sensor_msgs"/>
11
  <depend package="mikrokopter"/>
12

  
13
</package>
14

  
15

  
mikrokopter/joystick_node/src/joystick_node.cpp
1
#include "ros/ros.h"
2
#include "std_msgs/Float64.h"
3
#include "std_msgs/Bool.h"
4
#include "sensor_msgs/Joy.h"
5
#include "mikrokopter/Velocity2D.h"
6

  
7
// TODO
8
// publish to /altitude_node/enable based on a button
9
// publish velocity like in joystick_control.cpp
10

  
11
class JoystickNode
12
{
13
public:
14
  JoystickNode();
15
  void main_loop() {ros::spin();}
16
  void joy_cb(const sensor_msgs::Joy::ConstPtr& joy);
17

  
18
private:
19
  bool altitude_enabled;
20
  bool alt_en_pressed;
21
  ros::NodeHandle nh;
22
  ros::Subscriber joy_sub;
23
  ros::Publisher velocity_pub;
24
  ros::Publisher yaw_pub;
25
  ros::Publisher thrust_pub;
26
  ros::Publisher altitude_enable_pub;
27
};
28

  
29
JoystickNode::JoystickNode() : altitude_enabled(false), alt_en_pressed(false)
30
{
31
  joy_sub = nh.subscribe<sensor_msgs::Joy>("/joy", 100, &JoystickNode::joy_cb,
32
      this);
33
  velocity_pub = nh.advertise<mikrokopter::Velocity2D>("/mikrokopter/velocity",
34
      100);
35
  yaw_pub = nh.advertise<std_msgs::Float64>("/mikrokopter/yaw", 100);
36
  thrust_pub = nh.advertise<std_msgs::Float64>("/mikrokopter/thrust", 100);
37
  altitude_enable_pub = nh.advertise<std_msgs::Bool>("/altitude_node/enable",
38
      100);
39
}
40

  
41
void JoystickNode::joy_cb(const sensor_msgs::Joy::ConstPtr& joy)
42
{
43
  mikrokopter::Velocity2D vel;
44
  vel.forward = joy->axes[1];
45
  vel.lateral = -joy->axes[0];
46
  velocity_pub.publish(vel);
47
  
48
  std_msgs::Float64 yaw;
49
  yaw.data = -joy->axes[2];
50
  yaw_pub.publish(yaw);
51
  
52
  bool alt_en_down = joy->buttons[0];
53
  if (alt_en_down && !alt_en_pressed) // toggle altitude control
54
  {
55
    altitude_enabled = !altitude_enabled;
56
    
57
    std_msgs::Bool enable;
58
    enable.data = altitude_enabled;
59
    altitude_enable_pub.publish(enable);
60
  }
61
  alt_en_pressed = alt_en_down;
62
  
63
  if (!altitude_enabled)
64
  {
65
    std_msgs::Float64 thrust;
66
    thrust.data = (joy->axes[3]+1)/2;
67
    thrust_pub.publish(thrust);
68
  }
69
}
70

  
71
int main(int argc, char **argv) {
72
  ros::init(argc, argv, "joystick_node");
73
  JoystickNode jn = JoystickNode();
74
  jn.main_loop();
75
  return 0;
76
}
mikrokopter/mikrokopter/CMakeLists.txt
1
cmake_minimum_required(VERSION 2.4.6)
2
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3

  
4
# Set the build type.  Options are:
5
#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
6
#  Debug          : w/ debug symbols, w/o optimization
7
#  Release        : w/o debug symbols, w/ optimization
8
#  RelWithDebInfo : w/ debug symbols, w/ optimization
9
#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
10
#set(ROS_BUILD_TYPE RelWithDebInfo)
11

  
12
rosbuild_init()
13

  
14
#set the default path for built executables to the "bin" directory
15
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
16
#set the default path for built libraries to the "lib" directory
17
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
18

  
19
set(CONTROL_PATH ../../control)
20

  
21
#uncomment if you have defined messages
22
rosbuild_genmsg()
23
#uncomment if you have defined services
24
#rosbuild_gensrv()
25

  
26
include_directories(${CONTROL_PATH})
27

  
28
rosbuild_add_executable(${PROJECT_NAME} src/mikrokopter_node.cpp src/interface.cpp)
29
target_link_libraries(${PROJECT_NAME} serial)
30

  
31
rosbuild_add_executable(externctrl_client src/externctrl_client.cpp)
32
rosbuild_add_executable(keyboard_control src/keyboard_control.cpp src/nav_lib.cpp)
33
target_link_libraries(keyboard_control ncurses)
34
rosbuild_add_executable(joystick_control src/joystick_control.cpp src/nav_lib.cpp)
35
rosbuild_add_executable(node_control src/node_control.cpp src/nav_lib.cpp)
36
rosbuild_add_executable(CoordToPID src/CoordToPID.cpp src/nav_lib.cpp ${CONTROL_PATH}/pid_control.cpp)
37
rosbuild_add_executable(mk_wrapper src/mk_wrapper.cpp)
mikrokopter/mikrokopter/Makefile
1
include $(shell rospack find mk)/cmake.mk
mikrokopter/mikrokopter/include/mikrokopter/interface.h
1
 /*!
2
  *  \file    interface.h
3
  *  \brief   Defines the MikrokopterInterface class
4
  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
5
  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
6
  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
7
  *  \date    2011 October 25
8
  *  \details This class allows communication with and control of a Mikrokopter rotorcraft via ROS
9
  */
10

  
11
#ifndef _MIKROKOPTER_INTERFACE_H_
12
#define _MIKROKOPTER_INTERFACE_H_
13

  
14
#include <SerialStream.h>
15
using namespace LibSerial;
16

  
17
#include <string>
18
#include <ros/ros.h>
19

  
20
//Messages types:
21
//Command Output
22
#include <std_msgs/Empty.h>
23
#include <mikrokopter/PPM.h>
24
#include <mikrokopter/NcDebug.h>
25
#include <mikrokopter/FcDebug.h>
26
#include <mikrokopter/Version.h>
27
#include <mikrokopter/AnalogLabel.h>
28
#include <mikrokopter/Control.h>
29
#include <mikrokopter/Navi.h>
30
#include <mikrokopter/AngleOut.h>
31

  
32
namespace CA
33
{
34
  class MikrokopterInterface
35
  {
36
  public:
37
    MikrokopterInterface(ros::NodeHandle nh);
38
    virtual ~MikrokopterInterface();
39
    enum Address
40
    {
41
      BlankAddress  = 'a',
42
      FcAddress     = 'b',
43
      NcAddress     = 'c',
44
      Mk3MagAddress = 'd',
45
      BlCtrlAddress = 'f'
46
    };
47
  protected:
48
  
49
  
50
  private:
51
    
52
    // Member variables  
53
    SerialStream fd;
54
    std::vector<unsigned char> buffer;
55
    std::map<unsigned char, unsigned int> msg_len_table;
56
    ros::NodeHandle n;
57
    
58
    // Publishers
59
    ros::Publisher ppm_data_pub;
60
    ros::Publisher nc_debug_data_pub;
61
    ros::Publisher fc_debug_data_pub;
62
    ros::Publisher version_data_pub;
63
    ros::Publisher label_data_pub;
64
    ros::Publisher set_control_ack_pub;  
65
    ros::Publisher control_data_pub;
66
    ros::Publisher navi_data_pub;
67
    ros::Publisher angleout_data_pub;
68
    
69
    // Subscribers
70
    ros::Subscriber get_control_sub;
71
    ros::Subscriber set_control_sub;
72
    ros::Subscriber req_reset_sub;
73
    
74
    // Timers
75
    ros::Timer readSerialTimer;
76
    ros::Timer req_version_timer;
77
    ros::Timer req_analog_label_timer;
78
    ros::Timer req_nc_debug_timer;
79
    ros::Timer req_fc_debug_timer;
80
    ros::Timer req_navi_timer;
81
    ros::Timer req_external_control_timer;
82
    
83
    // Parameters
84
    double req_version_rate;
85
    int    req_version_addr;
86

  
87
    double req_analog_label_rate;
88
    int    req_analog_label_idx;
89

  
90
    double req_fc_debug_rate;
91
    double req_nc_debug_rate;
92
    double req_navi_rate;
93
    double req_reset_rate;
94
    double req_external_control_rate;
95

  
96
    //
97
    // Callback functions
98
    //
99
    void readSerialCallback(const ros::TimerEvent&);
100
    void reqVersionCallback(const ros::TimerEvent&);
101
    void reqAnalogLabelCallback(const ros::TimerEvent&);
102
    void reqNcDebugCallback(const ros::TimerEvent&);
103
    void reqFcDebugCallback(const ros::TimerEvent&);
104
    void reqNaviCallback(const ros::TimerEvent&);
105
    void reqResetCallback(const std_msgs::Empty& msg);
106
    void setControlCallback(const mikrokopter::Control::ConstPtr& msg);
107
    void getControlCallback(const ros::TimerEvent&);
108
    
109
    //
110
    // Message field extraction functions
111
    //
112
    void extract_nc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::NcDebug &debug_data);
113
    void extract_fc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::FcDebug &debug_data);
114
    void extract_navi_message(std::vector<unsigned char>& buffer, mikrokopter::Navi &msg);
115
    void extract_ppm_message(std::vector<unsigned char>& buffer, mikrokopter::PPM &message);
116
    void extract_label_message(std::vector<unsigned char>& buffer, mikrokopter::AnalogLabel &message);
117
    void extract_control_message(std::vector<unsigned char>& buffer, mikrokopter::Control &message);
118
    void extract_version_message(std::vector<unsigned char>& buffer, mikrokopter::Version &message);
119
    void extract_angleout_message(std::vector<unsigned char>& buffer, mikrokopter::AngleOut &msg);
120
    
121
    //
122
    // Message handling functions
123
    //
124
    void process_nc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);
125
    void process_fc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);
126
    void process_navi_message(std::vector<unsigned char>& buffer, ros::Time stamp);
127
    void process_ppm_message(std::vector<unsigned char>& buffer, ros::Time stamp);
128
    void process_label_message(std::vector<unsigned char>& buffer, ros::Time stamp);
129
    void process_control_message(std::vector<unsigned char>& buffer, ros::Time stamp);
130
    void process_version_message(std::vector<unsigned char>& buffer, ros::Time stamp);
131
    void process_angleout_message(std::vector<unsigned char>& buffer, ros::Time stamp);
132
    void process_external_control_ack_message(std::vector<unsigned char>& buffer, ros::Time stamp);
133

  
134
    //
135
    // Utility functions
136
    //
137
    void redirectSerialTo(MikrokopterInterface::Address addr);
138
    
139
    void decode64(std::vector<unsigned char>& in_arr, int offset, unsigned char* out_arr);
140
    int calculateChecksum(std::vector<unsigned char>& buffer);
141
    int calculateChecksum(unsigned char* buffer, int len);
142
    bool isValidChecksum(std::vector<unsigned char>& buffer);
143
    void updateWithChecksum(unsigned char* buffer, int len);
144
    void createCommand(unsigned char *commandbuffer,int &msglen,const unsigned char cmd, const unsigned char address,const unsigned char *data,int len);
145
    mikrokopter::GpsPos processGpsPos(unsigned char* buffer);
146
    mikrokopter::GpsPosDev processGpsPosDev(unsigned char* buffer);
147
    uint8_t connectedToAddress;
148
  };
149
  
150
}
151

  
152

  
153

  
154
#endif
mikrokopter/mikrokopter/launch/joystick.launch
1
<launch>
2
  <node name="joy_node" pkg="joy" type="joy_node">
3
  </node>
4
  <node name="joystick_control" pkg="mikrokopter" type="joystick_control">
5
    <!-- rate in Hz to publish Control messages -->
6
    <param name="publish_rate" value="20" />
7
  </node>
8
</launch>
9

  
10

  
mikrokopter/mikrokopter/launch/mikrokopter.launch
1
<launch>
2

  
3
  <node name="mikrokopter" pkg="mikrokopter" 	type="mikrokopter">
4
    
5
    <param name="port" value="/dev/ttyUSB0" />
6
    	
7
    <param name="req_version_rate" value="0.0" />	
8
    <param name="req_version_addr" value="97" />
9
    
10
    <param name="req_analog_label_rate" value="0.0" />
11
    <param name="req_nc_debug_rate" value="0.0" />
12
    <param name="req_fc_debug_rate" value="10.0" />
13
    <param name="req_navi_rate" value="0.0" />
14
    <param name="req_external_control_rate" value="0.0" />
15
    
16
  </node>
17
       
18
</launch>
mikrokopter/mikrokopter/launch/onboard.launch
1
<launch>
2
  <include file="$(find mikrokopter)/launch/mikrokopter.launch" />
3
  <node name="mk_wrapper" pkg="mikrokopter" type="mk_wrapper">
4
    <!-- if no Control message is received in this time in ms, go dead-->
5
    <param name="max_quiet_time" value="200" />
6
  </node>
7
</launch>
mikrokopter/mikrokopter/mainpage.dox
1
/**
2
\mainpage
3
\htmlinclude manifest.html
4

  
5
\b mikrokopter is ... 
6

  
7
<!-- 
8
Provide an overview of your package.
9
-->
10

  
11

  
12
\section codeapi Code API
13

  
14
<!--
15
Provide links to specific auto-generated API documentation within your
16
package that is of particular interest to a reader. Doxygen will
17
document pretty much every part of your code, so do your best here to
18
point the reader to the actual API.
19

  
20
If your codebase is fairly large or has different sets of APIs, you
21
should use the doxygen 'group' tag to keep these APIs together. For
22
example, the roscpp documentation has 'libros' group.
23
-->
24

  
25

  
26
*/
mikrokopter/mikrokopter/manifest.xml
1
<package>
2
  <description brief="mikrokopter">
3

  
4
     mikrokopter
5

  
6
  </description>
7
  <author>Andrew Chambers/achamber@gmail.com, Justin Haines/jhaines@andrew.cmu.edu, Sebastian Scherer/basti@andrew.cmu.edu</author>
8
  <license>GPLv3</license>
9
  <review status="unreviewed" notes=""/>
10
  <url>https://aeroscoutsim.frc.cs.cmu.edu/redmine/projects/riverine/wiki/Mikrokopter</url>
11
  <depend package="std_msgs" />
12
  <depend package="sensor_msgs" />
13
  <depend package="roscpp" />
14
  <rosdep name="libserial" />
15

  
16
</package>
17

  
18

  
mikrokopter/mikrokopter/msg/AnalogLabel.msg
1
# Labels of the analog values in the Debug Data Struct 
2
Header header
3
uint8 slaveAddr
4
uint8 index
5
string label
mikrokopter/mikrokopter/msg/AngleOut.msg
1
Header header
2

  
3
uint8 slaveAddr
4

  
5
int16[2] angle
6
uint8[2] userParameter
7
uint8 calcState
8
uint8 orientation
mikrokopter/mikrokopter/msg/Control.msg
1
# Feeds the ExternControl struct in the Mikrokopter
2
# struct str_ExternControl
3
# {
4
#  unsigned char Digital[2];
5
#  unsigned char RemoteTasten;
6
#  signed char   Nick;
7
#  signed char   Roll;
8
#  signed char   Gier;
9
#  unsigned char Gas;
10
#  signed char   Hight;
11
#  unsigned char free;
12
#  unsigned char Frame;
13
#  unsigned char Config;
14
# };
15

  
16
Header header
17

  
18
uint8[2] digital
19
uint8 remoteKey
20
int8 pitch
21
int8 roll
22
int8 yaw
23
uint8 thrust
24
int8 height
25
uint8 free
26
uint8 frame
27
uint8 config
28

  
29

  
mikrokopter/mikrokopter/msg/FcDebug.msg
1
Header header
2

  
3
float32 updateRate_hz
4
uint8 slaveAddr
5

  
6
uint8[2] status
7

  
8
int32 pitchAngle
9
int32 rollAngle
10
int32 accPitch
11
int32 accRoll
12
int32 gyroYaw
13
int32 heightValue
14
int32 accZ
15
int32 gas
16
int32 compassValue
17
int32 batteryVoltage
18

  
19
int32 receiverLevel
20
int32 gyroCompass
21
int32 engineFront
22
int32 engineBack
23
int32 engineLeft
24
int32 engineRight
25
int32 onesix
26
int32 oneseven
27
int32 oneeight
28
int32 onenine
29

  
30
int32 servo
31
int32 hovergas
32
int32 current
33
int32 capacity_mAh
34
int32 heightSetpoint
35
int32 twofive
36
int32 twosix
37
int32 compassSetpoint
38
int32 i2c_error
39
int32 bl_limit
40

  
41
int32 GPS_Pitch
42
int32 GPS_Roll
mikrokopter/mikrokopter/msg/GpsPos.msg
1
int32 longitude
2
int32 latitude
3
int32 altitude
4
uint8 status
mikrokopter/mikrokopter/msg/GpsPosDev.msg
1
uint16 distance
2
int16 bearing
mikrokopter/mikrokopter/msg/Navi.msg
1
Header header
2

  
3
float32 updateRate_hz
4
uint8 slaveAddr
5

  
6
uint8 version
7
GpsPos currentPosition
8
GpsPos targetPosition
9
GpsPosDev targetPositionDeviation
10
GpsPos homePosition
11
GpsPosDev homePositionDeviation
12
uint8 waypointIndex
13
uint8 waypointNumber
14
uint8 satsInUse
15
int16 altimeter
16

  
17
int16 variometer
18
uint16 flyingTime
19
uint8 uBat
20
uint16 groundSpeed
21
int16 heading
22
int16 compassHeading
23
int8 angleNick
24
int8 angleRoll
25
uint8 rcQuality
26
uint8 fcStatusFlags
27

  
28
uint8 ncFlags
29
uint8 errorCode
30
uint8 operatingRadius
31
int16 topSpeed
32
uint8 targetHoldTime
33
uint8 fcStatusFlags2
34
int16 setpointAltitude
35
uint8 gas
36
uint16 current
37
uint16 usedCapacity
mikrokopter/mikrokopter/msg/NcDebug.msg
1
Header header
2

  
3
float32 updateRate_hz
4
uint8 slaveAddr
5

  
6
uint8[2] status
7

  
8
int32 angleNick
9
int32 angleRoll
10
int32 accNick
11
int32 accRoll
12
int32 operatingRadius
13
int32 fcFlags
14
int32 ncFlags
15
int32 nickServo
16
int32 rollServo
17
int32 gpsData
18

  
19
int32 compassHeading
20
int32 gyroHeading
21
int32 spiError
22
int32 spiOkay
23
int32 i2cError
24
int32 i2cOkay
25
int32 poiIndex          # Kalman_K
26
int32 accSpeedN
27
int32 accSpeedE
28
int32 speedZ            # GPS ACC
29

  
30
int32 empty20
31
int32 nSpeed
32
int32 eSpeed
33
int32 empty23
34
int32 magnetX
35
int32 magnetY
36
int32 magnetZ
37
int32 distanceN
38
int32 distanceE
39
int32 gpsNick
40

  
41
int32 gpsRoll
42
int32 usedSats
43

  
mikrokopter/mikrokopter/msg/PPM.msg
1
Header header
2
int32[27] channels
mikrokopter/mikrokopter/msg/RC.msg
1
Header header
2
int16[8] channels
3
bool	 valid_signal
4

  
mikrokopter/mikrokopter/msg/Status.msg
1
Header header
2
float64 battery_volt
3
bool 	flying
4
bool 	motors_on
5
bool 	autonomous
6

  
7

  
mikrokopter/mikrokopter/msg/Thrust.msg
1
Header header
2
float64[4] thrust
mikrokopter/mikrokopter/msg/Velocity2D.msg
1
Header header
2
float64 forward
3
float64 lateral
mikrokopter/mikrokopter/msg/Version.msg
1
# // bitmask for HardwareError[0]
2
# FC_ERROR0_GYRO_NICK 	0x01
3
# FC_ERROR0_GYRO_ROLL 	0x02
4
# FC_ERROR0_GYRO_YAW 		0x04
5
# FC_ERROR0_ACC_NICK 		0x08
6
# FC_ERROR0_ACC_ROLL 		0x10
7
# FC_ERROR0_ACC_TOP  		0x20
8
# FC_ERROR0_PRESSURE		0x40
9
# FC_ERROR0_CAREFREE		0x80
10

  
11
# // bitmask for HardwareError[1]
12
# FC_ERROR1_I2C   	 	  0x01
13
# FC_ERROR1_BL_MISSING 	0x02
14
# FC_ERROR1_SPI_RX	 	  0x04
15
# FC_ERROR1_PPM	 		    0x08
16
# FC_ERROR1_MIXER			  0x10
17
# FC_ERROR1_RES1			  0x20
18
# FC_ERROR1_RES2			  0x40
19
# FC_ERROR1_RES3			  0x80
20

  
21
Header header
22
uint8 slaveAddr
23
uint8 swMajor
24
uint8 swMinor
25
uint8 protoMajor
26
uint8 protoMinor
27
uint8 swPatch
28
uint8[5] hardwareError
mikrokopter/mikrokopter/rosdep.yaml
1
libserial:
2
  ubuntu: libserial0 libserial-dev
3
  debian: libserial-dev
mikrokopter/mikrokopter/src/CoordToPID.cpp
1
#include "nav_lib.h"
2
#include <geometry_msgs/Point.h>
3
#include <math.h>
4
#include "pid_control.h"
5
#include <time.h>
6

  
7
class CoordToPID
8
{
9
public:
10
  CoordToPID();
11
  void mainLoop();
12
  void pointCallback(const geometry_msgs::Point::ConstPtr& p);
13

  
14
private:
15
  MikrokopterControl control;
16
  PID_control pidanglecontrol;
17
  PID_control pidvelcontrol;
18
  ros::Publisher pub;
19
  ros::NodeHandle n;
20
  ros::Subscriber sub;
21
  int goaldistance;
22
};
23

  
24
CoordToPID::CoordToPID():pidanglecontrol(0.4,0.01,0.4,0),pidvelcontrol(0.4,0.01,0.4,1)
25
{
26
  //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
27
  sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,
28
      &CoordToPID::pointCallback, this);
29
 
30
  //Sets Goal values for the PID	
31
  //Goal angle is 0 degrees
32
  float p = 0.4;
33
  float d = 0.01;
34
  float i = 0.4;
35
  PID_control pidanglecontrol = PID_control(p,d,i,0);
36
  
37
  //pdi values for velocity	
38
  p = 0.4;
39
  d = 0.01;
40
  i = 0.4;
41
  //Target distance to stay from target
42
  goaldistance = 100; //What units this in?
43
  PID_control pidvelcontrol = PID_control(p,d,i,goaldistance);
44
}
45

  
46
void CoordToPID::mainLoop()
47
{
48
  /*ros::Rate loop_rate(25);
49
  while(ros::ok())
50
  {
51
    ros::spinOnce();
52
    control.publish_on(pub);
53
    loop_rate.sleep();
54
  }*/
55
  control.main_loop();
56
}
57

  
58
void CoordToPID::pointCallback(const geometry_msgs::Point::ConstPtr& p)
59
{
60
  	//call on Priya's PID
61
	/* Initializes the PID controller 
62
	PID_control(int p_term, int d_term, int i_term, float goal); 
63

  
64
	Initializes the PID controller without goal term 
65
	PID_control(int p_term, int d_term, int i_term); 
66

  
67
	Reset the I term error and change the goal value 
68
	void change_goal(float goal);
69

  
70
	Given an input, get the PID controlled output 
71
	float pid(float input)*/
72

  
73
	// angle going CCW from forward in horizontal plane
74
  	float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct
75
 	 // should be from [-100, 100]
76
	float distance = sqrt(pow(p->x,2)+pow(p->y,2));
77
	//Updates pid with current value and gets pid output
78
	float pidangle = pidanglecontrol.pid(angle,(double)time(NULL));
79
	float pidvelocity = pidvelcontrol.pid(distance,(double)time(NULL));
80
    //ROS_INFO("angle %g, pidangle %g, distance %g, piddistance %g",angle,pidangle,distance,pidvelocity);	
81
	
82
    //sets pid output
83
	control.set_yaw(pidangle*100/M_PI);
84
	control.velocity_control(pidvelocity,0);
85
	//controls.set_thrust() //between [0,1]
86
}
87

  
88
int main(int argc, char **argv) {
89
  ros::init(argc, argv, "CoordToPID");
90
  CoordToPID ctp = CoordToPID();
91
  ctp.mainLoop();
92
  return 0;
93
}
mikrokopter/mikrokopter/src/externctrl_client.cpp
1
 /*!
2
  *  \file    externctrl_client.cpp
3
  *  \brief   A demonstration of the MikrokopterInterface class
4
  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
5
  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
6
  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
7
  *  \date    2011 October 25
8
  *  \details This program sends commands to a Mikrokopter rotorcraft via ROS
9
  */
10

  
11
#include "ros/ros.h"
12
#include "mikrokopter/Control.h"
13
#include <cstdlib>
14

  
15
int main(int argc, char **argv)
16
{
17
  ros::init(argc, argv, "mikrokopter_externctrl_client");
18

  
19
  ros::NodeHandle n;
20
  ros::Publisher pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
21
  
22
  mikrokopter::Control req;
23

  
24
  req.digital[0] = 0;
25
  req.digital[1] = 0;
26
  req.remoteKey = 0;
27
  req.pitch = 0;
28
  req.roll = 0;
29
  req.yaw = 0;
30
  req.thrust = 40;
31
  req.height = 0;
32
  req.free = 0;
33
  req.frame = 7;
34
  req.config = 1;
35
  
36
  ros::Rate loop_rate(25);
37
  
38
  
39
  int maxPt = 30;
40
  int minPt = -30;
41
  int step = 2;
42
  int val = 30;
43
  
44
  while (ros::ok())
45
  {
46

  
47
    if (val >= maxPt)
48
      step = -1*step;
49
    if (val <= minPt)
50
      step = -1*step;
51
  
52
    val += step;
53

  
54
    std::cout << val << std::endl;
55
  
56
    req.frame++;
57
    req.roll = val;
58

  
59
    pub.publish(req);
60

  
61
    ros::spinOnce();
62
    loop_rate.sleep();
63
  }
64

  
65
  return 0;
66
}
mikrokopter/mikrokopter/src/interface.cpp
1
 /*!
2
  *  \file    interface.cpp
3
  *  \brief   Implements the MikrokopterInterface class
4
  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
5
  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
6
  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
7
  *  \date    2011 October 25
8
  *  \details This class allows communication with and control of a Mikrokopter rotorcraft via ROS
9
  */
10

  
11
#include "mikrokopter/interface.h"
12
using namespace CA;
13

  
14
MikrokopterInterface::MikrokopterInterface(ros::NodeHandle nh):
15
    n(nh)
16
{
17
  //
18
  // Advertise all of the messages that this node might publish
19
  //
20
  ppm_data_pub = n.advertise<mikrokopter::PPM>("/mikrokopter/ppm", 100);
21
  fc_debug_data_pub = n.advertise<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100);
22
  nc_debug_data_pub = n.advertise<mikrokopter::NcDebug>("/mikrokopter/nc_debug", 100);
23
  navi_data_pub = n.advertise<mikrokopter::Navi>("/mikrokopter/navi", 100);
24
  version_data_pub = n.advertise<mikrokopter::Version>("/mikrokopter/version", 100);
25
  label_data_pub = n.advertise<mikrokopter::AnalogLabel>("/mikrokopter/analog_label", 100);
26
  set_control_ack_pub = n.advertise<mikrokopter::Control>("/mikrokopter/set_control_ack", 100);  
27
  control_data_pub = n.advertise<mikrokopter::Control>("/mikrokopter/extern_control", 100);
28
  angleout_data_pub = n.advertise<mikrokopter::AngleOut>("/mikrokopter/angle_out", 100);
29
  
30
  //
31
  // Subscribe to the external control and reset request topics
32
  //
33
  set_control_sub = n.subscribe("/mikrokopter/req_set_control", 100, &MikrokopterInterface::setControlCallback, this);
34
  req_reset_sub = n.subscribe("/mikrokopter/req_reset", 100, &MikrokopterInterface::reqResetCallback, this);
35

  
36
  //
37
  // Check to see if we should be sending periodic requests to the MK
38
  //
39
  if(n.getParam("/mikrokopter/req_version_rate",req_version_rate) && req_version_rate > 0.0)
40
  {
41
    if(!n.getParam("mikrokopter/req_version_addr",req_version_addr))
42
    {
43
      ROS_ERROR_STREAM("Must define req_version_addr");
44
      ros::shutdown();
45
    }
46
    req_version_timer = n.createTimer(ros::Duration(1/req_version_rate), &MikrokopterInterface::reqVersionCallback, this);
47
  }
48

  
49
  if(n.getParam("/mikrokopter/req_analog_label_rate",req_analog_label_rate) && req_analog_label_rate > 0.0)
50
  {
51
    req_analog_label_idx = 0;
52
    req_analog_label_timer = n.createTimer(ros::Duration(1/req_analog_label_rate), &MikrokopterInterface::reqAnalogLabelCallback, this);
53
  }
54
  
55
  if(n.getParam("/mikrokopter/req_nc_debug_rate",req_nc_debug_rate) && req_nc_debug_rate > 0.0)
56
  {
57
    req_nc_debug_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqNcDebugCallback, this);
58
  }
59

  
60
  if(n.getParam("/mikrokopter/req_fc_debug_rate",req_fc_debug_rate) && req_fc_debug_rate > 0.0)
61
  {
62
    req_fc_debug_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqFcDebugCallback, this);
63
  }
64

  
65
  if(n.getParam("/mikrokopter/req_navi_rate",req_navi_rate) && req_navi_rate > 0.0)
66
  {
67
    req_navi_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqNaviCallback, this);
68
  }
69

  
70
  if(n.getParam("/mikrokopter/req_external_control_rate",req_external_control_rate) && req_external_control_rate > 0.0)
71
  {
72
    req_external_control_timer = n.createTimer(ros::Duration(1/req_external_control_rate), &MikrokopterInterface::getControlCallback, this);
73
  }
74

  
75
  //
76
  // Set up the serial port and callback
77
  //
78
  readSerialTimer = n.createTimer(ros::Duration(0.02), &MikrokopterInterface::readSerialCallback, this);
79
  
80
  // Open serial port
81
  std::string portname;
82
  if(!n.getParam("/mikrokopter/port",portname))
83
  {
84
    ROS_ERROR_STREAM("Port not defined");
85
    ros::shutdown();
86
  }
87
	
88
  fd.Open(portname,std::ios::in|std::ios::out);
89
	
90
  if (!fd.good())
91
  {
92
    ROS_ERROR_STREAM( "Error : unable to open " << portname);
93
    ros::shutdown();
94
  }
95
  else
96
  {
97
    ROS_INFO_STREAM( "Opening port: " << portname );
98
  }
99
  
100
  fd.SetBaudRate(SerialStreamBuf::BAUD_57600);
101
  fd.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
102
  fd.SetNumOfStopBits(1);
103
  fd.SetParity(SerialStreamBuf::PARITY_NONE);
104
  fd.SetFlowControl(SerialStreamBuf::FLOW_CONTROL_NONE);//NONE
105
  
106
  //
107
  // Create a lookup table mapping from message ID to message length
108
  //
109
  msg_len_table['A'] = 30;
110
  msg_len_table['B'] = 10;
111
  msg_len_table['V'] = 22;
112
  msg_len_table['D'] = 94;
113
  msg_len_table['G'] = 22;
114
  msg_len_table['k'] = 18;
115
  msg_len_table['P'] = 78;
116
  msg_len_table['O'] = 118;
117

  
118
  redirectSerialTo(NcAddress);
119
}
120

  
121
MikrokopterInterface::~MikrokopterInterface()
122
{
123
  fd.Close();
124
}
125

  
126
void MikrokopterInterface::redirectSerialTo(MikrokopterInterface::Address addr)
127
{
128
    //
129
    // Nothing to do if we are already connected to the right board
130
    //
131
    if(connectedToAddress == addr)
132
    {
133
      return;
134
    }
135

  
136
    if (addr == FcAddress)
137
    {
138
      unsigned char commandbuffer[512];
139
      int cmdlen = 0;
140
      unsigned char board[]={0};  // uart selector for FC
141
      createCommand(commandbuffer,cmdlen,'u',NcAddress,board,1);
142
      fd.write((const char*)commandbuffer,cmdlen);  
143
      
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff