int main(int argc, char **argv)
{
ros::init(argc, argv, "imu");
ros::NodeHandle nh;
ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000);
std_msgs::Float64 msg;
ros::Rate loopRate(10);
int temp = 0;
removegyrooff();
while (ros::ok())
{
read_sensors();
msg.data = TO_DEG(-atan2(magnetom[1], magnetom[0]));
chatter_pub.publish(msg);
ROS_INFO("%s %f", "send an imu message", TO_DEG(-atan2(magnetom[1], magnetom[0])));
// ROS_INFO("%d \n",temp);
ros::spinOnce();
loopRate.sleep();
temp++;
}
}
int main(int argc, char**argv)
{
ros::init(argc,argv,"imu");
ros::NodeHandle n;
ros::Publisher out_pub = n.advertise<sensor_msgs::Imu >("imuyrp", 1000);
ros::NodeHandle nh;
ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000);
sensor_msgs::Imu imu_msg;
std_msgs::Float64 msg;
// Read sensors, init DCM algorithm
reset_sensor_fusion();
removegyrooff();
float Y=0.0;
int i=0;
while(ros::ok())
{
// Time to read the sensors again?
if((clock() - timestamp) >= OUTPUT__DATA_INTERVAL)
{
timestamp_old = timestamp;
timestamp = clock();
if (timestamp > timestamp_old)
G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else G_Dt = 0;
//cout << G_Dt << endl;
// Update sensor readings
read_sensors();
// Run DCM algorithm
Compass_Heading(); // Calculate magnetic heading
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
output_angles();
imu_msg = sensor_msgs::Imu();
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = "imu";
imu_msg.orientation.x = TO_DEG(pitch);
imu_msg.orientation.y = TO_DEG(roll);
imu_msg.orientation.z = TO_DEG(yaw);
imu_msg.orientation.w = 0.0;
imu_msg.orientation_covariance[0] = -1;
imu_msg.angular_velocity.x = 0.0;
imu_msg.angular_velocity.y = 0.0;
imu_msg.angular_velocity.z = 0.0;
imu_msg.angular_velocity_covariance[0] = -1;
imu_msg.linear_acceleration.x = 0.0;
imu_msg.linear_acceleration.y = 0.0;
imu_msg.linear_acceleration.z = 0.0;
imu_msg.linear_acceleration_covariance[0] = -1;
msg.data = TO_DEG(-atan2(magnetom[1],magnetom[0]));
chatter_pub.publish(msg);
ROS_INFO("%s %f", "send an imu message",TO_DEG(-atan2(magnetom[1],magnetom[0])));
//ros::spinOnce();
}
}
}
