root / mikrokopter / mikrokopter / include / mikrokopter / interface.h @ 98711613
History | View | Annotate | Download (5.33 KB)
1 | 07ec0a21 | Chris Burchhardt | /*!
|
---|---|---|---|
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 |