char *carmen_string_to_gps_gprmc_message(char *string, carmen_gps_gprmc_message *gps_msg) { char *current_pos = string; if (strncmp(current_pos, "NMEARMC ", 8) == 0) current_pos = carmen_next_word(current_pos); gps_msg->nr = CLF_READ_INT(¤t_pos); gps_msg->validity = CLF_READ_INT(¤t_pos); gps_msg->utc = CLF_READ_DOUBLE(¤t_pos); gps_msg->latitude_dm = CLF_READ_DOUBLE(¤t_pos); gps_msg->latitude = carmen_global_convert_degmin_to_double(gps_msg->latitude_dm); current_pos = carmen_next_word(current_pos); gps_msg->lat_orient = CLF_READ_CHAR(¤t_pos); gps_msg->longitude_dm = CLF_READ_DOUBLE(¤t_pos); gps_msg->longitude = carmen_global_convert_degmin_to_double(gps_msg->longitude_dm); current_pos = carmen_next_word(current_pos); gps_msg->long_orient = CLF_READ_CHAR(¤t_pos); gps_msg->speed = CLF_READ_DOUBLE(¤t_pos); gps_msg->true_course = CLF_READ_DOUBLE(¤t_pos); gps_msg->variation = CLF_READ_DOUBLE(¤t_pos); current_pos = carmen_next_word(current_pos); gps_msg->var_dir = CLF_READ_CHAR(¤t_pos); gps_msg->date = CLF_READ_INT(¤t_pos); gps_msg->timestamp = CLF_READ_DOUBLE(¤t_pos); copy_host_string(&gps_msg->host, ¤t_pos); return current_pos; }
char *carmen_string_to_gps_gpgga_message(char *string, carmen_gps_gpgga_message *gps_msg) { char *current_pos = string; if (strncmp(current_pos, "NMEAGGA ", 8) == 0) current_pos = carmen_next_word(current_pos); gps_msg->nr = CLF_READ_INT(¤t_pos); gps_msg->utc = CLF_READ_DOUBLE(¤t_pos); gps_msg->latitude_dm = CLF_READ_DOUBLE(¤t_pos); gps_msg->latitude = carmen_global_convert_degmin_to_double(gps_msg->latitude_dm); current_pos = carmen_next_word(current_pos); gps_msg->lat_orient = CLF_READ_CHAR(¤t_pos); gps_msg->longitude_dm = CLF_READ_DOUBLE(¤t_pos); gps_msg->longitude = carmen_global_convert_degmin_to_double(gps_msg->longitude_dm); current_pos = carmen_next_word(current_pos); gps_msg->long_orient = CLF_READ_CHAR(¤t_pos); gps_msg->gps_quality = CLF_READ_INT(¤t_pos); gps_msg->num_satellites = CLF_READ_INT(¤t_pos); gps_msg->hdop = CLF_READ_DOUBLE(¤t_pos); gps_msg->sea_level = CLF_READ_DOUBLE(¤t_pos); gps_msg->altitude = CLF_READ_DOUBLE(¤t_pos); gps_msg->geo_sea_level = CLF_READ_DOUBLE(¤t_pos); gps_msg->geo_sep = CLF_READ_DOUBLE(¤t_pos); gps_msg->data_age = CLF_READ_INT(¤t_pos); gps_msg->timestamp = CLF_READ_DOUBLE(¤t_pos); copy_host_string(&gps_msg->host, ¤t_pos); return current_pos; }
void velodyne_gps_handler(carmen_velodyne_gps_message *message) { carmen_velodyne_gps_xyz_message velodyne_gps_xyz_message; velodyne_gps_xyz_message.accel1_x = message->accel1_x; velodyne_gps_xyz_message.accel1_y = message->accel1_y; velodyne_gps_xyz_message.accel2_x = message->accel2_x; velodyne_gps_xyz_message.accel2_y = message->accel2_y; velodyne_gps_xyz_message.accel3_x = message->accel3_x; velodyne_gps_xyz_message.accel3_y = message->accel3_y; velodyne_gps_xyz_message.gyro1 = message->gyro1; velodyne_gps_xyz_message.gyro2 = message->gyro2; velodyne_gps_xyz_message.gyro3 = message->gyro3; velodyne_gps_xyz_message.temp1 = message->temp1; velodyne_gps_xyz_message.temp2 = message->temp2; velodyne_gps_xyz_message.temp3 = message->temp3; velodyne_gps_xyz_message.utc_time = message->utc_time; velodyne_gps_xyz_message.utc_date = message->utc_date; velodyne_gps_xyz_message.status = message->status; velodyne_gps_xyz_message.latitude = carmen_global_convert_degmin_to_double(message->latitude); velodyne_gps_xyz_message.latitude_hemisphere = message->latitude_hemisphere; velodyne_gps_xyz_message.longitude = carmen_global_convert_degmin_to_double(message->longitude); velodyne_gps_xyz_message.longitude_hemisphere = message->longitude_hemisphere; velodyne_gps_xyz_message.speed_over_ground = message->speed_over_ground; velodyne_gps_xyz_message.course_over_ground = message->course_over_ground; velodyne_gps_xyz_message.magnetic_variation_course = message->magnetic_variation_course; velodyne_gps_xyz_message.magnetic_variation_direction = message->magnetic_variation_direction; velodyne_gps_xyz_message.mode_indication = message->mode_indication; velodyne_gps_xyz_message.timestamp = message->timestamp; if (velodyne_gps_xyz_message.latitude_hemisphere == 'S') velodyne_gps_xyz_message.latitude = -velodyne_gps_xyz_message.latitude; if (velodyne_gps_xyz_message.longitude_hemisphere == 'W') velodyne_gps_xyz_message.longitude = -velodyne_gps_xyz_message.longitude; // Transformando o z utilizando como altitude o sea_level Gdc_Coord_3d gdc = Gdc_Coord_3d((double) velodyne_gps_xyz_message.latitude, (double)velodyne_gps_xyz_message.longitude, 0.0); Utm_Coord_3d utm; Gdc_To_Utm_Converter::Init(); Gdc_To_Utm_Converter::Convert(gdc,utm); // The axis are changed to match the carmen frame of reference velodyne_gps_xyz_message.position.x = utm.y; velodyne_gps_xyz_message.position.y = -utm.x; velodyne_gps_xyz_message.position.z = utm.z; publish_velodyne_gps_xyz_message(velodyne_gps_xyz_message); }
int gps_parse_gga( char * line, int num_chars ) { char * ptr; int i; for (i=1; i<num_chars-1; i++) { if (line[i]=='$' || line[i]=='*') return(FALSE); } if (num_chars>0 && carmen_extern_gpgga_ptr!=NULL) { ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->utc = atof(ptr); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->latitude_dm = atof(ptr); carmen_extern_gpgga_ptr->latitude = carmen_global_convert_degmin_to_double(carmen_extern_gpgga_ptr->latitude_dm); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->lat_orient = ptr[0]; ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->longitude_dm = atof(ptr); carmen_extern_gpgga_ptr->longitude = carmen_global_convert_degmin_to_double(carmen_extern_gpgga_ptr->longitude_dm); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->long_orient = ptr[0]; ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->gps_quality = atoi(ptr); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->num_satellites = atoi(ptr); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->hdop = atof(ptr); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->sea_level = atof(ptr); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->altitude = atof(ptr); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->geo_sea_level = atof(ptr); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->geo_sep = atof(ptr); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gpgga_ptr->data_age = atoi(ptr); return(TRUE); } return(FALSE); }
int gps_parse_rmc(char * line, int num_chars) { char * ptr; int i; for (i=1; i<num_chars-1; i++) { if (line[i]=='$' || line[i]=='*') return(FALSE); } if (num_chars>0 && carmen_extern_gpgga_ptr!=NULL) { ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->utc = atof(ptr); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->validity = (ptr[0] == 'A')?1:0; ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->latitude_dm = atof(ptr); carmen_extern_gprmc_ptr->latitude = carmen_global_convert_degmin_to_double(carmen_extern_gprmc_ptr->latitude_dm); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->lat_orient = ptr[0]; ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->longitude_dm = atof(ptr); carmen_extern_gprmc_ptr->longitude = carmen_global_convert_degmin_to_double(carmen_extern_gprmc_ptr->longitude_dm); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->long_orient = ptr[0]; ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->speed = carmen_knots_to_meters_per_second( atof(ptr) ); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->true_course = carmen_degrees_to_radians( atoi(ptr) ); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->date = atoi(ptr); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->variation = carmen_degrees_to_radians( atoi(ptr) ); ptr = strsep( &line, ","); if (ptr==NULL) return(FALSE); carmen_extern_gprmc_ptr->var_dir =ptr[0]; return(TRUE); } return(FALSE); }