motor/Common/uart/novatel_class/gps_novatel_protocol.c

/*
 * Acc_ADXL355.c
 *
 *  Created on: July 12, 2020
 *      Author: lqh
 */
#include "main.h"
#include "gps_novatel_protocol.h"
#include <string.h>
#include "same_endian.h"
#include <stdio.h>

int RTT_PRINT_INFO = 0;

/** decode gpst week & msec in frame header -----------------------------------*/
static void HeaderGpst(uint8_t *frame, uint16_t *week, uint32_t *msec)
{
  memcpy(week,frame+14,2);
  memcpy(msec,frame+16,4);
}

static GnssFixtypeEnum NovatelFixtypeConvert(NovatelFixtypeEnum type_covr)
{

  switch (type_covr)
  {
    case NOVATEL_FIXTYPE_NONE           :
      return GNSS_FIXTYPE_NOFIX;

    case NOVATEL_FIXTYPE_FIXEDPOS       :
      return GNSS_FIXTYPE_RTK_FIXED;

    case NOVATEL_FIXTYPE_SMOOTED_SINGLE :
      return GNSS_FIXTYPE_SINGLE;

    case NOVATEL_FIXTYPE_SINGLE         :
      return GNSS_FIXTYPE_SINGLE;
    
    case NOVATEL_FIXTYPE_PSRDIFF        :
      return GNSS_FIXTYPE_DGNSS;
    
    case NOVATEL_FIXTYPE_WAAS           :
      return GNSS_FIXTYPE_DGNSS;

    case NOVATEL_FIXTYPE_L1_FLOAT       :
      return GNSS_FIXTYPE_RTK_FLOAT;
    
    case NOVATEL_FIXTYPE_IONOFREE_FLOAT :
      return GNSS_FIXTYPE_RTK_FLOAT;
    
    case NOVATEL_FIXTYPE_NARROR_FLAOT   :
      return GNSS_FIXTYPE_RTK_FLOAT;
    
    case NOVATEL_FIXTYPE_FIXED_RECKON   :
      return GNSS_FIXTYPE_RTK_FIXED;
    
    case NOVATEL_FIXTYPE_L1_INT         :
      return GNSS_FIXTYPE_RTK_FIXED;
    
    case NOVATEL_FIXTYPE_NARROW_INT     :
      return GNSS_FIXTYPE_RTK_FIXED;
    
    default                             :
      return GNSS_FIXTYPE_UNKOWN;
  }
}

void NovatelDecodeTime(NovatelStruct *sol,uint8_t frame[])
{
  uint8_t *p = frame+28;
  uint32_t ms, UtcStatus;

  HeaderGpst(frame,&sol->HeaderWeekOfTime,&sol->HeaderMsecOfTime);
  
  sol->utc.year  = GET_U32(p + 28);
  sol->utc.month = GET_U08(p + 32);
  sol->utc.day   = GET_U08(p + 33);
  sol->utc.hour  = GET_U08(p + 34);
  sol->utc.min   = GET_U08(p + 35);
  ms             = GET_U32(p + 36); // ms:0-60999
  sol->utc.isec  = ms / 1000;
  sol->utc.msec  = ms % 1000;
  sol->utc.sec   = ms * B10_N3;
  UtcStatus       = GET_U32(p + 40); // 0:invalid, 1:valid, 2:warning
  if (1 == UtcStatus)
  {
    sol->utc.valid = 1;
  }

  sol->CntNovatelTime++;
  if (RTT_PRINT_INFO)
  printf("TIME,%u,%lu,time,%04d-%02d-%02d %02d:%02d:%02d,ms,%03d,utcstatus=%lu\r\n",
        sol->HeaderWeekOfTime, sol->HeaderMsecOfTime,
        sol->utc.year, sol->utc.month, sol->utc.day, sol->utc.hour, sol->utc.min,
        sol->utc.isec, sol->utc.msec, UtcStatus);
}

void NovatelDecodeBestpos(NovatelStruct *sol,uint8_t frame[])
{
  uint8_t *p = frame+28;

  HeaderGpst(frame,&sol->HeaderWeekOfBestpos,&sol->HeaderMsecOfBestpos);
  sol->flgPosUpd = 1;
  sol->PosType    = GET_U32(p +  4);
  sol->LatDeg     = GET_F64(p +  8);
  sol->LonDeg     = GET_F64(p + 16);
  sol->hgt        = GET_F64(p + 24);
  sol->LatSigma   = GET_F32(p + 40);
  sol->LonSigma   = GET_F32(p + 44);
  sol->HightSigma = GET_F32(p + 48);
  sol->baseid     = GET_U32(p + 52);
  sol->diffage    = GET_F32(p + 56); 
  sol->SolnSv     = GET_U08(p + 65);
  sol->fix_type   = NovatelFixtypeConvert(sol->PosType);

  sol->CntNovatelBestpos++;
  if (RTT_PRINT_INFO)
  if (0 == sol->CntNovatelBestpos % 10)
  {
    printf("BESTPOS,%u,%lu,Lat%d,Lon%d,H%dmm,sv,%c,type=%d\r\n",
          sol->HeaderWeekOfBestpos,sol->HeaderMsecOfBestpos,
          (int)(sol->LatDeg*1e7),(int)(sol->LonDeg*1e7),(int)(sol->hgt*1000),sol->SolnSv,sol->PosType);
  }
}


void NovatelDecodeHeading(NovatelStruct *sol,uint8_t frame[])
{
  uint8_t *p = frame+28;

  sol->flgAttUpd = 1;
  sol->NovatelAttType = GET_U32(p + 4);
  //sol->baselineLength = GET_F32(p +  8);
  sol->heading        = GET_F32(p + 12);
  sol->pitch          = GET_F32(p + 16);
  sol->ang_sv_num     = GET_U08(p + 37);
  sol->att_type       = NovatelFixtypeConvert(sol->NovatelAttType);
  sol->CntNovatelHeading++;
  if (RTT_PRINT_INFO)
  if (0 == sol->CntNovatelHeading % 10)
  {
    printf("HEADING,heading=%dcdeg,pitch=%dcdeg,angle_sv,%d,type=%d\r\n",
          (int)(sol->heading*100),(int)(sol->pitch*100),
          sol->ang_sv_num,sol->NovatelAttType);
  }

}


void NovatelDecodePsrdop(NovatelStruct *sol,uint8_t frame[])
{
  uint8_t *p = frame+28;

  sol->flgDopUpd = 1;
  sol->gdop = GET_F32(p);
  sol->Pdop = GET_F32(p + 4);
  sol->Hdop = GET_F32(p + 8);
  sol->CntNovatelPsrdop++;
  if (RTT_PRINT_INFO)
  if (0 == sol->CntNovatelPsrdop % 10)
  {
    printf("PSRDOP,gdop=%d,Pdop=%d,Hdop=%d\r\n",
          (int)(sol->gdop*100),(int)(sol->Pdop*100),
          (int)(sol->Hdop*100));
  }
}


void NovatelDecodeBestvel(NovatelStruct *sol,uint8_t frame[])
{
  uint8_t *p = frame+28;
  double ned[3]={0.0},hvy[3]={0.0};
  
  HeaderGpst(frame,&sol->HeaderWeekOfBestvel,&sol->HeaderMsecOfBestvel);
  sol->flgVelUpd = 1;
  sol->V_valid = GET_U32(p + 0);
  sol->HorSpd  = GET_F64(p + 16);
  sol->TrkGnd  = GET_F64(p + 24);
  sol->VertSpd = GET_F64(p + 32);

  hvy[0] = sol->HorSpd;
  hvy[1] = sol->VertSpd;
  hvy[2] = sol->TrkGnd;
  hvy2ned(hvy, ned);
  sol->velN = (float)ned[0];
  sol->velE = (float)ned[1];
  sol->velD = (float)ned[2];

  sol->CntNovatelBestvel++;
  if (RTT_PRINT_INFO)
   if (0 == sol->CntNovatelBestvel % 10)
   {
     printf("BESTVEL,%u,%lu,N%+5d,E%+5d,D%+5d(cm/s)\r\n",
           sol->HeaderWeekOfBestvel, sol->HeaderMsecOfBestvel,
		   (int)(sol->velN*100), (int)(sol->velE*100), (int)(sol->velD*100));
   }
}

void NovatelDecodeInit(NovatelStruct *sol)
{
	sol->CntNovatelBestpos = 0;
	sol->CntNovatelBestvel = 0;
	sol->CntNovatelHeading = 0;
  sol->CntNovatelPsrdop = 0;
	sol->CntNovatelTime = 0;
	sol->LastCntNovatelBestpos = 0;
	sol->LastCntNovatelBestvel = 0;
	sol->LastCntNovatelHeading = 0;
	sol->LastCntNovatelTime = 0;
	sol->LastCntNovatelPsrdop = 0;
}

extern void NovatelDecodeStats(NovatelStruct *sol)
{
	sol->PpsNovatelBestpos = sol->CntNovatelBestpos - sol->LastCntNovatelBestpos;
	sol->LastCntNovatelBestpos = sol->CntNovatelBestpos;

	sol->PpsNovatelBestvel = sol->CntNovatelBestvel - sol->LastCntNovatelBestvel;
	sol->LastCntNovatelBestvel = sol->CntNovatelBestvel;

	sol->PpsNovatelHeading = sol->CntNovatelHeading - sol->LastCntNovatelHeading;
	sol->LastCntNovatelHeading = sol->CntNovatelHeading;

    sol->PpsNovatelPsrdop = sol->CntNovatelPsrdop - sol->LastCntNovatelPsrdop;
	sol->LastCntNovatelPsrdop = sol->CntNovatelPsrdop;

	sol->PpsNovatelTime = sol->CntNovatelTime - sol->LastCntNovatelTime;
	sol->LastCntNovatelTime = sol->CntNovatelTime;

}