C++ (Cpp) FillBufWith32bitの例

コード例 #1

ファイル: ins_arduimu_basic.c プロジェクト: aibara/paparazzi

void ArduIMU_periodicGPS( void ) {

  if (ardu_gps_trans.status != I2CTransDone) { return; }

  // Test for high acceleration:
  //  - low speed
  //  - high thrust
  if (estimator_hspeed_dir < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
    high_accel_flag = TRUE;
  } else {
    high_accel_flag = FALSE;
    if (estimator_hspeed_dir > HIGH_ACCEL_LOW_SPEED && !high_accel_flag) {
      high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
    }
    if (estimator_hspeed_dir < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
      high_accel_done = FALSE; // Activate high accel after landing
    }
  }

  FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D
  FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed);   // ground speed
  FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)DegOfRad(gps.course / 1e6));   // course
  ardu_gps_trans.buf[12] = gps.fix;                              // status gps fix
  ardu_gps_trans.buf[13] = gps_ubx.status_flags;                      // status flags
  ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag;              // high acceleration flag (disable accelerometers in the arduimu filter)
  I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);

}

コード例 #2

ファイル: ins_arduimu_basic.c プロジェクト: AE4317group07/paparazzi

void ArduIMU_periodicGPS(void)
{

  if (ardu_gps_trans.status != I2CTransDone) { return; }

#if USE_HIGH_ACCEL_FLAG
  // Test for high acceleration:
  //  - low speed
  //  - high thrust
  float speed = stateGetHorizontalSpeedNorm_f();
  if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
    high_accel_flag = TRUE;
  } else {
    high_accel_flag = FALSE;
    if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
      high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
    }
    if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
      high_accel_done = FALSE; // Activate high accel after landing
    }
  }
#endif

  FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D
  FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed);   // ground speed
  FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps.course);   // course
  ardu_gps_trans.buf[12] = gps.fix;                               // status gps fix
  ardu_gps_trans.buf[13] = (uint8_t)arduimu_calibrate_neutrals;   // calibration flag
  ardu_gps_trans.buf[14] = (uint8_t)
                           high_accel_flag;              // high acceleration flag (disable accelerometers in the arduimu filter)
  i2c_transmit(&ARDUIMU_I2C_DEV, &ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);

  // Reset calibration flag
  if (arduimu_calibrate_neutrals) { arduimu_calibrate_neutrals = FALSE; }
}

コード例 #3

ファイル: ins_arduimu_basic.c プロジェクト: BrandoJS/Paparazzi_vtol

void ArduIMU_periodicGPS( void ) {

  if (ardu_gps_trans.status != I2CTransDone) { return; }

  FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D
  FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed);   // ground speed
  FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course);   // course
  ardu_gps_trans.buf[12] = gps_mode;                              // status gps fix
  ardu_gps_trans.buf[13] = gps_status_flags;                      // status flags
  I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 14);

}

コード例 #4

ファイル: generic_com.c プロジェクト: 1bitsquared/paparazzi

void generic_com_periodic( void ) {

  if (com_trans.status != I2CTransDone) { return; }

  com_trans.buf[0] = active_com;
  FillBufWith32bit(com_trans.buf, 1, gps.lla_pos.lat);
  FillBufWith32bit(com_trans.buf, 5, gps.lla_pos.lon);
  FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps.lla_pos.alt/1000)); // altitude (meters)
  FillBufWith16bit(com_trans.buf, 11, gps.gspeed); // ground speed (cm/s)
  FillBufWith16bit(com_trans.buf, 13, (int16_t)(gps.course/1e4)); // course (1e3rad)
  FillBufWith16bit(com_trans.buf, 15, (uint16_t)((*stateGetAirspeed_f())*100)); // TAS (cm/s)
  com_trans.buf[17] = electrical.vsupply; // decivolts
  com_trans.buf[18] = (uint8_t)(energy/100); // deciAh
  com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ);
  com_trans.buf[20] = pprz_mode;
  com_trans.buf[21] = nav_block;
  FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time);
  i2c_transmit(&GENERIC_COM_I2C_DEV, &com_trans, GENERIC_COM_SLAVE_ADDR, NB_DATA);

}

コード例 #5

ファイル: ins_arduimu_basic.c プロジェクト: swawa/swawa_PPRZ

void ArduIMU_periodicGPS( void ) {

    if (ardu_gps_trans.status != I2CTransDone) {
        return;
    }

    // Test for high acceleration:
    // - low speed
    // - high thrust
    if (gps_speed_3d < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE]) {
        high_accel_flag = TRUE;
    } else {
        high_accel_flag = FALSE;
    }

    FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D
    FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed); // ground speed
    FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course); // course
    ardu_gps_trans.buf[12] = gps_mode; // status gps fix
    ardu_gps_trans.buf[13] = gps_status_flags; // status flags
    ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter)
    I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);

}