void CalibrateData::callbackAccelGyro(const sensor_msgs::ImuConstPtr & msg)
{
Eigen::Vector3d accel_raw;
Eigen::Vector3d accel_cal;
Eigen::Vector3d gyro_raw;
Eigen::Vector3d gyro_cal;
accel_raw(0) = msg->linear_acceleration.x;
accel_raw(1) = msg->linear_acceleration.y;
accel_raw(2) = msg->linear_acceleration.z;
gyro_raw(0) = msg->angular_velocity.x;
gyro_raw(1) = msg->angular_velocity.y;
gyro_raw(2) = msg->angular_velocity.z;
accel_cal = calibrate(accel_raw,
accel_alignment_matrix_,
accel_sensitivity_matrix_,
accel_offset_vector_);
gyro_cal = calibrate(gyro_raw,
gyro_alignment_matrix_,
gyro_sensitivity_matrix_,
gyro_offset_vector_);
sensor_msgs::Imu imu_msg = *msg;
imu_msg.linear_acceleration.x = accel_cal(0);
imu_msg.linear_acceleration.y = accel_cal(1);
imu_msg.linear_acceleration.z = accel_cal(2);
imu_msg.angular_velocity.x = gyro_cal(0) * M_PI / 180.0;
imu_msg.angular_velocity.y = gyro_cal(1) * M_PI / 180.0;
imu_msg.angular_velocity.z = gyro_cal(2) * M_PI / 180.0;
imu_pub_.publish(imu_msg);
}
// Set baseline MPU readings at init
void FC_IMU::set_baseline() {
int16_t tmp_acc[3] = {0, 0, 0};
int16_t tmp_gyro[3] = {0, 0, 0};
int32_t acc_tot[3] = {0, 0, 0};
int32_t gyro_tot[3] = {0, 0, 0};
for (int i = 20; i --> 0;) {
accel_raw(tmp_acc);
gyro_raw(tmp_gyro);
for (int j = 0; j < 3; j++) {
acc_tot[j] += (int32_t)tmp_acc[j];
gyro_tot[j] += (int32_t)tmp_gyro[j];
}
// Serial.print("Acc: ");
// Serial.print(tmp_acc[0]);
// Serial.print(" ");
// Serial.print(tmp_acc[1]);
// Serial.print(" ");
// Serial.print(tmp_acc[2]);
// Serial.print(" Gyro: ");
// Serial.print(tmp_gyro[0]);
// Serial.print(" ");
// Serial.print(tmp_gyro[1]);
// Serial.print(" ");
// Serial.println(tmp_gyro[2]);
delay(100);
}
// Serial.print("Acc: ");
// Serial.print(acc_tot[0]);
// Serial.print(" ");
// Serial.print(acc_tot[1]);
// Serial.print(" ");
// Serial.print(acc_tot[2]);
// Serial.print(" Gyro: ");
// Serial.print(gyro_tot[0]);
// Serial.print(" ");
// Serial.print(gyro_tot[1]);
// Serial.print(" ");
// Serial.println(gyro_tot[2]);
for (int i = 0; i < 3; i++) {
_accel_baseline[i] = (int16_t)(acc_tot[i] / 20);
_gyro_baseline[i] = (int16_t)(gyro_tot[i] / 20);
}
}