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); }