C++ (Cpp) DOWNLINK_SEND_IMU_ACCEL 예제들

예제 #1

파일: ins_arduimu_basic.c 프로젝트: AE4317group07/paparazzi

void ArduIMU_event(void)
{
  // Handle INS I2C event
  if (ardu_ins_trans.status == I2CTransSuccess) {
    // received data from I2C transaction
    recievedData[0] = (ardu_ins_trans.buf[1] << 8) | ardu_ins_trans.buf[0];
    recievedData[1] = (ardu_ins_trans.buf[3] << 8) | ardu_ins_trans.buf[2];
    recievedData[2] = (ardu_ins_trans.buf[5] << 8) | ardu_ins_trans.buf[4];
    recievedData[3] = (ardu_ins_trans.buf[7] << 8) | ardu_ins_trans.buf[6];
    recievedData[4] = (ardu_ins_trans.buf[9] << 8) | ardu_ins_trans.buf[8];
    recievedData[5] = (ardu_ins_trans.buf[11] << 8) | ardu_ins_trans.buf[10];
    recievedData[6] = (ardu_ins_trans.buf[13] << 8) | ardu_ins_trans.buf[12];
    recievedData[7] = (ardu_ins_trans.buf[15] << 8) | ardu_ins_trans.buf[14];
    recievedData[8] = (ardu_ins_trans.buf[17] << 8) | ardu_ins_trans.buf[16];

    // Update ArduIMU data
    arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]) - ins_roll_neutral;
    arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]) - ins_pitch_neutral;
    arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]);
    arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]);
    arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]);
    arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]);
    arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]);
    arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]);
    arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]);

    // Update estimator
    stateSetNedToBodyEulers_f(&arduimu_eulers);
    stateSetBodyRates_f(&arduimu_rates);
    stateSetAccelNed_f(&((struct NedCoor_f)arduimu_accel));
    ardu_ins_trans.status = I2CTransDone;

#ifdef ARDUIMU_SYNC_SEND
    // uint8_t arduimu_id = 102;
    //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi, &arduimu_id));
    RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q,
                                            &arduimu_rates.r));
    RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y,
                 &arduimu_accel.z));
#endif
  } else if (ardu_ins_trans.status == I2CTransFailed) {
    ardu_ins_trans.status = I2CTransDone;
  }
  // Handle GPS I2C event
  if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) {
    ardu_gps_trans.status = I2CTransDone;
  }
}

예제 #2

파일: ins_arduimu_basic.c 프로젝트: swawa/swawa_PPRZ

void ArduIMU_event( void ) {
    // Handle INS I2C event
    if (ardu_ins_trans.status == I2CTransSuccess) {
        // received data from I2C transaction
        recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0];
        recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2];
        recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4];
        recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6];
        recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8];
        recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
        recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12];
        recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14];
        recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16];

        // Update ArduIMU data
        arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]);
        arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]);
        arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]);
        arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]);
        arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]);
        arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]);
        arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]);
        arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]);
        arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]);

        // Update estimator
        estimator_phi = arduimu_eulers.phi - ins_roll_neutral;
        estimator_theta = arduimu_eulers.theta - ins_pitch_neutral;
        estimator_p = arduimu_rates.p;
        ardu_ins_trans.status = I2CTransDone;

        //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
        RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
        RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
    }
    else if (ardu_ins_trans.status == I2CTransFailed) {
        ardu_ins_trans.status = I2CTransDone;
    }
    // Handle GPS I2C event
    if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) {
        ardu_gps_trans.status = I2CTransDone;
    }
}

예제 #3

파일: imu.c 프로젝트: Aishwaryabr/paparazzi

static void send_accel(void) {
  struct FloatVect3 accel_float;
  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
  DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice,
      &accel_float.x, &accel_float.y, &accel_float.z);
}

예제 #4

파일: imu.c 프로젝트: Aishwaryabr/paparazzi

static void send_accel(void) {
  DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice,
      &imuf.accel.x, &imuf.accel.y, &imuf.accel.z);
}