root / mikrokopter / mikrokopter / src / interface.cpp @ 98711613
History | View | Annotate | Download (27.4 KB)
1 | 07ec0a21 | Chris Burchhardt | /*!
|
---|---|---|---|
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]; |
||
369 | decode64(buffer,0,outbuffer);
|
||
370 | |||
371 | debug_data.status[0] = outbuffer[0]; |
||
372 | debug_data.status[1] = outbuffer[1]; |
||
373 | |||
374 | int val[32]; |
||
375 | for (int i=0; i<32; i++) |
||
376 | { |
||
377 | val[i] = (int16_t) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]); |
||
378 | } |
||
379 | |||
380 | debug_data.pitchAngle = val[0];
|
||
381 | debug_data.rollAngle = val[1];
|
||
382 | debug_data.accPitch = val[2];
|
||
383 | debug_data.accRoll = val[3];
|
||
384 | debug_data.gyroYaw = val[4];
|
||
385 | debug_data.heightValue = val[5];
|
||
386 | debug_data.accZ = val[6];
|
||
387 | debug_data.gas = val[7];
|
||
388 | debug_data.compassValue = val[8];
|
||
389 | debug_data.batteryVoltage = val[9];
|
||
390 | |||
391 | debug_data.receiverLevel = val[10];
|
||
392 | debug_data.gyroCompass = val[11];
|
||
393 | debug_data.engineFront = val[12];
|
||
394 | debug_data.engineBack = val[13];
|
||
395 | debug_data.engineLeft = val[14];
|
||
396 | debug_data.engineRight = val[15];
|
||
397 | debug_data.onesix = val[16];
|
||
398 | debug_data.oneseven = val[17];
|
||
399 | debug_data.oneeight = val[18];
|
||
400 | debug_data.onenine = val[19];
|
||
401 | |||
402 | debug_data.servo = val[20];
|
||
403 | debug_data.hovergas = val[21];
|
||
404 | debug_data.current = val[22];
|
||
405 | debug_data.capacity_mAh = val[23];
|
||
406 | debug_data.heightSetpoint = val[24];
|
||
407 | debug_data.twofive = val[25];
|
||
408 | debug_data.twosix = val[26];
|
||
409 | debug_data.compassSetpoint = val[27];
|
||
410 | debug_data.i2c_error = val[28];
|
||
411 | debug_data.bl_limit = val[29];
|
||
412 | |||
413 | debug_data.GPS_Pitch = val[30];
|
||
414 | debug_data.GPS_Roll = val[31];
|
||
415 | |||
416 | } |
||
417 | |||
418 | void MikrokopterInterface::extract_nc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::NcDebug &debug_data) |
||
419 | { |
||
420 | unsigned char outbuffer[512]; |
||
421 | decode64(buffer,0,outbuffer);
|
||
422 | |||
423 | debug_data.status[0] = outbuffer[0]; |
||
424 | debug_data.status[1] = outbuffer[1]; |
||
425 | |||
426 | int val[32]; |
||
427 | for (int i=0; i<32; i++){ |
||
428 | val[i] = (int16_t) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]); |
||
429 | } |
||
430 | |||
431 | debug_data.angleNick = val[0];
|
||
432 | debug_data.angleRoll = val[1];
|
||
433 | debug_data.accNick = val[2];
|
||
434 | debug_data.accRoll = val[3];
|
||
435 | debug_data.operatingRadius = val[4];
|
||
436 | debug_data.fcFlags = val[5];
|
||
437 | debug_data.ncFlags = val[6];
|
||
438 | debug_data.nickServo = val[7];
|
||
439 | debug_data.rollServo = val[8];
|
||
440 | debug_data.gpsData = val[9];
|
||
441 | |||
442 | debug_data.compassHeading = val[10];
|
||
443 | debug_data.gyroHeading = val[11];
|
||
444 | debug_data.spiError = val[12];
|
||
445 | debug_data.spiOkay = val[13];
|
||
446 | debug_data.i2cError = val[14];
|
||
447 | debug_data.i2cOkay = val[15];
|
||
448 | debug_data.poiIndex = val[16];
|
||
449 | debug_data.accSpeedN = val[17];
|
||
450 | debug_data.accSpeedE = val[18];
|
||
451 | debug_data.speedZ = val[19];
|
||
452 | |||
453 | debug_data.empty20 = val[20];
|
||
454 | debug_data.nSpeed = val[21];
|
||
455 | debug_data.eSpeed = val[22];
|
||
456 | debug_data.empty23 = val[23];
|
||
457 | debug_data.magnetX = val[24];
|
||
458 | debug_data.magnetY = val[25];
|
||
459 | debug_data.magnetZ = val[26];
|
||
460 | debug_data.distanceN = val[27];
|
||
461 | debug_data.distanceE = val[28];
|
||
462 | debug_data.gpsNick = val[29];
|
||
463 | |||
464 | debug_data.gpsRoll = val[30];
|
||
465 | debug_data.usedSats = val[31];
|
||
466 | |||
467 | } |
||
468 | |||
469 | void MikrokopterInterface::extract_navi_message(std::vector<unsigned char>& buffer, mikrokopter::Navi &msg) |
||
470 | { |
||
471 | unsigned char outbuffer[512]; |
||
472 | decode64(buffer,0,outbuffer);
|
||
473 | |||
474 | msg.version = (uint8_t)outbuffer[0];
|
||
475 | msg.currentPosition = processGpsPos(outbuffer+1);
|
||
476 | msg.targetPosition = processGpsPos(outbuffer+14);
|
||
477 | msg.targetPositionDeviation = processGpsPosDev(outbuffer+27);
|
||
478 | msg.homePosition = processGpsPos(outbuffer+31);
|
||
479 | msg.homePositionDeviation = processGpsPosDev(outbuffer+44);
|
||
480 | msg.waypointIndex = (uint8_t) outbuffer[48];
|
||
481 | msg.waypointNumber = (uint8_t) outbuffer[49];
|
||
482 | msg.satsInUse = (uint8_t) outbuffer[50];
|
||
483 | msg.altimeter = (int16_t) (outbuffer[52] << 8 | outbuffer[51]); |
||
484 | msg.variometer = (int16_t) (outbuffer[54] << 8 | outbuffer[53]); |
||
485 | msg.flyingTime = (uint16_t) (outbuffer[56] << 8 | outbuffer[55]); |
||
486 | msg.uBat = (uint8_t) outbuffer[57];
|
||
487 | msg.groundSpeed = (uint16_t) (outbuffer[59] << 8 | outbuffer[58]); |
||
488 | msg.heading = (int16_t) (outbuffer[61] << 8 | outbuffer[60]); |
||
489 | msg.compassHeading = (int16_t) (outbuffer[63] << 8 | outbuffer[62]); |
||
490 | msg.angleNick = (int8_t) outbuffer[64];
|
||
491 | msg.angleRoll = (int8_t) outbuffer[65];
|
||
492 | msg.rcQuality = (uint8_t) outbuffer[66];
|
||
493 | msg.fcStatusFlags = (uint8_t) outbuffer[67];
|
||
494 | msg.ncFlags = (uint8_t) outbuffer[68];
|
||
495 | msg.errorCode = (uint8_t) outbuffer[69];
|
||
496 | msg.operatingRadius = (uint8_t) outbuffer[70];
|
||
497 | msg.topSpeed = (int16_t) (outbuffer[72] << 8 | outbuffer[71]); |
||
498 | msg.targetHoldTime = (uint8_t) outbuffer[73];
|
||
499 | msg.fcStatusFlags2 = (uint8_t) outbuffer[74];
|
||
500 | msg.setpointAltitude = (int16_t) (outbuffer[76] << 8 | outbuffer[75]); |
||
501 | msg.gas = (uint8_t) outbuffer[77];
|
||
502 | msg.current = (uint16_t) (outbuffer[79] << 8 | outbuffer[78]); |
||
503 | msg.usedCapacity = (uint16_t) (outbuffer[81] << 8 | outbuffer[80]); |
||
504 | } |
||
505 | |||
506 | void MikrokopterInterface::process_nc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp) |
||
507 | { |
||
508 | if ( isValidChecksum(buffer) )
|
||
509 | { |
||
510 | uint8_t slaveAddr = buffer[1] - 'a'; |
||
511 | mikrokopter::NcDebug debug_data; |
||
512 | debug_data.header.frame_id="mikrokopter";
|
||
513 | debug_data.header.stamp=stamp; |
||
514 | debug_data.slaveAddr=slaveAddr - 'a';
|
||
515 | |||
516 | buffer.erase(buffer.begin(),buffer.begin()+3);
|
||
517 | extract_nc_debug_message(buffer, debug_data); |
||
518 | nc_debug_data_pub.publish(debug_data); |
||
519 | |||
520 | ROS_DEBUG_STREAM( "Published NC debug package");
|
||
521 | |||
522 | } else {
|
||
523 | ROS_WARN_STREAM( " Checksum failed for nc debug msg");
|
||
524 | } |
||
525 | } |
||
526 | void MikrokopterInterface::process_fc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp) |
||
527 | { |
||
528 | if ( isValidChecksum(buffer) )
|
||
529 | { |
||
530 | uint8_t slaveAddr = buffer[1] - 'a'; |
||
531 | mikrokopter::FcDebug debug_data; |
||
532 | debug_data.header.frame_id="mikrokopter";
|
||
533 | debug_data.header.stamp=stamp; |
||
534 | debug_data.slaveAddr=slaveAddr - 'a';
|
||
535 | |||
536 | buffer.erase(buffer.begin(),buffer.begin()+3);
|
||
537 | extract_fc_debug_message(buffer, debug_data); |
||
538 | |||
539 | fc_debug_data_pub.publish(debug_data); |
||
540 | |||
541 | ROS_DEBUG_STREAM( "Published FC debug package");
|
||
542 | |||
543 | } else {
|
||
544 | ROS_WARN_STREAM( " Checksum failed for fc debug msg");
|
||
545 | } |
||
546 | } |
||
547 | |||
548 | void MikrokopterInterface::process_navi_message(std::vector<unsigned char>& buffer, ros::Time stamp) |
||
549 | { |
||
550 | if ( buffer[1] == NcAddress && isValidChecksum(buffer) ) { |
||
551 | |||
552 | mikrokopter::Navi naviMsg; |
||
553 | naviMsg.header.frame_id="mikrokopter";
|
||
554 | naviMsg.header.stamp = stamp; |
||
555 | |||
556 | buffer.erase(buffer.begin(),buffer.begin()+3);
|
||
557 | extract_navi_message(buffer, naviMsg); |
||
558 | navi_data_pub.publish(naviMsg); |
||
559 | |||
560 | ROS_DEBUG_STREAM( "Published navi data package");
|
||
561 | |||
562 | } else {
|
||
563 | ROS_WARN_STREAM( " Checksum failed for navi data msg");
|
||
564 | } |
||
565 | } |
||
566 | |||
567 | void MikrokopterInterface::process_ppm_message(std::vector<unsigned char>& buffer, ros::Time stamp) |
||
568 | { |
||
569 | if ( buffer[1] == FcAddress && isValidChecksum(buffer) ) { |
||
570 | |||
571 | mikrokopter::PPM ppm_data; |
||
572 | ppm_data.header.frame_id="mikrokopter";
|
||
573 | ppm_data.header.stamp = stamp; |
||
574 | |||
575 | buffer.erase(buffer.begin(),buffer.begin()+3);
|
||
576 | extract_ppm_message(buffer, ppm_data); |
||
577 | ppm_data_pub.publish(ppm_data); |
||
578 | |||
579 | ROS_DEBUG_STREAM( "Published ppm package");
|
||
580 | |||
581 | } else {
|
||
582 | ROS_WARN_STREAM( " Checksum failed for ppm msg");
|
||
583 | } |
||
584 | } |
||
585 | void MikrokopterInterface::process_label_message(std::vector<unsigned char>& buffer, ros::Time stamp) |
||
586 | { |
||
587 | if ( isValidChecksum(buffer) )
|
||
588 | { |
||
589 | mikrokopter::AnalogLabel label_data; |
||
590 | label_data.header.frame_id="mikrokopter";
|
||
591 | label_data.header.stamp=stamp; |
||
592 | label_data.slaveAddr = buffer[1];
|
||
593 | |||
594 | buffer.erase(buffer.begin(),buffer.begin()+3);
|
||
595 | extract_label_message(buffer, label_data); |
||
596 | label_data_pub.publish(label_data); |
||
597 | |||
598 | ROS_DEBUG_STREAM( "Published Analog Label message");
|
||
599 | |||
600 | } else {
|
||
601 | ROS_WARN_STREAM( " Checksum failed for analog label msg");
|
||
602 | } |
||
603 | } |
||
604 | void MikrokopterInterface::process_control_message(std::vector<unsigned char>& buffer, ros::Time stamp) |
||
605 | { |
||
606 | if ( isValidChecksum(buffer) )
|
||
607 | { |
||
608 | mikrokopter::Control msg; |
||
609 | msg.header.frame_id="mikrokopter";
|
||
610 | msg.header.stamp=stamp; |
||
611 | |||
612 | buffer.erase(buffer.begin(),buffer.begin()+3);
|
||
613 | extract_control_message(buffer, msg); |
||
614 | control_data_pub.publish(msg); |
||
615 | |||
616 | ROS_DEBUG_STREAM( "Published ExternControl message");
|
||
617 | |||
618 | } else {
|
||
619 | ROS_WARN_STREAM( " Checksum failed for ExternControl msg");
|
||
620 | } |
||
621 | } |
||
622 | void MikrokopterInterface::process_version_message(std::vector<unsigned char>& buffer, ros::Time stamp) |
||
623 | { |
||
624 | if ( isValidChecksum(buffer) )
|
||
625 | { |
||
626 | mikrokopter::Version version_data; |
||
627 | version_data.header.frame_id="mikrokopter";
|
||
628 | version_data.header.stamp=stamp; |
||
629 | version_data.slaveAddr = buffer[1] - 'a'; |
||
630 | |||
631 | buffer.erase(buffer.begin(),buffer.begin()+3);
|
||
632 | extract_version_message(buffer, version_data); |
||
633 | version_data_pub.publish(version_data); |
||
634 | |||
635 | ROS_DEBUG_STREAM( "Published Version Info message");
|
||
636 | |||
637 | } else {
|
||
638 | ROS_WARN_STREAM( " Checksum failed for version info msg");
|
||
639 | } |
||
640 | } |
||
641 | void MikrokopterInterface::process_angleout_message(std::vector<unsigned char>& buffer, ros::Time stamp) |
||
642 | { |
||
643 | if ( buffer[1] == Mk3MagAddress && isValidChecksum(buffer) ) |
||
644 | { |
||
645 | mikrokopter::AngleOut msg; |
||
646 | msg.header.frame_id = "mikrokopter";
|
||
647 | msg.header.stamp = stamp; |
||
648 | msg.slaveAddr = buffer[1] - 'a'; |
||
649 | |||
650 | buffer.erase(buffer.begin(),buffer.begin()+3);
|
||
651 | extract_angleout_message(buffer, msg); |
||
652 | angleout_data_pub.publish(msg); |
||
653 | |||
654 | ROS_DEBUG_STREAM("Published Angle Out");
|
||
655 | |||
656 | } else {
|
||
657 | ROS_WARN_STREAM("Checksum failed for heading msg");
|
||
658 | } |
||
659 | } |
||
660 | |||
661 | void MikrokopterInterface::process_external_control_ack_message(std::vector<unsigned char>& buffer, ros::Time stamp) |
||
662 | { |
||
663 | if ( isValidChecksum(buffer) )
|
||
664 | { |
||
665 | mikrokopter::Control msg; |
||
666 | msg.header.frame_id="mikrokopter";
|
||
667 | msg.header.stamp=stamp; |
||
668 | |||
669 | unsigned char outbuffer[512]; |
||
670 | buffer.erase(buffer.begin(),buffer.begin()+3);
|
||
671 | decode64(buffer,0,outbuffer);
|
||
672 | msg.frame = outbuffer[0];
|
||
673 | |||
674 | set_control_ack_pub.publish(msg); |
||
675 | |||
676 | ROS_DEBUG_STREAM( "Published ExternControl ACK");
|
||
677 | |||
678 | } else {
|
||
679 | ROS_WARN_STREAM( " Checksum failed for ExternControl ACK");
|
||
680 | } |
||
681 | } |
||
682 | |||
683 | mikrokopter::GpsPos MikrokopterInterface::processGpsPos(unsigned char* buffer) |
||
684 | { |
||
685 | mikrokopter::GpsPos gps; |
||
686 | gps.longitude = (int32_t)(buffer[3] << 24 | buffer[2] << 16 | buffer[1] << 8 | buffer[0]); |
||
687 | gps.latitude = (int32_t)(buffer[7] << 24 | buffer[6] << 16 | buffer[5] << 8 | buffer[4]); |
||
688 | gps.altitude = (int32_t)(buffer[11] << 24 | buffer[10] << 16 | buffer[9] << 8 | buffer[8]); |
||
689 | gps.status = buffer[12];
|
||
690 | |||
691 | return gps;
|
||
692 | } |
||
693 | |||
694 | mikrokopter::GpsPosDev MikrokopterInterface::processGpsPosDev(unsigned char* buffer) |
||
695 | { |
||
696 | mikrokopter::GpsPosDev dev; |
||
697 | dev.distance = (uint32_t)(buffer[1] << 8 | buffer[0]); |
||
698 | dev.bearing = (int32_t)(buffer[3] << 8 | buffer[2]); |
||
699 | |||
700 | return dev;
|
||
701 | } |
||
702 | |||
703 | void MikrokopterInterface::readSerialCallback(const ros::TimerEvent&) |
||
704 | { |
||
705 | char c;
|
||
706 | static ros::Time nextStamp, stamp;
|
||
707 | |||
708 | while (fd.rdbuf()->in_avail()>0 && ros::ok()) // While there is data in the serial receive buffer |
||
709 | { |
||
710 | fd.get(c); |
||
711 | buffer.push_back(c); |
||
712 | |||
713 | if (c == '#') |
||
714 | { |
||
715 | stamp = nextStamp; |
||
716 | nextStamp = ros::Time::now(); |
||
717 | buffer.erase(buffer.begin(),buffer.end()-1);
|
||
718 | } |
||
719 | |||
720 | if ( buffer[0] == '#' && buffer.size() >= 2) |
||
721 | { |
||
722 | if( buffer.size() == msg_len_table[buffer[2]]) |
||
723 | { |
||
724 | switch (buffer[2]) |
||
725 | { |
||
726 | case 'A': |
||
727 | process_label_message(buffer,stamp); |
||
728 | break;
|
||
729 | |||
730 | case 'B': |
||
731 | process_external_control_ack_message(buffer,stamp); |
||
732 | break;
|
||
733 | |||
734 | case 'H': |
||
735 | ROS_WARN_STREAM("Unimplemented message \"H\"");
|
||
736 | break;
|
||
737 | |||
738 | case 'L': |
||
739 | ROS_WARN_STREAM("Unimplemented message \"L\"");
|
||
740 | break;
|
||
741 | |||
742 | case 'V': |
||
743 | process_version_message(buffer,stamp); |
||
744 | break;
|
||
745 | |||
746 | case 'D': |
||
747 | { |
||
748 | uint8_t slaveAddr = buffer[1];
|
||
749 | if(slaveAddr == FcAddress)
|
||
750 | { |
||
751 | process_fc_debug_message(buffer,stamp); |
||
752 | } |
||
753 | else if(slaveAddr == NcAddress) |
||
754 | { |
||
755 | process_nc_debug_message(buffer,stamp); |
||
756 | } |
||
757 | } |
||
758 | break;
|
||
759 | |||
760 | case 'G': |
||
761 | process_control_message(buffer,stamp); |
||
762 | break;
|
||
763 | |||
764 | case 'k': |
||
765 | process_angleout_message(buffer,stamp); |
||
766 | break;
|
||
767 | |||
768 | case 'P': |
||
769 | process_ppm_message(buffer,stamp); |
||
770 | break;
|
||
771 | |||
772 | case 'O': |
||
773 | process_navi_message(buffer,stamp); |
||
774 | break;
|
||
775 | |||
776 | default:
|
||
777 | ROS_WARN_STREAM("Unknown message type: "<<buffer[2]); |
||
778 | break;
|
||
779 | |||
780 | } // end switch (buffer[2])
|
||
781 | } // end if( buffer.size() == msg_len_map[buffer[2]])
|
||
782 | } // end if ( buffer[0] == '#')
|
||
783 | } // end while (fd.rdbuf()->in_avail()>0 && ros::ok())
|
||
784 | } |
||
785 | |||
786 | |||
787 | void MikrokopterInterface::decode64(std::vector<unsigned char>& in_arr, int offset, unsigned char* out_arr) { |
||
788 | int ptrIn=offset;
|
||
789 | int a,b,c,d,x,y,z;
|
||
790 | int ptr=0; |
||
791 | int len = in_arr.size();
|
||
792 | while(len!=0) |
||
793 | { |
||
794 | a = in_arr[ptrIn++] - '=';
|
||
795 | b = in_arr[ptrIn++] - '=';
|
||
796 | c = in_arr[ptrIn++] - '=';
|
||
797 | d = in_arr[ptrIn++] - '=';
|
||
798 | |||
799 | x = (a << 2) | (b >> 4); |
||
800 | y = ((b & 0x0f) << 4) | (c >> 2); |
||
801 | z = ((c & 0x03) << 6) | d; |
||
802 | |||
803 | if((len--)!=0) out_arr[ptr++] = x; else break; |
||
804 | if((len--)!=0) out_arr[ptr++] = y; else break; |
||
805 | if((len--)!=0) out_arr[ptr++] = z; else break; |
||
806 | } |
||
807 | } |
||
808 | |||
809 | int MikrokopterInterface::calculateChecksum(std::vector<unsigned char>& buffer) |
||
810 | { |
||
811 | int chk = 0; |
||
812 | unsigned int l=0; |
||
813 | for (l=0; l<buffer.size()-3; l++){ |
||
814 | chk += buffer[l]; |
||
815 | } |
||
816 | chk %= 4096;
|
||
817 | return chk;
|
||
818 | } |
||
819 | |||
820 | int MikrokopterInterface::calculateChecksum(unsigned char* buffer, int len) |
||
821 | { |
||
822 | int chk = 0; |
||
823 | int l=0; |
||
824 | for (l=0; l<len-3; l++){ |
||
825 | chk += buffer[l]; |
||
826 | } |
||
827 | chk %= 4096;
|
||
828 | return chk;
|
||
829 | } |
||
830 | |||
831 | bool MikrokopterInterface::isValidChecksum(std::vector<unsigned char>& buffer) |
||
832 | { |
||
833 | int chk = calculateChecksum(buffer);
|
||
834 | |||
835 | // Verify the Checksum
|
||
836 | int l=buffer.size()-3; |
||
837 | return (buffer[l++] == ('=' + chk / 64) && |
||
838 | buffer[l++] == ('=' + chk % 64) && |
||
839 | buffer[l++] == '\r');
|
||
840 | } |
||
841 | |||
842 | void MikrokopterInterface::updateWithChecksum(unsigned char* buffer, int len) |
||
843 | { |
||
844 | int chks = calculateChecksum(buffer, len);
|
||
845 | |||
846 | buffer[len-3] = ('=' + chks / 64); |
||
847 | buffer[len-2] = ('=' + chks % 64); |
||
848 | } |
||
849 | |||
850 | void MikrokopterInterface::createCommand(unsigned char *commandbuffer,int &msglen,const unsigned char cmd, const unsigned char address,const unsigned char *data,int len) |
||
851 | { |
||
852 | msglen = 0;
|
||
853 | commandbuffer[msglen++] = '#';
|
||
854 | commandbuffer[msglen++] = address; |
||
855 | commandbuffer[msglen++] = cmd; |
||
856 | unsigned char a,b,c; |
||
857 | a=0;b=0;c=0; |
||
858 | int ptr=0; |
||
859 | while(len)
|
||
860 | { |
||
861 | if(len>0) |
||
862 | { |
||
863 | a = data[ptr++]; |
||
864 | len--; |
||
865 | } |
||
866 | else a = 0; |
||
867 | if(len>0) |
||
868 | { |
||
869 | b = data[ptr++]; |
||
870 | len--; |
||
871 | } |
||
872 | else b = 0; |
||
873 | if(len>0) |
||
874 | { |
||
875 | c = data[ptr++]; |
||
876 | len--; |
||
877 | } |
||
878 | else c = 0; |
||
879 | commandbuffer[msglen++] = '=' + (a >> 2); |
||
880 | commandbuffer[msglen++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
||
881 | commandbuffer[msglen++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
||
882 | commandbuffer[msglen++] = '=' + ( c & 0x3f); |
||
883 | } |
||
884 | commandbuffer[msglen++] ='x';
|
||
885 | commandbuffer[msglen++] ='x';
|
||
886 | commandbuffer[msglen++] ='\r';
|
||
887 | updateWithChecksum(commandbuffer,msglen); |
||
888 | } |