Diff: MX12.cpp

Revision:

13:ccda9a56fef1

Parent:

11:9bc7f5e2ccee

Child:

14:5a0c7b30f8c0

diff -r acfd6c46954b -r ccda9a56fef1 MX12.cpp
--- a/MX12.cpp	Sat Nov 06 17:42:25 2021 +0000
+++ b/MX12.cpp	Sun Nov 07 10:36:06 2021 +0000
@@ -1,8 +1,7 @@
 /**  
  * @file MX12.ccp  
- * @brief this file will contain methods to manage au bus of servomotor 
- *        Dynaminel MX12
- * 
+ * @brief This file contains all the methods of the MX12 class 
+ *        whose prototypes are in the MX12.h header file 
  */ 
 
 #include "MX12.h"
@@ -20,18 +19,27 @@
         1                 /* stop bit */
     ); 
     // Register a callback to process a Rx (receive) interrupt.
-    _mx12.attach(callback(this, &MX12::_ReadCallback), SerialBase::RxIrq);
+    _mx12.attach(callback(this, &MX12::_Rx_interrupt), SerialBase::RxIrq);
     
     // variable used for message reception
-    _frame_pointer = 0;
-    _current_frame = { .motorId = 0, 
-                       .length = 0,
-                       .data="", 
-                       .valid=0};
-    
+    _status_pck = {.raw = "",
+                   .n_byte = 0,
+                   .servo_id = 0, 
+                   .length = 0,
+                   .error = 0,
+                   .n_param = 0,
+                   .param = "", 
+                   .received_checksum = 0,
+                   .calculated_checksum = 0,
+                   .parsed = false,
+                   .valid = false};
+                   
+    _parser_state = {.expected_field = PacketField::Header1,
+                       .byte_index = 0, 
+                       .param_index = 0};
+                       
     // Internal defaults states
-    _sstate = SerialState::Idle;
-    _pstate = ParsingState::Header;
+    _bus_state = SerialState::Idle;
     
 }
 
@@ -50,28 +58,33 @@
     data[1] = goal >> 8;
     
     // Send instruction
-    _sstate = SerialState::Writing;
+    _bus_state = SerialState::Writing;
     rw(mot_id, CONTROL_TABLE_MOVING_SPEED, 2, data);
 }
 
 char MX12::IsAvailable(void) {
-    return (_sstate == SerialState::Idle);
+    return (_bus_state == SerialState::Idle);
 }
 
 void MX12::rw(unsigned char mot_id, char address, char len, char *data) {
     
     /* Set variables for reception from servovotor */
     _answer  = 0;
-    //memset(_current_frame.data, 0, MX12_DATA_MAX_SIZE);
-    _scontext.checksum = 0;
-    _pstate = Header;
-    _scontext = {.headingCount = 0, .dataCount = 0, .checksum = 0};
-    
-    _current_frame = { .motorId = 0, 
-                       .length = 0,
-                       .data="", 
-                       .valid=0};
-    _frame_pointer = 0;
+    _status_pck = {.raw = "",
+                   .n_byte = 0,
+                   .servo_id = 0, 
+                   .length = 0,
+                   .error = 0,
+                   .n_param = 0,
+                   .param = "", 
+                   .received_checksum = 0,
+                   .calculated_checksum = 0,
+                   .parsed = false,
+                   .valid = false};
+    _parser_state = {.expected_field = PacketField::Header1,
+                     .byte_index = 0, 
+                     .param_index = 0};
+
     
     /* Initialise instruction packet to forge.
      * Instruction Packet is the command data sent to the servomotor.
@@ -174,15 +187,15 @@
 // Debug function to print Serial read
 void MX12::PrintSerial() 
 {
-    for(int i = 0; i < _frame_pointer; i++) {
-        printf("%x.", _stored_frame[i]);
+    for(int i = 0; i < _status_pck.n_byte; i++) {
+        printf("%x ", _status_pck.raw[i]);
     }
     printf("\n");
 }
  
 MX12::Status MX12::GetStatus() {
     // Return the corresponding status code
-    switch(_current_frame.data[0]) {
+    switch(_status_pck.error) {
         case 0:
             return Ok;
             break;
@@ -212,60 +225,97 @@
     }
 }
  
-void MX12::_ReadCallback() {
+void MX12::_Rx_interrupt() {
 
     char c;
     
     // Try to read serial
     if(_mx12.read(&c, 1)) {
         
-        _stored_frame[_frame_pointer++] = c;
-        
-        /* Include all characters in the checksum calculation, 
-         * even those that shouldn't, such as header characters. 
-         * They will be subtracted later (B. Denis remark)
-         */
-        _scontext.checksum += c;
+        _status_pck.raw[(_parser_state.byte_index)++] = c;
 
         // State-machine parsing
-        switch(_pstate) {
+        switch(_parser_state.expected_field) {
+            
+            /* c char is interpreted as a Header1 field */
+            case PacketField::Header1:
+            
+                /* do nothing and set next state to Header2 */
+                _parser_state.expected_field = PacketField::Header2;
+                break;
+
+            /* c char is interpreted as a Header2 field */
+            case PacketField::Header2:
+            
+                /* do nothing and set next state to Id */
+                _parser_state.expected_field = PacketField::Id;
+                break;
+                
+            /* c char is interpreted as ID field */
+            case PacketField::Id:
             
-            case Header:
-                if(++(_scontext.headingCount) >= 2) {
-                    //_scontext.headingCount = 0;
-                    _pstate = Id;
-                }
- 
-                _scontext.checksum -= c;
+                /* store ID, update checksum and set next state to Length */
+                _status_pck.servo_id = c;
+                _status_pck.calculated_checksum += c; 
+                _parser_state.expected_field = PacketField::Length;
                 break;
+
+            /* c char is interpreted as length of message data field
+             * Length = number of Parameters + 2
+             * where 2 stands for Length field (1 byte) + Error filed (1 byte)
+             */
+            case PacketField::Length:
             
-            case Id:
-                _current_frame.motorId = c;
-                _pstate = Length;
+                /* store number of param into _status_pck.n_param, 
+                 * update calculated_checksum and set next state to Error 
+                 */
+                _status_pck.n_param = c - 2;
+                _status_pck.calculated_checksum += c; 
+                _parser_state.expected_field = PacketField::Error;
                 break;
  
-            case Length:
-                _current_frame.length = c - 1;
-                _pstate = Data;
+            /* c char is interpreted as error status field */
+            case PacketField::Error:
+            
+                /* store error status, update checksum 
+                 * and set next state to Data 
+                 */
+                _status_pck.error = c;
+                _status_pck.calculated_checksum += c; 
+                _parser_state.expected_field = PacketField::Data;
                 break;
  
-            case Data:
-                _current_frame.data[_scontext.dataCount] = c;
+            /* c char is interpreted as a param field */
+            case PacketField::Data:
+
+                /* store current param, increase param_index 
+                 * and update checksum */
+                _status_pck.param[(_parser_state.param_index)++] = c;
+                _status_pck.received_checksum += c; 
                 
-                if(++(_scontext.dataCount) >= _current_frame.length) {
-                    _scontext.dataCount = 0;
-                    _pstate = Checksum;
+                /* increase param index (_parser_state.dataCount)
+                 * and test if it is the last param to read
+                 */
+                if(_parser_state.param_index > _status_pck.n_param) {
+                    /* reset param index and set next state to Checksum */
+                    _parser_state.param_index = 0;
+                    _parser_state.expected_field = PacketField::Checksum;
                 }
                 break;
             
-            case Checksum:
-                _current_frame.valid = (_scontext.checksum == 0xFF);
-                _scontext.checksum = 0;
-                _pstate = Header;
-                if(_answer) {
-                    _sstate = SerialState::Idle;
-                }
-                _answer = 1;
+            /* c char is interpreted as Checksum field */
+            case PacketField::Checksum:
+            
+                /* store received_checksum, set parsed, store n_byte,
+                 * evalutate valid and set next state to Header1 */
+                _status_pck.received_checksum = c;
+                _status_pck.parsed = true;
+                _status_pck.n_byte = _parser_state.byte_index;
+                _status_pck.valid = (_status_pck.received_checksum == c);
+                _parser_state.expected_field = PacketField::Header1;
+                
+                /* set seriel state to Idle */
+                _bus_state = SerialState::Idle;
                 break;
         }        
     }