/******************************************************************************
 * DESCRIPTION:
 * 
 * 
 * If you need to transform the number A°B.C, you have to calculate A+B.C*5/3. 
 * Example: 123°57.318 is equal to 123°.9553

 * packet coordinates
 * ddd°mm.mmmmm
 *****************************************************************************/
fCoor_t CoorToFloat (u32_t _coor) {
  fCoor_t res;  
  u32_t   cr = _coor & 0x7FFFFFFF;
  u8_t    ddd;
  
  ddd = cr / 10000000;
  res = (fCoor_t)(cr - ddd * 10000000);
  res = res * (5.0/30000000.0) + (fCoor_t)ddd;
  	       
  if (_coor & 0x80000000) {
    res *= -1.0;
  }
  	       
  return res;
}

/******************************************************************************
 * DESCRIPTION:
 * get distance between to points
 *****************************************************************************/
u32_t Get_Distance(vNavData_t *pA,vNavData_t *pB) {

fCoor_t flatA = CoorToFloat (pA->lat);
fCoor_t flatB = CoorToFloat (pB->lat);
fCoor_t flonA = CoorToFloat (pA->lon);
fCoor_t flonB = CoorToFloat (pB->lon);

fCoor_t dLat;
fCoor_t dLon;
        dLat =  flatB - flatA;
        dLon = (flonB - flonA) * sa_cos((flatB + flatA)  * (PI/360.0));  
 return (u32_t)(sqrt(dLat * dLat  + dLon * dLon) * 111194.92665);
}