Subversion Repositories Projects

Rev

Rev 211 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
211 dhylands 1
/****************************************************************************
2
*
3
*   Copyright (c) 2009 Dave Hylands     <dhylands@gmail.com>
4
*
5
*   This program is free software; you can redistribute it and/or modify
6
*   it under the terms of the GNU General Public License version 2 as
7
*   published by the Free Software Foundation.
8
*
9
*   Alternatively, this software may be distributed under the terms of BSD
10
*   license.
11
*
12
*   See README and COPYING for more details.
13
*
14
****************************************************************************/
15
/**
16
*
17
*   @file   BioloidPacket.cpp
18
*
19
*   @brief  This file implements the BioloidPacket class, which is used
20
*           to parse incoming byte streams into packets.
21
*
22
****************************************************************************/
23
 
24
// ---- Include Files -------------------------------------------------------
25
 
26
#include "Log.h"
27
#include "BioloidPacket.h"
28
 
29
// ---- Public Variables ----------------------------------------------------
30
// ---- Private Constants and Types -----------------------------------------
31
// ---- Private Variables ---------------------------------------------------
32
// ---- Private Function Prototypes -----------------------------------------
33
// ---- Functions -----------------------------------------------------------
34
 
35
/**
36
 * @addtogroup bioloid
37
 * @{
38
 */
39
 
40
//***************************************************************************
41
/**
42
*   Constructor
43
*/
44
 
45
BioloidPacket::BioloidPacket()
213 dhylands 46
    : m_state( BioloidPacket::STATE_IDLE ),
47
      m_param( NULL ),
48
      m_maxParam( 0 )
211 dhylands 49
{
50
}
51
 
52
//***************************************************************************
53
/**
213 dhylands 54
*   Constructor where storage for parameter data is specified.
55
*/
56
 
57
BioloidPacket::BioloidPacket( void *data, uint8_t maxData )
58
    : m_state( BioloidPacket::STATE_IDLE ),
59
      m_param( static_cast< uint8_t * >( data )),
60
      m_maxParam( maxData )
61
{
62
}
63
 
64
//***************************************************************************
65
/**
211 dhylands 66
*   Destructor
67
*
68
*   virtual
69
*/
70
 
71
BioloidPacket::~BioloidPacket()
72
{
73
}
74
 
75
//***************************************************************************
76
/**
77
*   Runs a single character through the state machine. Once a packet
78
*   has been parsed successfully, the PacketReceived virtual method
79
*   is called.
80
*
81
*   virtual
82
*/
83
 
213 dhylands 84
Bioloid::Error BioloidPacket::ProcessChar( uint8_t ch )
211 dhylands 85
{
213 dhylands 86
    State           nextState = m_state;
87
    Bioloid::Error  err = Bioloid::ERROR_NOT_DONE;
211 dhylands 88
 
89
    switch ( nextState )
90
    {
91
        case STATE_IDLE:    // We're waiting for the beginning of the packet (0xFF)
92
        {
93
            if ( ch == 0xFF )
94
            {
95
                nextState = STATE_1ST_FF_RCVD;
96
            }
97
            break;
98
        }
99
 
100
        case STATE_1ST_FF_RCVD:  // We've received the 1st 0xFF
101
        {
102
            if ( ch == 0xFF )
103
            {
104
                nextState = STATE_2ND_FF_RCVD;
105
            }
106
            else
107
            {
108
                nextState = STATE_IDLE;
109
            }
110
            break;
111
        }
112
 
113
        case STATE_2ND_FF_RCVD:  // We've received the 2nd 0xFF, ch is the ID
114
        {
115
            if ( ch == 0xFF )
116
            {
117
                // 0xFF is invalid as an ID, so just stay in this state until we receive
118
                // a non-0xFF
119
 
120
                nextState = STATE_2ND_FF_RCVD;
121
                break;
122
            }
123
            m_id = ch;
124
            m_checksum = ch;
125
            nextState = STATE_ID_RCVD;
126
            break;
127
        }
128
 
129
        case STATE_ID_RCVD:      // We've received the ID, ch is the length
130
        {
131
            m_length = ch;
132
            m_checksum += ch;
133
            nextState = STATE_LENGTH_RCVD;
134
            break;
135
        }
136
 
137
        case STATE_LENGTH_RCVD:  // We've received the length, ch is the command
138
        {
139
            m_cmd = ch;
140
            m_checksum += ch;
141
            m_paramIdx = 0;
142
            nextState = STATE_COMMAND_RCVD;
143
            break;
144
        }
145
 
146
        // NOTE: In the future, we should decode the SYNC_WRITE
147
        //       packet so that we only need to keep the portion that
148
        //       belongs to our ID
149
 
150
        case STATE_COMMAND_RCVD: // We've received the command, ch is a param byte or checksum
151
        {
152
            if (( m_paramIdx + 2 ) >= m_length )
153
            {
154
                // ch is the Checksum
155
 
156
                m_checksum = ~m_checksum;
157
 
213 dhylands 158
                if ( m_checksum == ch )
211 dhylands 159
                {
213 dhylands 160
                    if ( m_paramIdx <= m_maxParam )
161
                    {
162
                        err = Bioloid::ERROR_NONE;
163
                    }
164
                    else
165
                    {
166
                        err = Bioloid::ERROR_TOO_MUCH_DATA;
167
                    }
168
                }
169
                else
170
                {
211 dhylands 171
                    // CRC failed
172
 
173
                    err = Bioloid::ERROR_CHECKSUM;
174
                    LogError( "Rcvd Checksum: 0x%02x Expecting: 0x%02x\n",
175
                              ch, m_checksum );
176
                }
177
                nextState = STATE_IDLE;
178
                break;
179
            }
180
 
213 dhylands 181
            m_checksum += ch;
182
            if ( m_paramIdx < m_maxParam )
211 dhylands 183
            {
184
                m_param[ m_paramIdx ] = ch;
185
            }
186
            m_paramIdx++;
187
            break;
188
        }
189
    }
190
 
191
    m_state = nextState;
213 dhylands 192
 
193
    return err;
211 dhylands 194
}
195