Ejemplos de quat_to_rpy en C++ (Cpp)

Ejemplo n.º 1

Archivo: basestation.cpp Proyecto: matthewbot/FlamewheelFC

static void send_status_message() {
    INSCompDebugState attstate = inscomp_get_debug_state();
    VectorF<3> rpy = quat_to_rpy(attstate.quat);
    ControllerDebug controllerdebug = controller_get_debug();
    ControllerGains controllergains = controller_get_gains();

    msg.id = MSGID_STATUS;

    msg.roll = float16(rpy[0], 4);
    msg.pitch = float16(rpy[1], 4);
    msg.yaw = float16(rpy[2], 4);

    msg.roll_rate = float16(attstate.rate[0], 4);
    msg.pitch_rate = float16(attstate.rate[1], 4);
    msg.yaw_rate = float16(attstate.rate[2], 4);

    msg.roll_bias = float16(attstate.bias_gyro[0], 4);
    msg.pitch_bias = float16(attstate.bias_gyro[1], 4);
    msg.yaw_bias = float16(attstate.bias_gyro[2], 4);

    msg.roll_p = float16(controllerdebug.pout[0], 4);
    msg.pitch_p = float16(controllerdebug.pout[1], 4);
    msg.yaw_p = float16(controllerdebug.pout[2], 4);

    msg.roll_d = float16(controllerdebug.dout[0], 4);
    msg.pitch_d = float16(controllerdebug.dout[1], 4);
    msg.yaw_d = float16(controllerdebug.dout[2], 4);

    msg.gain_roll_p = float16(controllergains.p[0], 4);
    msg.gain_pitch_p = float16(controllergains.p[1], 4);
    msg.gain_yaw_p = float16(controllergains.p[2], 4);

    msg.gain_roll_d = float16(controllergains.d[0], 4);
    msg.gain_pitch_d = float16(controllergains.d[1], 4);
    msg.gain_yaw_d = float16(controllergains.d[2], 4);

    msg.mag_x_bias = float16(attstate.bias_mag[0], 4);
    msg.mag_y_bias = float16(attstate.bias_mag[1], 4);
    msg.mag_z_bias = float16(attstate.bias_mag[2], 4);

    msg.esc_fl = float16(controllerdebug.motors[0], 4);
    msg.esc_fr = float16(controllerdebug.motors[1], 4);
    msg.esc_rr = float16(controllerdebug.motors[2], 4);
    msg.esc_rl = float16(controllerdebug.motors[3], 4);

    msg.altitude = float16(altitude_get(), 3);
    msg.altitude_rate = float16(altitude_get_rate(), 3);

    msg.battery = float16(board_get_voltage(), 3);

    XBeeSendResponse resp = xbee_send(1, reinterpret_cast<const char *>(&msg), sizeof(msg));
    valid = (resp == XBeeSendResponse::SUCCESS);
}

Ejemplo n.º 2

Archivo: publish_tags.cpp Proyecto: rdeits/crazytags

crazyflie_t::webcam_pos_t encodeWebcamPos(Eigen::Isometry3d const & frame) 
{
    Eigen::Vector3d t(frame.translation());
    Eigen::Quaterniond r(frame.rotation());
    Eigen::Vector3d rpy = quat_to_rpy(r);

    crazyflie_t::webcam_pos_t msg;
    msg.x = frame.translation().x();
    msg.y = frame.translation().y();
    msg.z = frame.translation().z();

    msg.roll = rpy(0);
    msg.pitch = rpy(1);
    msg.yaw = rpy(2);

    return msg;
}