Statistics
| Branch: | Revision:

root / mikrokopter / mikrokopter / src / interface.cpp @ 98711613

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
}