Position error with GPS/GSM v3

//HCS insert correction begin 
double DFRobot_SIM808::decimalgps(double rawdata)
{
  int degrees = (int)(rawdata / 100);
  double minutes = rawdata - (degrees*100);
  double mindecimal = minutes / 60.0;
  double total = degrees + mindecimal;
//  Serial.println("degrees:");
//  Serial.println(degrees,5);
//  Serial.println("minutes:");
//  Serial.println(minutes,5);
//  Serial.println("final:");
//  Serial.println(total,5);
 
  return total;
} 
//HCS insert correction end 
 
bool DFRobot_SIM808::getGPS() 
{
	 if(!getGPRMC())    //没有得到$GPRMC字符串开头的GPS信息
		 return false;
	// Serial.println(receivedStack);
	 if(!parseGPRMC(receivedStack))  //不是$GPRMC字符串开头的GPS信息
		 return false;  
		
	// skip mode
    char *tok = strtok(receivedStack, ",");     //起始引导符
    if (! tok) return false;
 
   // grab time                                  //<1> UTC时间，格式为hhmmss.sss；
   // tok = strtok(NULL, ",");
	char *time = strtok(NULL, ",");
    if (! time) return false;
	uint32_t newTime = (uint32_t)parseDecimal(time);
	getTime(newTime);
 
    // skip fix
    tok = strtok(NULL, ",");              //<2> 定位状态，A=有效定位，V=无效定位
    if (! tok) return false;
 
    // grab the latitude
    char *latp = strtok(NULL, ",");       //<3> 纬度ddmm.mmmm(度分)格式(前面的0也将被传输)
    if (! latp) return false;
 
    // grab latitude direction              // <4> 纬度半球N(北半球)或S(南半球)
    char *latdir = strtok(NULL, ",");
    if (! latdir) return false;
 
    // grab longitude                       //<5> 经度dddmm.mmmm(度分)格式(前面的0也将被传输)
    char *longp = strtok(NULL, ",");
    if (! longp) return false;
 
    // grab longitude direction            //<6> 经度半球E(东经)或W(西经)
    char *longdir = strtok(NULL, ",");
    if (! longdir) return false;
           	
	//HCS insert corrections begin
	double newlat =  atof(latp);
	double newlon =  atof(longp);
	
	float latitude  = decimalgps(newlat);
    float longitude = decimalgps(newlon);
 
	GPSdata.lat = latitude;
 
	GPSdata.lon= longitude;	
    //HCS insert corrections end
    
	/* HCS remove begin
	
	float latitude = atof(latp);
    float longitude = atof(longp);
	
	
	
	GPSdata.lat = latitude/100;
 
    // convert longitude from minutes to decimal  
	GPSdata.lon= longitude/100;
	
	HCS remove end */
	
    // only grab speed if needed                  //<7> 地面速率(000.0~999.9节，前面的0也将被传输)
   // if (speed_kph != NULL) {
 
      // grab the speed in knots
      char *speedp = strtok(NULL, ",");
      if (! speedp) return false;
 
      // convert to kph
      //*speed_kph = atof(speedp) * 1.852;
	  GPSdata.speed_kph= atof(speedp) * 1.852;
 
   // }
 
    // only grab heading if needed             
   // if (heading != NULL) {
 
      // grab the speed in knots
      char *coursep = strtok(NULL, ",");
      if (! coursep) return false;
 
      //*heading = atof(coursep);
		GPSdata.heading = atof(coursep);
   // }
	
	// grab date
	char *date = strtok(NULL, ",");   
    if (! date) return false;
	uint32_t newDate = atol(date);
	getDate(newDate);
 
    // no need to continue
   // if (altitude == NULL){
   //   return true;
	//}
	return true;
}

// Product name: GPS/GPRS/GSM Module V3.0
// # Product SKU : TEL0051
// # Version     : 1.2
 
// # Description:
// # The sketch for driving the gps mode via the Arduino board
 
// # Steps:
// #        1. Turn the S1 switch to the Prog(right side)
// #        2. Turn the S2 switch to the Arduino side(left side)
// #        3. Set the UART select switch to middle one.
// #        4. Upload the sketch to the Arduino board
// #        5. Turn the S1 switch to the comm(left side) 
// #        6. RST the board 
 
// #        If you get 'inf' values, go outdoors and wait until it is connected.
// #        wiki link- https://www.dfrobot.com/wiki/index.php/GPS/GPRS/GSM_Module_V3.0_(SKU:TEL0051)
double Datatransfer(char *data_buf,char num)//convert the data to the float type
{                                           //*data_buf：the data array                                       
  double temp=0.0;                           //the number of the right of a decimal point
  unsigned char i,j;
  if(data_buf[0]=='-')
  {
    i=1;
    //process the data array
    while(data_buf[i]!='.')
      temp=temp*10+(data_buf[i++]-0x30);
    for(j=0;j<num;j++)
      temp=temp*10+(data_buf[++i]-0x30);
    //convert the int type to the float type
    for(j=0;j<num;j++)
      temp=temp/10;
    //convert to the negative numbe
    temp=0-temp;
  }
  else//for the positive number
  {
    i=0;
    while(data_buf[i]!='.')
      temp=temp*10+(data_buf[i++]-0x30);
    for(j=0;j<num;j++)
      temp=temp*10+(data_buf[++i]-0x30);
    for(j=0;j<num;j++)
      temp=temp/10 ;
  }
  return temp;
}
 
char ID()//Match the ID commands
{ 
  char i=0;
  char value[6]={
    '$','G','P','G','G','A'      };//match the gps protocol
  char val[6]={
    '0','0','0','0','0','0'      };
 
  while(1)
  {
    if(Serial.available())
    {
      val[i] = Serial.read();//get the data from the serial interface
      if(val[i]==value[i]) //Match the protocol
      {    
        i++;
        if(i==6)
        {
          i=0;
          return 1;//break out after get the command
        }
      }
      else
        i=0;
    }
  } 
}
 
void comma(char num)//get ','
{   
  char val;
  char count=0;//count the number of ','
 
  while(1)
  {
    if(Serial.available())
    {
      val = Serial.read();
      if(val==',')
        count++;
    }
    if(count==num)//if the command is right, run return
      return;
  }
 
}
void UTC()//get the UTC data -- the time
{
  char i;
  char time[9]={
    '0','0','0','0','0','0','0','0','0'
  };
  double t=0.0;
 
  if( ID())//check ID
  {
    comma(1);//remove 1 ','
    //get the datas after headers
    while(1)
    {
      if(Serial.available())
      {
        time[i] = Serial.read();
        i++;
      }
      if(i==9)
      {
        i=0;
        t=Datatransfer(time,2);//convert data
        //t=t-30000.00;//convert to the chinese time GMT+8 Time zone
        long time=t;
        int h=time/10000;
        int m=(time/100)%100;
        int s=time%100;
        
        if(h>=24)               //UTC+
        {
        h-=24;
        }
 
//         if (h<0)               //UTC-
//        {
//          h+=24;
//        }
        Serial.print(h);
        Serial.print("h");
        Serial.print(m);
        Serial.print("m");
        Serial.print(s);
        Serial.println("s");
 
        //Serial.println(t);//Print data 
        return;
      }  
    }
  }
}

double decimalgps(double rawdata)
{
  int degrees = (int)(rawdata / 100);
  double minutes = rawdata - (degrees*100);
  double mindecimal = minutes / 60.0;
  double total = degrees + mindecimal;
//  Serial.println("degrees:");
//  Serial.println(degrees,5);
//  Serial.println("minutes:");
//  Serial.println(minutes,5);
//  Serial.println("final:");
//  Serial.println(total,5);

  return total;
}
float latitude()//get latitude
{
  char i;
  char lat[10]={
    '0','0','0','0','0','0','0','0','0','0'
  };
 
 
  if( ID())
  {
    comma(2);
    while(1)
    {
      if(Serial.available())
      {
        lat[i] = Serial.read();
        i++;
      }
      if(i==10)
      {
        i=0;
        double newlat =  Datatransfer(lat,5);
        float corrected = decimalgps(newlat);
        return corrected;
      }  
    }
  }
}
float longitude()//get longitude
{
  char i;
  char lon[11]={
    '0','0','0','0','0','0','0','0','0','0','0'
  };
 
  if( ID())
  {
    comma(4);
    while(1)
    {
      if(Serial.available())
      {
        lon[i] = Serial.read();
        i++;
      }
      if(i==11)
      {
        i=0;
        double newlon = Datatransfer(lon,5);
        float corrected = decimalgps(newlon);
        return corrected;
      }  
    }
  }
}

void altitude()//get altitude data
{
  char i,flag=0;
  char alt[8]={
    '0','0','0','0','0','0','0','0'
  };
 
  if( ID())
  {
    comma(9);
    while(1)
    {
      if(Serial.available())
      {
        alt[i] = Serial.read();
        if(alt[i]==',')
          flag=1;
        else
          i++;
      }
      if(flag)
      {
        i=0;
        Serial.println(Datatransfer(alt,1),1);//print altitude data
        return;
      }  
    }
  }
}
void setup()
{
  pinMode(3,OUTPUT);//The default digital driver pins for the GSM and GPS mode
  pinMode(4,OUTPUT);
  pinMode(5,OUTPUT);
  digitalWrite(5,HIGH);
  delay(1500);
  digitalWrite(5,LOW);
 
  digitalWrite(3,LOW);//Enable GSM mode
  digitalWrite(4,HIGH);//Disable GPS mode
  delay(2000);
  Serial.begin(9600); 
  delay(5000);//GPS ready
 
  Serial.println("AT");   
  delay(2000);
  //turn on GPS power supply
  Serial.println("AT+CGPSPWR=1");
  delay(1000);
  //reset GPS in autonomy mode
  Serial.println("AT+CGPSRST=1");
  delay(1000);
 
  digitalWrite(4,LOW);//Enable GPS mode
  digitalWrite(3,HIGH);//Disable GSM mode
  delay(2000);
 
  Serial.println("$GPGGA statement information: ");
}
void loop()
{
  while(1)
  { 
    Serial.print("UTC:");
    UTC();
    float lat = latitude();
    float lon = longitude();
    Serial.println("coordinates:");
    Serial.print(lat,5);
    Serial.print(", ");
    Serial.println(-lon,5);     
    Serial.print("Alt:");
    altitude();
    Serial.println(' ');
    Serial.println(' ');
  }
}