PlotJuggler自定义函数

• 1. 欧拉转换自定义函数
  • 1.1. pitchAngleFromQ()
  • 1.2. rollAngleFromQ()
  • 1.3. yawAngleFromQ()

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); //偏航角
}