Added BLD_SendStatus function

dhylands · Nov 19, 2008 · 03def4e · 03def4e
1 parent a1d1f79
commit 03def4e
Show file tree

Hide file tree

Showing 2 changed files with 58 additions and 3 deletions.
diff --git a/common/bioloid-pkt.c b/common/bioloid-pkt.c
@@ -109,6 +109,10 @@ void BLD_ProcessChar( BLD_Instance_t *inst, uint8_t ch )
             break;
         }
 
+        // NOTE: In the future, we should decode the SYNC_WRITE
+        //       packet so that we only need to keep the portion that
+        //       belongs to our ID
+
         case BLD_STATE_COMMAND_RCVD: // We've received the command, ch is a param byte or checksum
         {
             if (( inst->m_paramIdx + 2 ) >= inst->m_pkt.m_length )
@@ -149,6 +153,55 @@ void BLD_ProcessChar( BLD_Instance_t *inst, uint8_t ch )
 
 } // BLD_ProcessChar
 
+//***************************************************************************
+/**
+*   Sends out a status packet.
+*
+*   The format of a status packet looks like the following:
+*
+*       0xff 0xff ID Len Error [Param1 ... ParamN] ChkSum
+*/
+
+void BLD_SendStatus
+(
+    BLD_Instance_t *inst,       ///< Instance data 
+    uint8_t         errCode,    ///< Error bitmask to send
+    const void     *param,      ///< Pointer to parameter data to send. May be NULL if paramLen is zero
+    uint8_t         paramLen    ///< Number of bytes of parameter data to send
+)
+{
+    const uint8_t  *data;
+    uint8_t         checksum;
+    uint8_t         paramIdx;
+    BLD_SendChar    sendChar = inst->m_sendChar;
+
+    // Send out the preamble
+
+    sendChar( 0xff );
+    sendChar( 0xff );
+    sendChar( inst->m_id );
+    checksum = inst->m_id;
+
+    // The length sent in the packet is the number of parameter bytes + 2
+
+    sendChar( paramLen + 2 );
+    checksum += ( paramLen + 2 );
+
+    sendChar( errCode );
+    checksum += errCode;
+
+    data = param;
+    for ( paramIdx = 0; paramIdx < paramLen; paramIdx++ )
+    {
+        checksum += *data;
+        sendChar( *data++ );
+    }
+
+    // Finally send out the checksum
+
+    sendChar( checksum );
+
+} // BLD_SendStatus
 
 /** @} */
 
diff --git a/common/bioloid-pkt.h b/common/bioloid-pkt.h
@@ -71,15 +71,16 @@ struct BLD_Instance_s;
 typedef struct BLD_Instance_s BLD_Instance_t;
 
 typedef void (*BLD_PacketReceived)( BLD_Instance_t *instance, BLD_Packet_t *pkt, BLD_Error_t err );
+typedef void (*BLD_SendChar)( uint8_t ch );
 
 struct BLD_Instance_s
 {
     BLD_ID_t            m_id;       ///< Our ID on the Bioloid bus, 0xFF if we're just monitoring
     BLD_State_t         m_state;
     BLD_Length_t        m_paramIdx; 
-    BLD_Packet_t        m_pkt;
-    BLD_PacketReceived  m_pktRcvd;
-
+    BLD_Packet_t        m_pkt;      ///< Contains the packet that was actually received
+    BLD_PacketReceived  m_pktRcvd;  ///< Ptr to Fn called when a packet is successfully received
+    BLD_SendChar        m_sendChar; ///< Ptr to Fn called to send out a character
 };
 
 /*
@@ -159,6 +160,7 @@ struct BLD_Instance_s
 
 void BLD_Init( BLD_Instance_t *instance );
 void BLD_ProcessChar( BLD_Instance_t *instance, uint8_t ch );
+void BLD_SendStatus( BLD_Instance_t *instance, uint8_t error, const void *param, uint8_t paramLen );
 
 /** @} */