Statistics
| Branch: | Revision:

root / quad2 / mikrokopter / include / mikrokopter / interface.h @ 98e35b23

History | View | Annotate | Download (5.33 KB)

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