root / mikrokopter / src / interface.cpp @ e5af41b0
History | View | Annotate | Download (27.4 KB)
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]; |
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 |
} |