void Get_Imu_Init_Position(float *Imu_Init_Position)
{
short Acc[3]={0};
short Gyo[3]={0};
short Mag[3]={0};
short Mag_Offset[3]={0};
int Acc_Filter_Sum[3]={0};
int Gyo_Filter_Sum[3]={0};
int Mag_Filter_Sum[3]={0};
int i=0,j=0;
MPU9250_GetMag_Offset(Mag_Offset);
for(i=0;i<FILTER_LENGTH;i++){
READ_MPU9250(Acc,Gyo,Mag);
for(j=0;j<3;j++){
Acc_Filter_Sum[j]+=Acc[j];
Gyo_Filter_Sum[j]+=Gyo[j];
Mag_Filter_Sum[j]+=Mag[j];
}
}
for(i=0;i<3;i++){
Acc[i]=Acc_Filter_Sum[i]/FILTER_LENGTH;
Gyo[i]=Gyo_Filter_Sum[i]/FILTER_LENGTH;
Mag[i]=Mag_Filter_Sum[i]/FILTER_LENGTH;
}
reset_sensor_fusion(Acc,Gyo,Mag,Imu_Init_Position);
}
void setupImu()
{
// Init sensors
delay(50); // Give sensors enough time to start
I2C_Init();
Accel_Init();
Magn_Init();
Gyro_Init();
// Read sensors, init DCM algorithm
delay(20); // Give sensors enough time to collect data
reset_sensor_fusion();
}
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();
}
}
}
