Revision 98711613
ID | 987116130cae8b17f511c8da908bf6e1780c956c |
Moved mikrokopter package one folder deeper
The packages arduino and altitude_node aren't found inside another
package directory, so this way they're both found and the repo is a
little more organized.
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]; |
Also available in: Unified diff