int main(int argc, char **argv)
{
int x,y,z;
int minx,miny,maxy,maxx;
union {
heading_evt_t hd;
magnetometer_evt_t magn;
evt_head_t head;
char buf[256];
} evt;
init_compass_calib();
minx=miny=32767;
maxx=maxy=-32768;
while(fread(&evt,1,sizeof(evt.head),stdin)) {
fread(evt.buf+sizeof(evt.head),1,evt.head.len-sizeof(evt.head),stdin);
switch(evt.head.type) {
case EVENT_MAGNETOMETER:
case EVENT_MAGNETOMETER_EXT:
calculate_heading(&evt.magn,&x,&y,&z);
printf("x: %d y: %d z: %d\n",x,y,z);
if (x<minx)
minx=x;
if (x>maxx)
maxx=x;
if (y<miny)
miny=y;
if (y>maxy)
maxy=y;
break;
}
}
fprintf(stderr,"MinX: %d MaxX: %d, pp: %d\n",minx,maxx,maxx-minx);
fprintf(stderr,"MinY: %d MaxY: %d, pp: %d\n",miny,maxy,maxy-miny);
printf("%d %d 0\n",-((maxx-minx)/2+minx),-((maxy-miny)/2+miny));
return 0;
}
void utmCallback(const msgs::gpgga_utm::ConstPtr& msg)
{
static tf::TransformBroadcaster odom_broadcaster;
if(msg->hdop < 2 && utm_settled == false)
{
utm_settled_count++;
if(utm_settled_count >= utm_settled_count_top)
{
utm_settled = true;
ref_fix = *msg;
prev_msg = *msg;
}
}
else if(utm_settled)
{
gps_points_t p;
p.x = msg->utm_e;
p.y = msg->utm_n;
gps_points_buffer.push_front(p);
odom.header.stamp = ros::Time::now();
odom.header.frame_id =frame_id;
odom.child_frame_id = child_frame_id;
if(publish_relative){
odom.pose.pose.position.x = -(ref_fix.utm_e - msg->utm_e);
odom.pose.pose.position.y = -(ref_fix.utm_n - msg->utm_n);
}
else
{
// ENU
odom.pose.pose.position.x =msg->utm_e;
odom.pose.pose.position.y =msg->utm_n;
}
odom.pose.pose.position.z = 0;
static double yaw=0;
if(calculate_heading(yaw))
{
odom.pose.covariance[35] = heading_variance;
}
else
{
odom.pose.covariance[35] = 999999;
}
// publish the transform message
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = tf_frame_id;
odom_trans.child_frame_id = tf_child_frame_id;
odom_trans.transform.translation.x = odom.pose.pose.position.x;
odom_trans.transform.translation.y = odom.pose.pose.position.y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(yaw);
odom_broadcaster.sendTransform(odom_trans);
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
odom.pose.covariance[0] = msg->hdop * gps_variance;
odom.pose.covariance[7] = msg->hdop * gps_variance;
odom.pose.covariance[14] = 999999;
odom.pose.covariance[21] = 999999;
odom.pose.covariance[28] = 999999;
odom_pub.publish(odom);
prev_msg = *msg;
}
}
