void Options::update_quaternion()
{
quaternion = Quaternionf(rotation_x, rotation_y, rotation_z, order_YXZ);
write_quaternion();
update_all_slider_text();
}
void Options::slider_quaternion_k_changed()
{
float value = quaternion_k_view->get_value(-1.0f, 1.0f);
for (int cnt=0; cnt < 32; cnt++) // Iterate, so value quaternion is transformed in a normalised way
{
quaternion.k = value;
quaternion.normalize();
}
write_quaternion();
update_euler();
}
int main(int argc, char **argv)
{
// enableMuxer(0b00000011);
if(argc != 5) {
printf("Please supply address for lsm, lis, topic, name\n");
exit(0);
}
int lsmaddr, lisaddr;
lsmaddr = (int) strtol(argv[1], NULL, 16);
lisaddr = (int) strtol(argv[2], NULL, 16);
ros::init(argc, argv, argv[3]);
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<sensor_msgs::Imu>(argv[4], 1000);
std::string frame = "1";
// Initializa IMU
MinIMU9 imu("/dev/i2c-1", lsmaddr, lisaddr);
imu.loadCalibration();
imu.enable();
imu.measureOffsets();
int count = 0;
ros::Rate loop_rate(100);
quaternion rotation = quaternion::Identity();
int start = millis();
while (ros::ok())
{
int last_start = start;
start = millis();
float dt = (start-last_start)/1000.0;
if (dt < 0){ throw std::runtime_error("Time went backwards."); }
vector angular_velocity = imu.readGyro();
vector acceleration = imu.readAcc();
vector magnetic_field = imu.readMag();
fuse_default(rotation, dt, angular_velocity, acceleration, magnetic_field);
sensor_msgs::Imu msg;
msg.header.frame_id = frame;
msg.header.stamp = ros::Time::now();
output_quaternion(rotation);
std::cout << " " << acceleration << " " << magnetic_field << std::endl << std::flush;
write_quaternion(msg, rotation);
msg.linear_acceleration.x = acceleration[0];
msg.linear_acceleration.y = acceleration[1];
msg.linear_acceleration.z = acceleration[2];
msg.angular_velocity.x = angular_velocity[0];
msg.angular_velocity.y = angular_velocity[1];
msg.angular_velocity.z = angular_velocity[2];
chatter_pub.publish(msg);
// Ensure that each iteration of the loop takes at least 20 ms.
/* while(millis() - start < 20)
{
usleep(1000);
}*/
}
return 0;
}
void Options::set_new_quaternion(const Quaternionf &new_quaternion)
{
quaternion = new_quaternion;
write_quaternion();
update_euler();
}