root / mikrokopter / mikrokopter / include / mikrokopter / interface.h @ 98711613
History | View | Annotate | Download (5.33 KB)
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
|