Revision e5af41b0

View differences:

mikrokopter/CMakeLists.txt
1
cmake_minimum_required(VERSION 2.4.6)
2
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3

  
4
# Set the build type.  Options are:
5
#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
6
#  Debug          : w/ debug symbols, w/o optimization
7
#  Release        : w/o debug symbols, w/ optimization
8
#  RelWithDebInfo : w/ debug symbols, w/ optimization
9
#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
10
#set(ROS_BUILD_TYPE RelWithDebInfo)
11

  
12
rosbuild_init()
13

  
14
#set the default path for built executables to the "bin" directory
15
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
16
#set the default path for built libraries to the "lib" directory
17
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
18

  
19
#uncomment if you have defined messages
20
rosbuild_genmsg()
21
#uncomment if you have defined services
22
#rosbuild_gensrv()
23

  
24
#common commands for building c++ executables and libraries
25
rosbuild_add_executable(${PROJECT_NAME} src/mikrokopter_node.cpp src/interface.cpp)
26
target_link_libraries(${PROJECT_NAME} serial)
27

  
28
rosbuild_add_executable(externctrl_client src/externctrl_client.cpp)
mikrokopter/Makefile
1
include $(shell rospack find mk)/cmake.mk
mikrokopter/include/mikrokopter/interface.h
1
 /*!
2
  *  \file    interface.h
3
  *  \brief   Defines the MikrokopterInterface class
4
  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>
5
  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>
6
  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>
7
  *  \date    2011 October 25
8
  *  \details This class allows communication with and control of a Mikrokopter rotorcraft via ROS
9
  */
10

  
11
#ifndef _MIKROKOPTER_INTERFACE_H_
12
#define _MIKROKOPTER_INTERFACE_H_
13

  
14
#include <SerialStream.h>
15
using namespace LibSerial;
16

  
17
#include <string>
18
#include <ros/ros.h>
19

  
20
//Messages types:
21
//Command Output
22
#include <std_msgs/Empty.h>
23
#include <mikrokopter/PPM.h>
24
#include <mikrokopter/NcDebug.h>
25
#include <mikrokopter/FcDebug.h>
26
#include <mikrokopter/Version.h>
27
#include <mikrokopter/AnalogLabel.h>
28
#include <mikrokopter/Control.h>
29
#include <mikrokopter/Navi.h>
30
#include <mikrokopter/AngleOut.h>
31

  
32
namespace CA
33
{
34
  class MikrokopterInterface
35
  {
36
  public:
37
    MikrokopterInterface(ros::NodeHandle nh);
38
    virtual ~MikrokopterInterface();
39
    enum Address
40
    {
41
      BlankAddress  = 'a',
42
      FcAddress     = 'b',
43
      NcAddress     = 'c',
44
      Mk3MagAddress = 'd',
45
      BlCtrlAddress = 'f'
46
    };
47
  protected:
48
  
49
  
50
  private:
51
    
52
    // Member variables  
53
    SerialStream fd;
54
    std::vector<unsigned char> buffer;
55
    std::map<unsigned char, unsigned int> msg_len_table;
56
    ros::NodeHandle n;
57
    
58
    // Publishers
59
    ros::Publisher ppm_data_pub;
60
    ros::Publisher nc_debug_data_pub;
61
    ros::Publisher fc_debug_data_pub;
62
    ros::Publisher version_data_pub;
63
    ros::Publisher label_data_pub;
64
    ros::Publisher set_control_ack_pub;  
65
    ros::Publisher control_data_pub;
66
    ros::Publisher navi_data_pub;
67
    ros::Publisher angleout_data_pub;
68
    
69
    // Subscribers
70
    ros::Subscriber get_control_sub;
71
    ros::Subscriber set_control_sub;
72
    ros::Subscriber req_reset_sub;
73
    
74
    // Timers
75
    ros::Timer readSerialTimer;
76
    ros::Timer req_version_timer;
77
    ros::Timer req_analog_label_timer;
78
    ros::Timer req_nc_debug_timer;
79
    ros::Timer req_fc_debug_timer;
80
    ros::Timer req_navi_timer;
81
    ros::Timer req_external_control_timer;
82
    
83
    // Parameters
84
    double req_version_rate;
85
    int    req_version_addr;
86

  
87
    double req_analog_label_rate;
88
    int    req_analog_label_idx;
89

  
90
    double req_fc_debug_rate;
91
    double req_nc_debug_rate;
92
    double req_navi_rate;
93
    double req_reset_rate;
94
    double req_external_control_rate;
95

  
96
    //
97
    // Callback functions
98
    //
99
    void readSerialCallback(const ros::TimerEvent&);
100
    void reqVersionCallback(const ros::TimerEvent&);
101
    void reqAnalogLabelCallback(const ros::TimerEvent&);
102
    void reqNcDebugCallback(const ros::TimerEvent&);
103
    void reqFcDebugCallback(const ros::TimerEvent&);
104
    void reqNaviCallback(const ros::TimerEvent&);
105
    void reqResetCallback(const std_msgs::Empty& msg);
106
    void setControlCallback(const mikrokopter::Control::ConstPtr& msg);
107
    void getControlCallback(const ros::TimerEvent&);
108
    
109
    //
110
    // Message field extraction functions
111
    //
112
    void extract_nc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::NcDebug &debug_data);
113
    void extract_fc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::FcDebug &debug_data);
114
    void extract_navi_message(std::vector<unsigned char>& buffer, mikrokopter::Navi &msg);
115
    void extract_ppm_message(std::vector<unsigned char>& buffer, mikrokopter::PPM &message);
116
    void extract_label_message(std::vector<unsigned char>& buffer, mikrokopter::AnalogLabel &message);
117
    void extract_control_message(std::vector<unsigned char>& buffer, mikrokopter::Control &message);
118
    void extract_version_message(std::vector<unsigned char>& buffer, mikrokopter::Version &message);
119
    void extract_angleout_message(std::vector<unsigned char>& buffer, mikrokopter::AngleOut &msg);
120
    
121
    //
122
    // Message handling functions
123
    //
124
    void process_nc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);
125
    void process_fc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);
126
    void process_navi_message(std::vector<unsigned char>& buffer, ros::Time stamp);
127
    void process_ppm_message(std::vector<unsigned char>& buffer, ros::Time stamp);
128
    void process_label_message(std::vector<unsigned char>& buffer, ros::Time stamp);
129
    void process_control_message(std::vector<unsigned char>& buffer, ros::Time stamp);
130
    void process_version_message(std::vector<unsigned char>& buffer, ros::Time stamp);
131
    void process_angleout_message(std::vector<unsigned char>& buffer, ros::Time stamp);
132
    void process_external_control_ack_message(std::vector<unsigned char>& buffer, ros::Time stamp);
133

  
134
    //
135
    // Utility functions
136
    //
137
    void redirectSerialTo(MikrokopterInterface::Address addr);
138
    
139
    void decode64(std::vector<unsigned char>& in_arr, int offset, unsigned char* out_arr);
140
    int calculateChecksum(std::vector<unsigned char>& buffer);
141
    int calculateChecksum(unsigned char* buffer, int len);
142
    bool isValidChecksum(std::vector<unsigned char>& buffer);
143
    void updateWithChecksum(unsigned char* buffer, int len);
144
    void createCommand(unsigned char *commandbuffer,int &msglen,const unsigned char cmd, const unsigned char address,const unsigned char *data,int len);
145
    mikrokopter::GpsPos processGpsPos(unsigned char* buffer);
146
    mikrokopter::GpsPosDev processGpsPosDev(unsigned char* buffer);
147
    uint8_t connectedToAddress;
148
  };
149
  
150
}
151

  
152

  
153

  
154
#endif
mikrokopter/launch/mikrokopter.launch
1
<launch>
2

  
3
  <node name="mikrokopter" pkg="mikrokopter" 	type="mikrokopter">
4
    
5
    <param name="port" value="/dev/ttyUSB0" />
6
    	
7
    <param name="req_version_rate" value="0.0" />	
8
    <param name="req_version_addr" value="97" />
9
    
10
    <param name="req_analog_label_rate" value="0.0" />
11
    <param name="req_nc_debug_rate" value="0.0" />
12
    <param name="req_fc_debug_rate" value="10.0" />
13
    <param name="req_navi_rate" value="0.0" />
14
    <param name="req_external_control_rate" value="0.0" />
15
    
16
  </node>
17
       
18
</launch>
mikrokopter/mainpage.dox
1
/**
2
\mainpage
3
\htmlinclude manifest.html
4

  
5
\b mikrokopter is ... 
6

  
7
<!-- 
8
Provide an overview of your package.
9
-->
10

  
11

  
12
\section codeapi Code API
13

  
14
<!--
15
Provide links to specific auto-generated API documentation within your
16
package that is of particular interest to a reader. Doxygen will
17
document pretty much every part of your code, so do your best here to
18
point the reader to the actual API.
19

  
20
If your codebase is fairly large or has different sets of APIs, you
21
should use the doxygen 'group' tag to keep these APIs together. For
22
example, the roscpp documentation has 'libros' group.
23
-->
24

  
25

  
26
*/
mikrokopter/manifest.xml
1
<package>
2
  <description brief="mikrokopter">
3

  
4
     mikrokopter
5

  
6
  </description>
7
  <author>Andrew Chambers/achamber@gmail.com, Justin Haines/jhaines@andrew.cmu.edu, Sebastian Scherer/basti@andrew.cmu.edu</author>
8
  <license>GPLv3</license>
9
  <review status="unreviewed" notes=""/>
10
  <url>https://aeroscoutsim.frc.cs.cmu.edu/redmine/projects/riverine/wiki/Mikrokopter</url>
11
  <depend package="std_msgs" />
12
  <depend package="roscpp" />
13
  <rosdep name="libserial" />
14

  
15
</package>
16

  
17

  
mikrokopter/msg/AnalogLabel.msg
1
# Labels of the analog values in the Debug Data Struct 
2
Header header
3
uint8 slaveAddr
4
uint8 index
5
string label
mikrokopter/msg/AngleOut.msg
1
Header header
2

  
3
uint8 slaveAddr
4

  
5
int16[2] angle
6
uint8[2] userParameter
7
uint8 calcState
8
uint8 orientation
mikrokopter/msg/Control.msg
1
# Feeds the ExternControl struct in the Mikrokopter
2
# struct str_ExternControl
3
# {
4
#  unsigned char Digital[2];
5
#  unsigned char RemoteTasten;
6
#  signed char   Nick;
7
#  signed char   Roll;
8
#  signed char   Gier;
9
#  unsigned char Gas;
10
#  signed char   Hight;
11
#  unsigned char free;
12
#  unsigned char Frame;
13
#  unsigned char Config;
14
# };
15

  
16
Header header
17

  
18
uint8[2] digital
19
uint8 remoteKey
20
int8 pitch
21
int8 roll
22
int8 yaw
23
uint8 thrust
24
int8 height
25
uint8 free
26
uint8 frame
27
uint8 config
28

  
29

  
mikrokopter/msg/FcDebug.msg
1
Header header
2

  
3
float32 updateRate_hz
4
uint8 slaveAddr
5

  
6
uint8[2] status
7

  
8
int32 pitchAngle
9
int32 rollAngle
10
int32 accPitch
11
int32 accRoll
12
int32 gyroYaw
13
int32 heightValue
14
int32 accZ
15
int32 gas
16
int32 compassValue
17
int32 batteryVoltage
18

  
19
int32 receiverLevel
20
int32 gyroCompass
21
int32 engineFront
22
int32 engineBack
23
int32 engineLeft
24
int32 engineRight
25
int32 onesix
26
int32 oneseven
27
int32 oneeight
28
int32 onenine
29

  
30
int32 servo
31
int32 hovergas
32
int32 current
33
int32 capacity_mAh
34
int32 heightSetpoint
35
int32 twofive
36
int32 twosix
37
int32 compassSetpoint
38
int32 i2c_error
39
int32 bl_limit
40

  
41
int32 GPS_Pitch
42
int32 GPS_Roll
mikrokopter/msg/GpsPos.msg
1
int32 longitude
2
int32 latitude
3
int32 altitude
4
uint8 status
mikrokopter/msg/GpsPosDev.msg
1
uint16 distance
2
int16 bearing
mikrokopter/msg/Navi.msg
1
Header header
2

  
3
float32 updateRate_hz
4
uint8 slaveAddr
5

  
6
uint8 version
7
GpsPos currentPosition
8
GpsPos targetPosition
9
GpsPosDev targetPositionDeviation
10
GpsPos homePosition
11
GpsPosDev homePositionDeviation
12
uint8 waypointIndex
13
uint8 waypointNumber
14
uint8 satsInUse
15
int16 altimeter
16

  
17
int16 variometer
18
uint16 flyingTime
19
uint8 uBat
20
uint16 groundSpeed
21
int16 heading
22
int16 compassHeading
23
int8 angleNick
24
int8 angleRoll
25
uint8 rcQuality
26
uint8 fcStatusFlags
27

  
28
uint8 ncFlags
29
uint8 errorCode
30
uint8 operatingRadius
31
int16 topSpeed
32
uint8 targetHoldTime
33
uint8 fcStatusFlags2
34
int16 setpointAltitude
35
uint8 gas
36
uint16 current
37
uint16 usedCapacity
mikrokopter/msg/NcDebug.msg
1
Header header
2

  
3
float32 updateRate_hz
4
uint8 slaveAddr
5

  
6
uint8[2] status
7

  
8
int32 angleNick
9
int32 angleRoll
10
int32 accNick
11
int32 accRoll
12
int32 operatingRadius
13
int32 fcFlags
14
int32 ncFlags
15
int32 nickServo
16
int32 rollServo
17
int32 gpsData
18

  
19
int32 compassHeading
20
int32 gyroHeading
21
int32 spiError
22
int32 spiOkay
23
int32 i2cError
24
int32 i2cOkay
25
int32 poiIndex          # Kalman_K
26
int32 accSpeedN
27
int32 accSpeedE
28
int32 speedZ            # GPS ACC
29

  
30
int32 empty20
31
int32 nSpeed
32
int32 eSpeed
33
int32 empty23
34
int32 magnetX
35
int32 magnetY
36
int32 magnetZ
37
int32 distanceN
38
int32 distanceE
39
int32 gpsNick
40

  
41
int32 gpsRoll
42
int32 usedSats
43

  
mikrokopter/msg/PPM.msg
1
Header header
2
int32[27] channels
mikrokopter/msg/RC.msg
1
Header header
2
int16[8] channels
3
bool	 valid_signal
4

  
mikrokopter/msg/Status.msg
1
Header header
2
float64 battery_volt
3
bool 	flying
4
bool 	motors_on
5
bool 	autonomous
6

  
7

  
mikrokopter/msg/Thrust.msg
1
Header header
2
float64[4] thrust
mikrokopter/msg/Version.msg
1
# // bitmask for HardwareError[0]
2
# FC_ERROR0_GYRO_NICK 	0x01
3
# FC_ERROR0_GYRO_ROLL 	0x02
4
# FC_ERROR0_GYRO_YAW 		0x04
5
# FC_ERROR0_ACC_NICK 		0x08
6
# FC_ERROR0_ACC_ROLL 		0x10
7
# FC_ERROR0_ACC_TOP  		0x20
8
# FC_ERROR0_PRESSURE		0x40
9
# FC_ERROR0_CAREFREE		0x80
10

  
11
# // bitmask for HardwareError[1]
12
# FC_ERROR1_I2C   	 	  0x01
13
# FC_ERROR1_BL_MISSING 	0x02
14
# FC_ERROR1_SPI_RX	 	  0x04
15
# FC_ERROR1_PPM	 		    0x08
16
# FC_ERROR1_MIXER			  0x10
17
# FC_ERROR1_RES1			  0x20
18
# FC_ERROR1_RES2			  0x40
19
# FC_ERROR1_RES3			  0x80
20

  
21
Header header
22
uint8 slaveAddr
23
uint8 swMajor
24
uint8 swMinor
25
uint8 protoMajor
26
uint8 protoMinor
27
uint8 swPatch
28
uint8[5] hardwareError
mikrokopter/src/externctrl_client.cpp
1
 /*!
2
  *  \file    externctrl_client.cpp
3
  *  \brief   A demonstration of 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 program sends commands to a Mikrokopter rotorcraft via ROS
9
  */
10

  
11
#include "ros/ros.h"
12
#include "mikrokopter/Control.h"
13
#include <cstdlib>
14

  
15
int main(int argc, char **argv)
16
{
17
  ros::init(argc, argv, "mikrokopter_externctrl_client");
18

  
19
  ros::NodeHandle n;
20
  ros::Publisher pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
21
  
22
  mikrokopter::Control req;
23

  
24
  req.digital[0] = 0;
25
  req.digital[1] = 0;
26
  req.remoteKey = 0;
27
  req.pitch = 0;
28
  req.roll = 0;
29
  req.yaw = 0;
30
  req.thrust = 40;
31
  req.height = 0;
32
  req.free = 0;
33
  req.frame = 7;
34
  req.config = 1;
35
  
36
  ros::Rate loop_rate(25);
37
  
38
  
39
  int maxPt = 30;
40
  int minPt = -30;
41
  int step = 2;
42
  int val = 30;
43
  
44
  while (ros::ok())
45
  {
46

  
47
    if (val >= maxPt)
48
      step = -1*step;
49
    if (val <= minPt)
50
      step = -1*step;
51
  
52
    val += step;
53

  
54
    std::cout << val << std::endl;
55
  
56
    req.frame++;
57
    req.roll = val;
58

  
59
    pub.publish(req);
60

  
61
    ros::spinOnce();
62
    loop_rate.sleep();
63
  }
64

  
65
  return 0;
66
}
mikrokopter/src/interface.cpp
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);
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff