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();
}
}
}
// Main loop
void razor_loop(void)
{
razor_loop_input();
// Time to read the sensors again?
if ((timer_systime() - timestamp) >= OUTPUT__DATA_INTERVAL)
{
timestamp_old = timestamp;
timestamp = timer_systime();
if (timestamp > timestamp_old)
G_Dt = (double)MS2S(timestamp - timestamp_old); // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else
G_Dt = 0.0;
// Update sensor readings
read_sensors();
switch (output_mode)
{
case OUTPUT__MODE_CALIBRATE_SENSORS:
// We're in calibration mode
{
check_reset_calibration_session(); // Check if this session needs a reset
if (output_stream_on || output_single_on) output_calibration(curr_calibration_sensor);
break;
}
case OUTPUT__MODE_YPR_RAZOR:
{
// Apply sensor calibration
compensate_sensor_errors();
// Run DCM algorithm
Compass_Heading(); // Calculate magnetic heading
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
if (output_stream_on || output_single_on) output_angles();
break;
}
case OUTPUT__MODE_YPR_MADGWICK:
{
compensate_sensor_errors();
gyro[0] = GYRO_SCALED_RAD(gyro[0]);
gyro[1] = GYRO_SCALED_RAD(gyro[1]);
gyro[2] = GYRO_SCALED_RAD(gyro[2]);
// acc and mag already scaled in compensate_sensor_errors()
// Apply Madgwick algorithm
MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]);
//MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]);
//MadgwickAHRSupdate(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, magnetom[0], magnetom[1], magnetom[2]);
MadgwickYawPitchRoll(&yaw, &pitch, &roll);
if (output_stream_on || output_single_on) output_angles();
break;
}
case OUTPUT__MODE_YPR_FREEIMU:
{
compensate_sensor_errors();
gyro[0] = GYRO_SCALED_RAD(gyro[0]);
gyro[1] = GYRO_SCALED_RAD(gyro[1]);
gyro[2] = GYRO_SCALED_RAD(gyro[2]);
// acc and mag already scaled in compensate_sensor_errors()
// get sensorManager and initialise sensor listeners
//mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
//initListeners();
// wait for one second until gyroscope and magnetometer/accelerometer
// data is initialised then scedule the complementary filter task
//fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(), 1000, TIME_CONSTANT);
//public void onSensorChanged(SensorEvent event) {
//switch(event.sensor.getType()) {
//case Sensor.TYPE_ACCELEROMETER:
// copy new accelerometer data into accel array and calculate orientation
//System.arraycopy(event.values, 0, accel, 0, 3);
calculateAccMagOrientation();
//case Sensor.TYPE_GYROSCOPE:
// process gyro data
//gyroFunction(event);
gyroFunction();
//case Sensor.TYPE_MAGNETIC_FIELD:
// copy new magnetometer data into magnet array
//System.arraycopy(event.values, 0, magnet, 0, 3);
calculateFusedOrientation();
yaw = -fusedOrientation[0];
roll = -fusedOrientation[1];
pitch = fusedOrientation[2];
//if (output_stream_on || output_single_on) output_angles_freedom();
if (output_stream_on || output_single_on) output_angles();
break;
}
case OUTPUT__MODE_SENSORS_RAW:
case OUTPUT__MODE_SENSORS_CALIB:
case OUTPUT__MODE_SENSORS_BOTH:
{
if (output_stream_on || output_single_on) output_sensors();
break;
}
}
output_single_on = false;
#if DEBUG__PRINT_LOOP_TIME == true
//printf("loop time (ms) = %lu\r\n", timer_systime() - timestamp);
// Not really useful since RTC measures only 4 ms.
#endif
}
#if DEBUG__PRINT_LOOP_TIME == true
else
{
printf("waiting...\r\n");
}
#endif
return;
}
