1. 欧拉转换自定义函数
1.1. pitchAngleFromQ()
说明
从四元数计算俯仰角(角度)。
var M_PI = 3.14159265358979323846;
function pitchAngleFromQ(time, value) {
// 粘贴至PlotJuggler时请确保vehicle_attitude/q.0*间无空格
var a = $$vehicle_attitude/q.00$$;
var b = $$vehicle_attitude/q.01$$;
var c = $$vehicle_attitude/q.02$$;
var d = $$vehicle_attitude/q.03$$;
var aa = a * a;
var ab = a * b;
var ac = a * c;
var ad = a * d;
var bb = b * b;
var bc = b * c;
var bd = b * d;
var cc = c * c;
var cd = c * d;
var dd = d * d;
var dcm00 = aa + bb - cc - dd;
var dcm01 = 2.0 * (bc - ad);
var dcm02 = 2.0 * (ac + bd);
var dcm10 = 2.0 * (bc + ad);
var dcm11 = aa - bb + cc - dd;
var dcm12 = 2.0 * (cd - ab);
var dcm20 = 2.0 * (bd - ac);
var dcm21 = 2.0 * (ab + cd);
var dcm22 = aa - bb - cc + dd;
var phi_val = atan2(dcm21, dcm22);
var theta_val = asin(-dcm20);
var psi_val = atan2(dcm10, dcm00);
var pi = M_PI;
if (Math.abs(theta_val - pi / 2) < 1.0e-3) {
phi_val = 0;
psi_val = atan2(dcm12, dcm02);
} else if (Math.abs(theta_val + pi / 2) < 1.0e-3) {
phi_val = 0;
psi_val = atan2(-dcm12, -dcm02);
}
return theta_val * (180 / M_PI); //俯仰角
}
1.2. rollAngleFromQ()
说明
从四元数计算滚转角(角度)。
var M_PI = 3.14159265358979323846;
function rollAngleFromQ(time, value) {
// 粘贴至PlotJuggler时请确保vehicle_attitude/q.0*间无空格
var a = $$vehicle_attitude/q.00$$;
var b = $$vehicle_attitude/q.01$$;
var c = $$vehicle_attitude/q.02$$;
var d = $$vehicle_attitude/q.03$$;
var aa = a * a;
var ab = a * b;
var ac = a * c;
var ad = a * d;
var bb = b * b;
var bc = b * c;
var bd = b * d;
var cc = c * c;
var cd = c * d;
var dd = d * d;
var dcm00 = aa + bb - cc - dd;
var dcm01 = 2.0 * (bc - ad);
var dcm02 = 2.0 * (ac + bd);
var dcm10 = 2.0 * (bc + ad);
var dcm11 = aa - bb + cc - dd;
var dcm12 = 2.0 * (cd - ab);
var dcm20 = 2.0 * (bd - ac);
var dcm21 = 2.0 * (ab + cd);
var dcm22 = aa - bb - cc + dd;
var phi_val = atan2(dcm21, dcm22);
var theta_val = asin(-dcm20);
var psi_val = atan2(dcm10, dcm00);
var pi = M_PI;
if (Math.abs(theta_val - pi / 2) < 1.0e-3) {
phi_val = 0;
psi_val = atan2(dcm12, dcm02);
} else if (Math.abs(theta_val + pi / 2) < 1.0e-3) {
phi_val = 0;
psi_val = atan2(-dcm12, -dcm02);
}
return phi_val * (180 / M_PI); //滚转角
}
1.3. yawAngleFromQ()
说明
从四元数计算偏航角(角度)。
var M_PI = 3.14159265358979323846;
function yawAngleFromQ(time, value) {
// 粘贴至PlotJuggler时请确保vehicle_attitude/q.0*间无空格
var a = $$vehicle_attitude/q.00$$;
var b = $$vehicle_attitude/q.01$$;
var c = $$vehicle_attitude/q.02$$;
var d = $$vehicle_attitude/q.03$$;
var aa = a * a;
var ab = a * b;
var ac = a * c;
var ad = a * d;
var bb = b * b;
var bc = b * c;
var bd = b * d;
var cc = c * c;
var cd = c * d;
var dd = d * d;
var dcm00 = aa + bb - cc - dd;
var dcm01 = 2.0 * (bc - ad);
var dcm02 = 2.0 * (ac + bd);
var dcm10 = 2.0 * (bc + ad);
var dcm11 = aa - bb + cc - dd;
var dcm12 = 2.0 * (cd - ab);
var dcm20 = 2.0 * (bd - ac);
var dcm21 = 2.0 * (ab + cd);
var dcm22 = aa - bb - cc + dd;
var phi_val = atan2(dcm21, dcm22);
var theta_val = asin(-dcm20);
var psi_val = atan2(dcm10, dcm00);
var pi = M_PI;
if (Math.abs(theta_val - pi / 2) < 1.0e-3) {
phi_val = 0;
psi_val = atan2(dcm12, dcm02);
} else if (Math.abs(theta_val + pi / 2) < 1.0e-3) {
phi_val = 0;
psi_val = atan2(-dcm12, -dcm02);
}
return psi_val * (180 / M_PI); //偏航角
}