void GazeboMavlinkInterface::ImuCallback(ImuPtr& imu_message) {
math::Pose T_W_I = model_->GetWorldPose();
math::Vector3 pos_W_I = T_W_I.pos; // Use the models'world position for GPS and pressure alt.
math::Quaternion C_W_I;
C_W_I.w = imu_message->orientation().w();
C_W_I.x = imu_message->orientation().x();
C_W_I.y = imu_message->orientation().y();
C_W_I.z = imu_message->orientation().z();
math::Vector3 mag_I = C_W_I.RotateVectorReverse(mag_W_); // TODO: Add noise based on bais and variance like for imu and gyro
math::Vector3 body_vel = C_W_I.RotateVectorReverse(model_->GetWorldLinearVel());
standard_normal_distribution_ = std::normal_distribution<float>(0, 0.01f);
float mag_noise = standard_normal_distribution_(random_generator_);
if(use_mavlink_udp){
mavlink_hil_sensor_t sensor_msg;
sensor_msg.time_usec = world_->GetSimTime().nsec*1000;
sensor_msg.xacc = imu_message->linear_acceleration().x();
sensor_msg.yacc = imu_message->linear_acceleration().y();
sensor_msg.zacc = imu_message->linear_acceleration().z();
sensor_msg.xgyro = imu_message->angular_velocity().x();
sensor_msg.ygyro = imu_message->angular_velocity().y();
sensor_msg.zgyro = imu_message->angular_velocity().z();
sensor_msg.xmag = mag_I.x + mag_noise;
sensor_msg.ymag = mag_I.y + mag_noise;
sensor_msg.zmag = mag_I.z + mag_noise;
sensor_msg.abs_pressure = 0.0;
sensor_msg.diff_pressure = 0.5*1.2754*body_vel.x*body_vel.x;
sensor_msg.pressure_alt = pos_W_I.z;
sensor_msg.temperature = 0.0;
sensor_msg.fields_updated = 4095;
send_mavlink_message(MAVLINK_MSG_ID_HIL_SENSOR, &sensor_msg, 200);
} else{
hil_sensor_msg_.set_time_usec(world_->GetSimTime().nsec*1000);
hil_sensor_msg_.set_xacc(imu_message->linear_acceleration().x());
hil_sensor_msg_.set_yacc(imu_message->linear_acceleration().y());
hil_sensor_msg_.set_zacc(imu_message->linear_acceleration().z());
hil_sensor_msg_.set_xgyro(imu_message->angular_velocity().x());
hil_sensor_msg_.set_ygyro(imu_message->angular_velocity().y());
hil_sensor_msg_.set_zgyro(imu_message->angular_velocity().z());
hil_sensor_msg_.set_xmag(mag_I.x);
hil_sensor_msg_.set_ymag(mag_I.y);
hil_sensor_msg_.set_zmag(mag_I.z);
hil_sensor_msg_.set_abs_pressure(0.0);
hil_sensor_msg_.set_diff_pressure(0.5*1.2754*body_vel.x*body_vel.x);
hil_sensor_msg_.set_pressure_alt(pos_W_I.z);
hil_sensor_msg_.set_temperature(0.0);
hil_sensor_msg_.set_fields_updated(4095); // 0b1111111111111 (All updated since new data with new noise added always)
hil_sensor_pub_->Publish(hil_sensor_msg_);
}
}
void GazeboMavlinkInterface::ImuCallback(ImuPtr& imu_message) {
math::Pose T_W_I = model_->GetWorldPose();
math::Vector3 pos_W_I = T_W_I.pos; // Use the models'world position for GPS and pressure alt.
math::Quaternion C_W_I;
C_W_I.w = imu_message->orientation().w();
C_W_I.x = imu_message->orientation().x();
C_W_I.y = imu_message->orientation().y();
C_W_I.z = imu_message->orientation().z();
// gzerr << "got imu: " << C_W_I << "\n";
float declination = get_mag_declination(lat_rad, lon_rad);
math::Quaternion C_D_I(0.0, 0.0, declination);
math::Vector3 mag_decl = C_D_I.RotateVectorReverse(mag_W_);
// TODO replace mag_W_ in the line below with mag_decl
math::Vector3 mag_I = C_W_I.RotateVectorReverse(mag_decl); // TODO: Add noise based on bais and variance like for imu and gyro
math::Vector3 body_vel = C_W_I.RotateVectorReverse(model_->GetWorldLinearVel());
standard_normal_distribution_ = std::normal_distribution<float>(0, 0.01f);
float mag_noise = standard_normal_distribution_(random_generator_);
mavlink_hil_sensor_t sensor_msg;
sensor_msg.time_usec = world_->GetSimTime().nsec*1000;
sensor_msg.xacc = imu_message->linear_acceleration().x();
sensor_msg.yacc = imu_message->linear_acceleration().y();
sensor_msg.zacc = imu_message->linear_acceleration().z();
sensor_msg.xgyro = imu_message->angular_velocity().x();
sensor_msg.ygyro = imu_message->angular_velocity().y();
sensor_msg.zgyro = imu_message->angular_velocity().z();
sensor_msg.xmag = mag_I.x + mag_noise;
sensor_msg.ymag = mag_I.y + mag_noise;
sensor_msg.zmag = mag_I.z + mag_noise;
sensor_msg.abs_pressure = 0.0;
sensor_msg.diff_pressure = 0.5*1.2754*(body_vel.z + body_vel.x)*(body_vel.z + body_vel.x) / 100;
sensor_msg.pressure_alt = pos_W_I.z;
sensor_msg.temperature = 0.0;
sensor_msg.fields_updated = 4095;
//gyro needed for optical flow message
optflow_xgyro = imu_message->angular_velocity().x();
optflow_ygyro = imu_message->angular_velocity().y();
optflow_zgyro = imu_message->angular_velocity().z();
send_mavlink_message(MAVLINK_MSG_ID_HIL_SENSOR, &sensor_msg, 200);
}
