WebSVN - NaviCtrl - Diff - Rev 244 and 245

 // +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#include <string.h>
 #include "91x_lib.h"
 #include "uart1.h"
 #include "ubx.h"
 #include "led.h"
 #include "timer1.h"
 #define LEAP_SECONDS_FROM_1980  15
 // message sync bytes
 #define UBX_SYNC1_CHAR  0xB5
 #define UBX_SYNC2_CHAR  0x62
+// protocoll identifiers
+#define UBX_CLASS_UNDEF 0x00
-// protocoll identifier
+// navigation class
-#define UBX_CLASS_NAV   0x01
-// message id
+#define UBX_CLASS_NAV   0x01
 #define UBX_ID_POSLLH   0x02
 #define UBX_ID_SOL              0x06
-#define UBX_ID_VELNED   0x12
+#define UBX_ID_VELNED   0x12
+// acknowledge class
+#define UBX_CLASS_ACK   0x05
+#define UBX_ID_ACK_NAK  0x00
 #define UBX_ID_ACK_ACK  0x01
 // ------------------------------------------------------------------------------------------------
 // typedefs
         u32                     Hacc;           // mm Horizontal Accuracy Estimate
         u32                     Vacc;           // mm Vertical Accuracy Estimate
         u8                      Status;         // invalid/newdata/processed
 } __attribute__((packed)) ubx_nav_posllh_t;
 //------------------------------------------------------------------------------------
 // global variables
 // local buffers for the incomming ubx messages
 volatile ubx_nav_sol_t          UbxSol    = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
+volatile ubx_nav_posllh_t       UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
-volatile ubx_nav_posllh_t       UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
+volatile ubx_nav_velned_t       UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
-volatile ubx_nav_velned_t       UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
+volatile ubx_ack_t                      UbxAck    = {0,0,0,INVALID};
+{
         // mark msg buffers invalid
         UbxSol.Status = INVALID;
         UbxPosLlh.Status = INVALID;
         UbxVelNed.Status = INVALID;
+        UbxAck.Status = INVALID;
         GPSData.Status = INVALID;
         UBX_Timeout = SetDelay(2 * UBX_Timeout);
 /********************************************************/
 /*                   UBX Parser                         */
 /********************************************************/
-void UBX_Parser(u8 c)
+void UBX_RxParser(u8 c)
+{
         static ubxState_t ubxState = UBXSTATE_IDLE;
-        static ubxState_t ubxState = UBXSTATE_IDLE;
+        static u8 ubxclass = UBX_CLASS_UNDEF;
         static u16 msglen;
                         if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2;
                         else ubxState = UBXSTATE_IDLE; // out of synchronization
                         break;
                 case UBXSTATE_SYNC2: // check msg class to be NAV
-                        if (c == UBX_CLASS_NAV) ubxState = UBXSTATE_CLASS;
+                        ubxclass = c;
-                        else ubxState = UBXSTATE_IDLE; // unsupported message class
+                        ubxState = UBXSTATE_CLASS;
                         break;
                 case UBXSTATE_CLASS: // check message identifier
+                        switch(ubxclass)
+                        {
+                                case UBX_CLASS_NAV:
-                        switch(c)
+                                        switch(c)
-                        {
+                                        {
-                                case UBX_ID_POSLLH: // geodetic position
+                                                case UBX_ID_POSLLH: // geodetic position
-                                        ubxP =  (u8 *)&UbxPosLlh; // data start pointer
+                                                        ubxP =  (u8 *)&UbxPosLlh; // data start pointer
-                                        ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer
+                                                        ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer
-                                        ubxSp = (u8 *)&UbxPosLlh.Status; // status pointer
+                                                        ubxSp = (u8 *)&UbxPosLlh.Status; // status pointer
-                                        break;
+                                                        break;
-                                case UBX_ID_SOL: // navigation solution
+                                                case UBX_ID_SOL: // navigation solution
-                                        ubxP =  (u8 *)&UbxSol; // data start pointer
+                                                        ubxP =  (u8 *)&UbxSol; // data start pointer
+                                                        ubxEp = (u8 *)(&UbxSol + 1); // data end pointer
+                                                        ubxSp = (u8 *)&UbxSol.Status; // status pointer
+                                                        break;
+                                                case UBX_ID_VELNED: // velocity vector in tangent plane
+                                                        ubxP =  (u8 *)&UbxVelNed; // data start pointer
+                                                        ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer
+                                                        ubxSp = (u8 *)&UbxVelNed.Status; // status pointer
+                                                        break;
+                                                default:                        // unsupported identifier
+                                                        ubxState = UBXSTATE_IDLE;
-                                        ubxEp = (u8 *)(&UbxSol + 1); // data end pointer
+                                                        return;
+                                        }
+                                        break;
+                                case UBX_CLASS_ACK:
+                                        switch(c)
+                                        {
+                                                case UBX_ID_ACK_ACK:
-                                        ubxSp = (u8 *)&UbxSol.Status; // status pointer
+                                                        UbxAck.Ack = 1;
+                                                        break;
+                                                case UBX_ID_ACK_NAK:
+                                                        UbxAck.Ack = 0;
+                                                        break;
+                                                default:
-                                        break;
+                                                        ubxState = UBXSTATE_IDLE;
+                                                        return;
-                                case UBX_ID_VELNED: // velocity vector in tangent plane
+                                        }
-                                        ubxP =  (u8 *)&UbxVelNed; // data start pointer
+                                        ubxP =  (u8 *)&(UbxAck.clsID); // data start pointer
-                                        ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer
+                                        ubxEp = (u8 *)(&UbxAck + 1); // data end pointer
-                                        ubxSp = (u8 *)&UbxVelNed.Status; // status pointer
+                                        ubxSp = (u8 *)&UbxAck.Status; // status pointer
                                         break;
-                                default:                        // unsupported identifier
+                                default: // unsupported  class
                                         ubxState = UBXSTATE_IDLE;
                         // if the old data are not processed so far then break parsing now
                         // to avoid writing new data in ISR during reading by another function
                         if ( *ubxSp == NEWDATA )
+                        {
                                 ubxState = UBXSTATE_IDLE;
-                                Update_GPSData(); //update GPS info respectively
+                                if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively
+                        }
                         else // data invalid or allready processd
+                        {
                                 *ubxSp = INVALID; // mark invalid during buffer filling
                                 ubxState = UBXSTATE_DATA;
                 case UBXSTATE_CKB:
                         if (c == ckb)
+                        {
                                 *ubxSp = NEWDATA; // new data are valid
-                                Update_GPSData(); //update GPS info respectively
+                                if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively
+                        }
                         else
                         {       // if checksum not match then set data invalid
                                 *ubxSp = INVALID;
                         ubxState = UBXSTATE_IDLE;
                         break;
+        }
+}
+u8 UBX_CreateMsg(Buffer_t* pBuff, u8 Class, u8 Id, u8* pData, u16 Len)
+{
+        u16 i;
+        u8 cka = 0, ckb = 0;
+        // check if buffer is available
+        if(pBuff->Locked == TRUE) return(0);
+        // check if buffer size is sufficient
+        if(pBuff->Size < 8 + Len) return(0);
+        // lock the buffer
+        pBuff->Locked = TRUE;
+        // start at begin
+        pBuff->Position = 0;
+        pBuff->pData[pBuff->Position++] = UBX_SYNC1_CHAR;
+        pBuff->pData[pBuff->Position++] = UBX_SYNC2_CHAR;
+        pBuff->pData[pBuff->Position++] = Class;
+        pBuff->pData[pBuff->Position++] = Id;
+        pBuff->pData[pBuff->Position++] = (u8)(Len);
+        pBuff->pData[pBuff->Position++] = (u8)(Len>>8);
+        memcpy(&(pBuff->pData[pBuff->Position]), pData, Len);
+        pBuff->Position += Len;
+        // calculate checksum
+        for(i=2;i<pBuff->Position;i++)
+        {
+                cka += pBuff->pData[i];
+                ckb += cka;
+        }
+        pBuff->pData[pBuff->Position++] = cka;
+        pBuff->pData[pBuff->Position++] = ckb;
+        pBuff->DataBytes = pBuff->Position;
+        pBuff->Position = 0;  // reset buffer position for transmision
+        return(1);

Subversion Repositories NaviCtrl

(root)/tags/V0_24c/ubx.c @ 308 - Rev 244 → 245