Exemplo n.º 1
0
void AHRSClass::update(float dt)
{
	getSensors();
	MerayoCalibClass::apply(&scaledSensorData.x, &scaledSensorData.y, &scaledSensorData.z, &accCalibData);
	fuse(dt);
	transformOrientation();

}
Exemplo n.º 2
0
geometry_msgs::Pose transformPose2(geometry_msgs::Pose pose_in, Eigen::Matrix4f tf){
	geometry_msgs::Pose pose_out;
	pose_out.position = transformPoint(pose_in.position, tf);
	pose_out.orientation = transformOrientation(pose_in.orientation, tf);
	return pose_out;
}