Revision 98711613

View differences:

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
#uncomment if you have defined messages
20
rosbuild_genmsg()
21
#uncomment if you have defined services
22
#rosbuild_gensrv()
23

  
24
rosbuild_add_executable(${PROJECT_NAME} src/mikrokopter_node.cpp src/interface.cpp)
25
target_link_libraries(${PROJECT_NAME} serial)
26

  
27
rosbuild_add_executable(externctrl_client src/externctrl_client.cpp)
28
rosbuild_add_executable(keyboard_control src/keyboard_control.cpp src/nav_lib.cpp)
29
target_link_libraries(keyboard_control ncurses)
30
rosbuild_add_executable(joystick_control src/joystick_control.cpp src/nav_lib.cpp)
31
rosbuild_add_executable(node_control src/node_control.cpp src/nav_lib.cpp)
32
rosbuild_add_executable(CoordToPID src/CoordToPID.cpp src/nav_lib.cpp ../control/pid_control.cpp)
33
rosbuild_add_executable(mk_wrapper src/mk_wrapper.cpp)
mikrokopter/Makefile
1
include $(shell rospack find mk)/cmake.mk
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/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/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/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/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/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/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
      
144
    } else if (addr == Mk3MagAddress)
145
    {
146
      unsigned char commandbuffer[512];
147
      int cmdlen = 0;
148
      unsigned char board[]={1};  // uart selector for MK3MAG
149
      createCommand(commandbuffer,cmdlen,'u',NcAddress,board,1);
150
      fd.write((const char*)commandbuffer,cmdlen); 
151
    }
152
    /*else if (addr == MkGpsAddress)
153
    {
154
      unsigned char commandbuffer[512];
155
      int cmdlen = 0;
156
      unsigned char board[]={2};  // uart selector for MKGPS
157
      createCommand(commandbuffer,cmdlen,'u',NC_ADDRESS,board,1);
158
      fd.write((const char*)commandbuffer,cmdlen); 
159
    }*/
160
    else // By default connect to NaviCtrl Bd
161
    {
162
      unsigned char commandbuffer[] = {0x1B,0x1B,0x55,0xAA,0x00};  // Magic packet defined by Mikrokopter serial protocol
163
      int cmdlen = 5;
164
      fd.write((const char*)commandbuffer,cmdlen);
165
    }
166
  
167
  connectedToAddress = addr;
168
  
169
  return;
170

  
171
}
172

  
173
void MikrokopterInterface::reqResetCallback(const std_msgs::Empty& msg)
174
{
175
  int cmdlen;
176
  unsigned char commandbuffer[512];  
177
  createCommand(commandbuffer,cmdlen,'R',BlankAddress,NULL,0);
178
  fd.write((const char*)commandbuffer,cmdlen);
179
  ROS_DEBUG_STREAM("Sent Reset Request to MK");
180
}
181

  
182
void MikrokopterInterface::reqVersionCallback(const ros::TimerEvent&)
183
{
184
  int cmdlen;
185
  unsigned char commandbuffer[512];  
186
  createCommand(commandbuffer,cmdlen,'v',req_version_addr,NULL,0);
187
  fd.write((const char*)commandbuffer,cmdlen);
188
  ROS_DEBUG_STREAM("Sent Version Request to MK");
189
}
190

  
191
void MikrokopterInterface::reqAnalogLabelCallback(const ros::TimerEvent&)
192
{
193
  unsigned char index[1];
194
  index[0] = req_analog_label_idx;
195
  
196
  int cmdlen;
197
  unsigned char commandbuffer[512];
198
  createCommand(commandbuffer,cmdlen,'a',BlankAddress,index,1);
199
  fd.write((const char*)commandbuffer,cmdlen);
200

  
201
  req_analog_label_idx++;
202
  req_analog_label_idx %= 32;
203

  
204
  ROS_DEBUG_STREAM("Sent Analog Label Request to MK");
205
}
206

  
207
void MikrokopterInterface::reqNcDebugCallback(const ros::TimerEvent&)
208
{
209
  redirectSerialTo(NcAddress);
210
    
211
  // Debug Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds. 
212
  // Subsciption needs to be renewed every 4s.
213
  unsigned char rate[1];
214
  rate[0] = (unsigned char) 100.0/(req_nc_debug_rate);
215
  
216
  int cmdlen;
217
  unsigned char commandbuffer[512];
218
  createCommand(commandbuffer,cmdlen,'d',NcAddress,rate,1);
219
  fd.write((const char*)commandbuffer,cmdlen);
220

  
221
  ROS_DEBUG_STREAM("Sent NC Debug Request to MK");
222
}
223

  
224
void MikrokopterInterface::reqFcDebugCallback(const ros::TimerEvent&)
225
{
226
  redirectSerialTo(FcAddress);
227
      
228
  // Debug Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds. 
229
  // Subsciption needs to be renewed every 4s.
230
  unsigned char rate[1];
231
  rate[0] = (unsigned char) 100.0/(req_fc_debug_rate);
232
    
233
  int cmdlen;
234
  unsigned char commandbuffer[512];
235
  createCommand(commandbuffer,cmdlen,'d',FcAddress,rate,1);
236
  fd.write((const char*)commandbuffer,cmdlen);
237

  
238
  ROS_DEBUG_STREAM("Sent FC Debug Request to MK");
239
}
240

  
241
void MikrokopterInterface::reqNaviCallback(const ros::TimerEvent&)
242
{
243
  redirectSerialTo(NcAddress);
244
    
245
  // Navi Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds.
246
  unsigned char rate[1];
247
  rate[0] = (unsigned char) 100.0/(req_navi_rate);
248
  int cmdlen;
249
  unsigned char commandbuffer[512];
250
  createCommand(commandbuffer,cmdlen,'o',NcAddress,rate,1);
251
  fd.write((const char*)commandbuffer,cmdlen);
252

  
253
  ROS_DEBUG_STREAM("Sent Navi Request to MK");
254
  
255
}
256

  
257
void MikrokopterInterface::setControlCallback(const mikrokopter::Control::ConstPtr& msg)
258
{
259
  redirectSerialTo(FcAddress);
260

  
261
  unsigned char dataBytes[11];
262
	    
263
  dataBytes[0] = msg->digital[0];
264
  dataBytes[1] = msg->digital[1];
265
  dataBytes[2] = msg->remoteKey;
266
  dataBytes[3] = msg->pitch;
267
  dataBytes[4] = msg->roll;
268
  dataBytes[5] = msg->yaw;
269
  dataBytes[6] = msg->thrust;
270
  dataBytes[7] = msg->height;
271
  dataBytes[8] = msg->free;
272
  dataBytes[9] = msg->frame;
273
  dataBytes[10] = msg->config;
274
  
275
  int cmdlen;
276
  unsigned char commandbuffer[512];
277
  createCommand(commandbuffer,cmdlen,'b',FcAddress,dataBytes,11);
278
  fd.write((const char*)commandbuffer,cmdlen);
279
  ROS_DEBUG_STREAM("Sent Set Control Request to MK");
280
}
281

  
282
void MikrokopterInterface::getControlCallback(const ros::TimerEvent&)
283
{
284
  int cmdlen;
285
  unsigned char commandbuffer[512];
286
  createCommand(commandbuffer,cmdlen,'g',FcAddress,NULL,0);
287
  fd.write((const char*)commandbuffer,cmdlen);
288
  ROS_DEBUG_STREAM("Sent Get Control Request to MK");
289
}
290

  
291
void MikrokopterInterface::extract_version_message(std::vector<unsigned char>& buffer, mikrokopter::Version &message)
292
{
293
  unsigned char outbuffer[512];
294
  decode64(buffer,0,outbuffer);
295
  
296
  message.swMajor     = outbuffer[0];
297
  message.swMinor     = outbuffer[1];
298
  message.protoMajor  = outbuffer[2];
299
  message.protoMinor  = outbuffer[3];
300
  message.swPatch     = outbuffer[4];
301
  
302
  for (int i = 0; i < 5; i++)
303
  {
304
    message.hardwareError[i] = outbuffer[i+5];
305
  }
306
  
307
}
308

  
309
void MikrokopterInterface::extract_control_message(std::vector<unsigned char>& buffer, mikrokopter::Control &message)
310
{
311
  unsigned char outbuffer[512];
312
  decode64(buffer,0,outbuffer);
313
  
314
  message.digital[0]    = outbuffer[0];
315
  message.digital[1]    = outbuffer[1];
316
  message.remoteKey     = outbuffer[2];
317
  message.pitch         = (signed int)outbuffer[3];
318
  message.roll          = (signed int)outbuffer[4];
319
  message.yaw           = (signed int)outbuffer[5];  
320
  message.thrust        = outbuffer[6];
321
  message.height        = (signed int)outbuffer[7];
322
  message.free          = outbuffer[8];
323
  message.frame         = outbuffer[9];
324
  message.config        = outbuffer[10];
325
  
326
}
327

  
328
void MikrokopterInterface::extract_angleout_message(std::vector<unsigned char>& buffer, mikrokopter::AngleOut &msg)
329
{
330
  unsigned char outbuffer[512];
331
  decode64(buffer,0,outbuffer);
332
  
333
  msg.angle[0] = (int16_t) (outbuffer[1] << 8 | outbuffer[0]);
334
  msg.angle[1] = (int16_t) (outbuffer[3] << 8 | outbuffer[2]);
335
  msg.userParameter[0] = (uint8_t) outbuffer[4];
336
  msg.userParameter[1] = (uint8_t) outbuffer[5];
337
  msg.calcState = (uint8_t) outbuffer[6];
338
  msg.orientation = (uint8_t) outbuffer[7];  
339
}
340

  
341
void MikrokopterInterface::extract_label_message(std::vector<unsigned char>& buffer, mikrokopter::AnalogLabel &message)
342
{
343
  unsigned char outbuffer[512];
344
  decode64(buffer,0,outbuffer);
345
  
346
  message.index     = outbuffer[0];
347
 
348
  for (int i = 0; i < 16; i++)
349
    {
350
      message.label += outbuffer[i+1];
351
    }
352
}
353

  
354

  
355
void MikrokopterInterface::extract_ppm_message(std::vector<unsigned char>& buffer, mikrokopter::PPM &message)
356
{
357
  unsigned char outbuffer[512];
358
  decode64(buffer,0,outbuffer);
359
  for (int i=0; i<27; i++)
360
  {
361
    message.channels[i] = (signed int) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]);
362
  }
363
}
364

  
365

  
366
void MikrokopterInterface::extract_fc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::FcDebug &debug_data)
367
{
368
  unsigned char outbuffer[512];
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff