|
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);
|