C++ (Cpp) _itofQ16 예제들

예제 #1

파일: UART1.c 프로젝트: hariharsubramanyam/UAV_Team_Sem1

void UART1_SendAHRSpacket()
{

    _Q16 pErr = AHRSdata.p - CmdData.p;
    _Q16 qErr = AHRSdata.q - CmdData.q;
    _Q16 rErr = AHRSdata.r - CmdData.r;

    tAHRSPacket pkt;

    /*
    pkt.qw = (int16_t)(_itofQ16(mult(AHRSdata.q_est.o, num10000)));
    
    pkt.qx = (int16_t)(_itofQ16(mult(AHRSdata.q_est.x, num10000)));
    pkt.qy = (int16_t)(_itofQ16(mult(AHRSdata.q_est.y, num10000)));
    pkt.qz = (int16_t)(_itofQ16(mult(AHRSdata.q_est.z, num10000)));
    
    
    pkt.qx = (int16_t)(_itofQ16(mult(AHRSdata.p, num1000)));
    pkt.qy = (int16_t)(_itofQ16(mult(AHRSdata.q, num1000)));
    pkt.qz = (int16_t)(_itofQ16(mult(AHRSdata.r, num1000)));
    
    
    pkt.p = (int16_t)(_itofQ16(mult(qerror.x, num1000)));
    pkt.q = (int16_t)(_itofQ16(mult(qerror.y, num1000)));
    pkt.r = (int16_t)(_itofQ16(mult(qerror.z, num1000)));
    */


    pkt.p = (int16_t)(_itofQ16(mult(AHRSdata.q_est.x, num1000)));
    pkt.q = (int16_t)(_itofQ16(mult(AHRSdata.q_est.y, num1000)));
    pkt.r = (int16_t)(_itofQ16(mult(AHRSdata.q_est.z, num1000)));

    pkt.qw = (int16_t)(_itofQ16(mult(AHRSdata.q_est.o, num1000)));
    pkt.qx = (int16_t)(_itofQ16(mult(CmdData.q_cmd.x, num1000)));
    pkt.qy = (int16_t)(_itofQ16(mult(CmdData.q_cmd.y, num1000)));
    pkt.qz = (int16_t)(_itofQ16(mult(CmdData.q_cmd.z, num1000)));
    /*
    pkt.p = (int16_t)(_itofQ16(mult(pErr, num1000)));
    pkt.q = (int16_t)(_itofQ16(mult(qErr, num1000)));
    pkt.r = (int16_t)(_itofQ16(mult(rErr, num1000)));
*/
    UART1_SendPacket(PACKETID_AHRS, sizeof(tAHRSPacket), &pkt);
}

예제 #2

파일: attitude_q.c 프로젝트: OlinRoboticsAndBioinspiration/DynaCam

float attGetRoll(void) {
    return _itofQ16(PoseQ.qdata.phi);
}

예제 #3

파일: attitude_q.c 프로젝트: OlinRoboticsAndBioinspiration/DynaCam

float attGetPitch(void) {
    return _itofQ16(PoseQ.qdata.theta);
}

예제 #4

파일: attitude_q.c 프로젝트: OlinRoboticsAndBioinspiration/DynaCam

float attGetYaw(void) {
    return _itofQ16(PoseQ.qdata.psi);
}