void* sensorReadLoop(void *arg)
{
// using RTIMULib here allows it to use the .ini file generated by RTIMULibDemo.
RTIMUSettings *settings = new RTIMUSettings("RTIMULib");
RTIMU *imu = RTIMU::createIMU(settings);
RTPressure *pressure = RTPressure::createPressure(settings);
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL)) {
printf("No IMU found\n");
exit(1);
}
// This is an opportunity to manually override any settings before the call IMUInit
// set up IMU
imu->IMUInit();
// this is a convenient place to change fusion parameters
imu->setSlerpPower(0.02);
imu->setGyroEnable(true);
imu->setAccelEnable(true);
imu->setCompassEnable(true);
// set up pressure sensor
if (pressure != NULL)
pressure->pressureInit();
// now just process datawhile (1) {
// poll at the rate recommended by the IMU
while(1){
usleep(imu->IMUGetPollInterval() * 1000);
while (imu->IMURead()) {
RTIMU_DATA imuData = imu->getIMUData();
// add the pressure data to the structure
//if (pressure != NULL){
//pressure->pressureRead(imuData);
x = imuData.fusionPose.x() * RTMATH_RAD_TO_DEGREE;
y = imuData.fusionPose.y() * RTMATH_RAD_TO_DEGREE;
z = imuData.fusionPose.z() * RTMATH_RAD_TO_DEGREE;
//printf("%f %f %f \n",imuData.fusionPose.x() * RTMATH_RAD_TO_DEGREE, imuData.fusionPose.y() * RTMATH_RAD_TO_DEGREE, imuData.fusionPose.z() * RTMATH_RAD_TO_DEGREE);
/* if (pressure != NULL) {
printf("Pressure: %4.1f, height above sea level: %4.1f, temperature: %4.1f\n",
imuData.pressure, RTMath::convertPressureToHeight(imuData.pressure), imuData.temperature);
}
*/
//fflush(stdout);
//}
}
}
}
void I2cImu::update()
{
while (imu_->IMURead() && ros::ok())
{
RTIMU_DATA imuData = imu_->getIMUData();
ros::Time current_time = ros::Time::now();
imu_msg.header.stamp = current_time;
imu_msg.header.frame_id = imu_frame_id_;
imu_msg.orientation.x = imuData.fusionQPose.x();
imu_msg.orientation.y = imuData.fusionQPose.y();
imu_msg.orientation.z = imuData.fusionQPose.z();
imu_msg.orientation.w = imuData.fusionQPose.scalar();
imu_msg.angular_velocity.x = imuData.gyro.x();
imu_msg.angular_velocity.y = imuData.gyro.y();
imu_msg.angular_velocity.z = imuData.gyro.z();
imu_msg.linear_acceleration.x = imuData.accel.x() * G_2_MPSS;
imu_msg.linear_acceleration.y = imuData.accel.y() * G_2_MPSS;
imu_msg.linear_acceleration.z = imuData.accel.z() * G_2_MPSS;
imu_pub_.publish(imu_msg);
if (magnetometer_pub_ != NULL && imuData.compassValid)
{
sensor_msgs::MagneticField msg;
msg.header.frame_id=imu_frame_id_;
msg.header.stamp=ros::Time::now();
msg.magnetic_field.x = imuData.compass.x()/uT_2_T;
msg.magnetic_field.y = imuData.compass.y()/uT_2_T;
msg.magnetic_field.z = imuData.compass.z()/uT_2_T;
magnetometer_pub_.publish(msg);
}
if (euler_pub_ != NULL)
{
geometry_msgs::Vector3 msg;
msg.x = imuData.fusionPose.x();
msg.y = imuData.fusionPose.y();
msg.z = -imuData.fusionPose.z();
msg.z = (-imuData.fusionPose.z()) - declination_radians_;
euler_pub_.publish(msg);
}
ros::spinOnce();
}
}
int main(int argc, char **argv){
std::cout << "IMU is opening" << std::endl;
RTIMUSettings *settings = new RTIMUSettings("~/imu_test/", "");
RTIMU *imu = RTIMU::createIMU(settings);
int sampleCount = 0;
int sampleRate = 0;
uint64_t rateTimer;
uint64_t displayTimer;
uint64_t now;
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL)){
std::cout << "Error, couldn't open IMU" << std::endl;
return -1;
}
// Initialise the imu object
imu->IMUInit();
// Set the Fusion coefficient
imu->setSlerpPower(0.02);
// Enable the sensors
imu->setGyroEnable(true);
imu->setAccelEnable(true);
imu->setCompassEnable(true);
while (1) {
// poll at the rate recommended by the IMU
usleep(imu->IMUGetPollInterval() * 1000);
while (imu->IMURead()) {
RTIMU_DATA imuData = imu->getIMUData();
sampleCount++;
now = RTMath::currentUSecsSinceEpoch();
// display 10 times per second
if ((now - displayTimer) > 100000) {
printf("Sample rate %d: %s\r", sampleRate, RTMath::displayDegrees("", imuData.fusionPose));
fflush(stdout);
displayTimer = now;
}
// update rate every second
if ((now - rateTimer) > 1000000) {
sampleRate = sampleCount;
sampleCount = 0;
rateTimer = now;
}
}
}
}
int main() {
int sampleCount = 0;
int sampleRate = 0;
uint64_t rateTimer;
uint64_t displayTimer;
uint64_t now;
// Using RTIMULib here allows it to use the .ini file generated by RTIMULibDemo.
// Or, you can create the .ini in some other directory by using:
// RTIMUSettings *settings = new RTIMUSettings("<directory path>", "RTIMULib");
// where <directory path> is the path to where the .ini file is to be loaded/saved
RTIMUSettings *settings = new RTIMUSettings("RTIMULib");
RTIMU *imu = RTIMU::createIMU(settings);
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL)) {
printf("No IMU found\n");
exit(1);
}
// This is an opportunity to manually override any settings before the call IMUInit
// set up IMU
imu->IMUInit();
// this is a convenient place to change fusion parameters
imu->setSlerpPower(0.02);
imu->setGyroEnable(true);
imu->setAccelEnable(true);
imu->setCompassEnable(true);
// set up for rate timer
/* Code in this loop will run repeatedly
*/
for (;;) {
if (imu->IMURead()) {
RTIMU_DATA imuData = imu->getIMUData() ;
printf("Sensors Valid? : %d %d %d\n", imuData.accelValid, imuData.compassValid, imuData.gyroValid) ;
printf("Fusion Success? : %d \n", imuData.fusionPoseValid) ;
RTVector3 curr_pose = imuData.fusionPose ;
printf("Roll = %g Pitch = %g Yaw = %g \n", curr_pose.x()*R2D, curr_pose.y()*R2D, curr_pose.z()*R2D ) ;
}
else
printf("No data available") ;
usleep(1000000) ;
}
return 0;
}
void I2cImu::spin()
{
ros::Rate r(1.0 / (imu_->IMUGetPollInterval() / 1000.0));
while (ros::ok())
{
update();
r.sleep();
}
}
int main()
{
int sampleCount = 0;
int sampleRate = 0;
uint64_t rateTimer;
uint64_t displayTimer;
uint64_t now;
// using RTIMULib here allows it to use the .ini file generated by RTIMULibDemo.
RTIMUSettings *settings = new RTIMUSettings("RTIMULib");
RTIMU *imu = RTIMU::createIMU(settings);
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL)) {
printf("No IMU found\n");
exit(1);
}
// This is an opportunity to manually override any settings before the call IMUInit
// set up IMU
imu->IMUInit();
// set up for rate timer
rateTimer = displayTimer = RTMath::currentUSecsSinceEpoch();
// create the vrpn servee
vrpn = new vrpnServer();
// now just process data
while (1) {
// poll at the rate recommended by the IMU
usleep(imu->IMUGetPollInterval() * 1000);
// call the vrpn main loop
vrpn->mainloop(); // this function can be placed inside a new thread, which should run faster than
// the loop that calls the update_tracking function.
while (imu->IMURead()) {
RTIMU_DATA imuData = imu->getIMUData();
vrpn->serverTracker->update_tracking(RTVector3(), imuData.fusionQPose);
sampleCount++;
now = RTMath::currentUSecsSinceEpoch();
// display 10 times per second
if ((now - displayTimer) > 100000) {
printf("Sample rate %d: %s\r", sampleRate, RTMath::displayDegrees("", imuData.fusionPose));
fflush(stdout);
displayTimer = now;
}
// update rate every second
if ((now - rateTimer) > 1000000) {
sampleRate = sampleCount;
sampleCount = 0;
rateTimer = now;
}
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "rtimulib_node");
ROS_INFO("Imu driver is now running");
ros::NodeHandle n;
ros::NodeHandle private_n("~");
std::string topic_name;
if(!private_n.getParam("topic_name", topic_name))
{
ROS_WARN("No topic_name provided - default: imu/data");
topic_name = "imu/data";
}
std::string calibration_file_path;
if(!private_n.getParam("calibration_file_path", calibration_file_path))
{
ROS_ERROR("The calibration_file_path parameter must be set to use a calibration file.");
}
std::string calibration_file_name;
if(!private_n.getParam("calibration_file_name", calibration_file_name))
{
ROS_WARN("No calibration_file_name provided - default: RTIMULib.ini");
calibration_file_name = "RTIMULib";
}
std::string frame_id;
if(!private_n.getParam("frame_id", frame_id))
{
ROS_WARN("No frame_id provided - default: imu_link");
frame_id = "imu_link";
}
double update_rate;
if(!private_n.getParam("update_rate", update_rate))
{
ROS_WARN("No update_rate provided - default: 20 Hz");
update_rate = 20;
}
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>(topic_name.c_str(), 1);
// Load the RTIMULib.ini config file
RTIMUSettings *settings = new RTIMUSettings(calibration_file_path.c_str(), calibration_file_name.c_str());
RTIMU *imu = RTIMU::createIMU(settings);
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL))
{
ROS_ERROR("No Imu found");
return -1;
}
// Initialise the imu object
imu->IMUInit();
// Set the Fusion coefficient
imu->setSlerpPower(0.02);
// Enable the sensors
imu->setGyroEnable(true);
imu->setAccelEnable(true);
imu->setCompassEnable(true);
ros::Rate loop_rate(update_rate);
while (ros::ok())
{
sensor_msgs::Imu imu_msg;
if (imu->IMURead())
{
RTIMU_DATA imu_data = imu->getIMUData();
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
imu_msg.orientation.x = imu_data.fusionQPose.x();
imu_msg.orientation.y = imu_data.fusionQPose.y();
imu_msg.orientation.z = imu_data.fusionQPose.z();
imu_msg.orientation.w = imu_data.fusionQPose.scalar();
imu_msg.angular_velocity.x = imu_data.gyro.x();
imu_msg.angular_velocity.y = imu_data.gyro.y();
imu_msg.angular_velocity.z = imu_data.gyro.z();
imu_msg.linear_acceleration.x = imu_data.accel.x();
imu_msg.linear_acceleration.y = imu_data.accel.y();
imu_msg.linear_acceleration.z = imu_data.accel.z();
imu_pub.publish(imu_msg);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int IMU::readIMU()
{
int sampleCount = 0;
int sampleRate = 0;
uint64_t rateTimer;
uint64_t displayTimer;
uint64_t now;
// Using RTIMULib here allows it to use the .ini file generated by RTIMULib
//Demo.
// Or, you can create the .ini in some other directory by using:
// RTIMUSettings *settings = new RTIMUSettings("<directory path>", "RTIMULib");
// where <directory path> is the path to where the .ini file is to be loaded/saved
RTIMUSettings *settings = new RTIMUSettings("RTIMULib");
RTIMU *imu = RTIMU::createIMU(settings);
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL)) {
printf("No IMU found\n");
exit(1);
}
// This is an opportunity to manually override any settings before the call IMUInit
imu->IMUInit();
// this is a convenient place to change fusion parameters
imu->setSlerpPower(0.02);
imu->setGyroEnable(true);
imu->setAccelEnable(true);
imu->setCompassEnable(true);
// set up for rate timer
rateTimer = displayTimer = RTMath::currentUSecsSinceEpoch();
// now just process data
while (1) {
// poll at the rate recommended by the IMU
usleep(imu->IMUGetPollInterval() * 1000);
while (imu->IMURead()) {
RTIMU_DATA imuData = imu->getIMUData();
sampleCount++;
now = RTMath::currentUSecsSinceEpoch();
// display 10 times per second
if ((now - displayTimer) > 100000) {
printf("Sample rate %d: %s\r", sampleRate, RTMath::displayDegrees("", imuData.fusionPose));
fflush(stdout);
displayTimer = now;
}
// update rate every second
if ((now - rateTimer) > 1000000) {
sampleRate = sampleCount;
sampleCount = 0;
rateTimer = now;
}
}
}
}
int main()
{
int sampleCount = 0;
int sampleRate = 0;
uint64_t rateTimer;
uint64_t displayTimer;
uint64_t now;
// Using RTIMULib here allows it to use the .ini file generated by RTIMULibDemo.
// Or, you can create the .ini in some other directory by using:
// RTIMUSettings *settings = new RTIMUSettings("<directory path>", "RTIMULib");
// where <directory path> is the path to where the .ini file is to be loaded/saved
RTIMUSettings *settings = new RTIMUSettings("RTIMULib");
RTIMU *imu = RTIMU::createIMU(settings);
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL)) {
printf("No IMU found\n");
exit(1);
}
// This is an opportunity to manually override any settings before the call IMUInit
// set up IMU
imu->IMUInit();
// this is a convenient place to change fusion parameters
imu->setSlerpPower(0.02);
imu->setGyroEnable(true);
imu->setAccelEnable(true);
imu->setCompassEnable(true);
// Set up serial out UART ports
mraa_uart_context uart;
//uart = mraa_uart_init(0);
uart=mraa_uart_init_raw("/dev/ttyGS0");
mraa_uart_set_baudrate(uart, 9600);
if (uart == NULL) {
fprintf(stderr, "UART failed to setup\n");
return 0;
}
char buffer[20]={}; // To hold output
while (1) {
while (imu->IMURead()) {
RTIMU_DATA imuData = imu->getIMUData();
//sleep(1);
printf("%s\r", RTMath::displayDegrees("Gyro", imuData.fusionPose) );
//printf("\nx %f ",imuData.gyro.x()*(180.0/3.14));
//printf("\ny %f ",imuData.gyro.y()*(180.0/3.14));
//printf("\nz %f\n",imuData.gyro.z()*(180.0/3.14));
sprintf(buffer,"%4f\n",imuData.fusionPose.x()*(180.0/3.14));
mraa_uart_write(uart, buffer, sizeof(buffer));
printf("\n\n");
//imuData.
fflush(stdout);
//displayTimer = now;
}
}
mraa_uart_stop(uart);
mraa_deinit();
}
int main()
{
int sampleCount = 0;
int sampleRate = 0;
uint64_t rateTimer;
uint64_t displayTimer;
uint64_t now;
// using RTIMULib here allows it to use the .ini file generated by RTIMULibDemo.
RTIMUSettings *settings = new RTIMUSettings("RTIMULib");
RTIMU *imu = RTIMU::createIMU(settings);
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL)) {
printf("No IMU found\n");
exit(1);
}
// This is an opportunity to manually override any settings before the call IMUInit
// set up IMU
imu->IMUInit();
// this is a convenient place to change fusion parameters
imu->setSlerpPower(0.02);
imu->setGyroEnable(true);
imu->setAccelEnable(true);
imu->setCompassEnable(true);
// set up for rate timer
rateTimer = displayTimer = RTMath::currentUSecsSinceEpoch();
// now just process data
while (1) {
// poll at the rate recommended by the IMU
usleep(imu->IMUGetPollInterval() * 1000);
while (imu->IMURead()) {
RTIMU_DATA imuData = imu->getIMUData();
sampleCount++;
now = RTMath::currentUSecsSinceEpoch();
// display 10 times per second
if ((now - displayTimer) > 100000) {
printf("Sample rate %d: %s\n", sampleRate, RTMath::displayDegrees("", imuData.fusionPose));
if (pressure != NULL) {
printf("Pressure: %4.1f, height above sea level: %4.1f, temperature: %4.1f\n",
imuData.pressure, RTMath::convertPressureToHeight(imuData.pressure), imuData.temperature);
}
fflush(stdout);
displayTimer = now;
}
// update rate every second
if ((now - rateTimer) > 1000000) {
sampleRate = sampleCount;
sampleCount = 0;
rateTimer = now;
}
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "rtimulib_node");
ROS_INFO("Imu driver is now running");
ros::NodeHandle nh("~");
std::string calibration_file_path;
if(!nh.getParam("calibration_file_path", calibration_file_path))
{
ROS_ERROR("The calibration_file_path parameter must be set to use a "
"calibration file.");
ROS_BREAK();
}
std::string calibration_file_name = "RTIMULib";
if(!nh.getParam("calibration_file_name", calibration_file_name))
{
ROS_WARN_STREAM("No calibration_file_name provided - default: "
<< calibration_file_name);
}
std::string frame_id = "imu_link";
if(!nh.getParam("frame_id", frame_id))
{
ROS_WARN_STREAM("No frame_id provided - default: " << frame_id);
}
ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 1);
// Load the RTIMULib.ini config file
RTIMUSettings *settings = new RTIMUSettings(calibration_file_path.c_str(),
calibration_file_name.c_str());
RTIMU *imu = RTIMU::createIMU(settings);
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL))
{
ROS_ERROR("No Imu found");
ROS_BREAK();
}
// Initialise the imu object
imu->IMUInit();
// Set the Fusion coefficient
imu->setSlerpPower(0.02);
// Enable the sensors
imu->setGyroEnable(true);
imu->setAccelEnable(true);
imu->setCompassEnable(true);
sensor_msgs::Imu imu_msg;
while (ros::ok())
{
if (imu->IMURead())
{
RTIMU_DATA imu_data = imu->getIMUData();
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
imu_msg.orientation.x = imu_data.fusionQPose.x();
imu_msg.orientation.y = imu_data.fusionQPose.y();
imu_msg.orientation.z = imu_data.fusionQPose.z();
imu_msg.orientation.w = imu_data.fusionQPose.scalar();
imu_msg.angular_velocity.x = imu_data.gyro.x();
imu_msg.angular_velocity.y = imu_data.gyro.y();
imu_msg.angular_velocity.z = imu_data.gyro.z();
imu_msg.linear_acceleration.x = imu_data.accel.x() * G_TO_MPSS;
imu_msg.linear_acceleration.y = imu_data.accel.y() * G_TO_MPSS;
imu_msg.linear_acceleration.z = imu_data.accel.z() * G_TO_MPSS;
imu_pub.publish(imu_msg);
}
ros::spinOnce();
ros::Duration(imu->IMUGetPollInterval() / 1000.0).sleep();
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "rtimulib_node");
ROS_INFO("Imu driver is running");
ros::NodeHandle n;
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 1);
std::string path_calib_file;
n.getParam("/rtimulib_node/calibration_file_path", path_calib_file);
std::string frame_id;
n.getParam("/rtimulib_node/frame_id", frame_id);
double update_rate;
if(!n.getParam("/rtimulib_node/update_rate", update_rate))
{
ROS_WARN("No update_rate provided - default: 20 Hz");
update_rate = 20;
}
// Load the RTIMULib.ini config file
RTIMUSettings *settings = new RTIMUSettings(path_calib_file.c_str(), "RTIMULib");
RTIMU *imu = RTIMU::createIMU(settings);
if ((imu == NULL) || (imu->IMUType() == RTIMU_TYPE_NULL))
{
ROS_ERROR("No Imu found");
return -1;
}
// Initialise the imu object
imu->IMUInit();
// Set the Fusion coefficient
imu->setSlerpPower(0.02);
// Enable the sensors
imu->setGyroEnable(true);
imu->setAccelEnable(true);
imu->setCompassEnable(true);
ros::Rate loop_rate(update_rate);
while (ros::ok())
{
sensor_msgs::Imu imu_msg;
if (imu->IMURead())
{
RTIMU_DATA imu_data = imu->getIMUData();
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = frame_id;
imu_msg.orientation.x = imu_data.fusionQPose.x();
imu_msg.orientation.y = imu_data.fusionQPose.y();
imu_msg.orientation.z = imu_data.fusionQPose.z();
imu_msg.orientation.w = imu_data.fusionQPose.scalar();
imu_msg.angular_velocity.x = imu_data.gyro.x();
imu_msg.angular_velocity.y = imu_data.gyro.y();
imu_msg.angular_velocity.z = imu_data.gyro.z();
imu_msg.linear_acceleration.x = imu_data.accel.x();
imu_msg.linear_acceleration.y = imu_data.accel.y();
imu_msg.linear_acceleration.z = imu_data.accel.z();
imu_pub.publish(imu_msg);
}
ros::spinOnce();
}
return 0;
}
