Exemplos de update_state_interface em C++ (Cpp)

Exemplo n.º 1

Arquivo: ins_xsens.c Projeto: Pold87/paparazzi_new

static void handle_ins_msg(void)
{

  update_state_interface();

  if (xsens.new_attitude) {
    new_ins_attitude = true;
    xsens.new_attitude = false;
  }

#if USE_GPS_XSENS
  if (xsens.gps_available) {
    // Horizontal speed
    float fspeed = FLOAT_VECT2_NORM(xsens.vel);
    if (xsens.gps.fix != GPS_FIX_3D) {
      fspeed = 0;
    }
    xsens.gps.gspeed = fspeed * 100.;
    xsens.gps.speed_3d = float_vect3_norm(&xsens.vel) * 100;

    float fcourse = atan2f(xsens.vel.y, xsens.vel.x);
    xsens.gps.course = fcourse * 1e7;
    SetBit(xsens.gps.valid_fields, GPS_VALID_COURSE_BIT);

    gps_xsens_publish();
    xsens.gps_available = false;
  }
#endif // USE_GPS_XSENS
}

Exemplo n.º 2

Arquivo: ins_xsens.c Projeto: pulver22/paparazzi

void handle_ins_msg(void)
{

#if USE_INS_MODULE
    update_state_interface();
#endif

#if USE_IMU
#ifdef XSENS_BACKWARDS
    if (imu_xsens.gyro_available) {
        RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(ins_p), -RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
    }
    if (imu_xsens.accel_available) {
        VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(ins_ax), -ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
    }
    if (imu_xsens.mag_available) {
        VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(ins_mx), -MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
    }
#else
    if (imu_xsens.gyro_available) {
        RATES_ASSIGN(imu.gyro_unscaled, RATE_BFP_OF_REAL(ins_p), RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
    }
    if (imu_xsens.accel_available) {
        VECT3_ASSIGN(imu.accel_unscaled, ACCEL_BFP_OF_REAL(ins_ax), ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
    }
    if (imu_xsens.mag_available) {
        VECT3_ASSIGN(imu.mag_unscaled, MAG_BFP_OF_REAL(ins_mx), MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
    }
#endif /* XSENS_BACKWARDS */
#endif /* USE_IMU */

#if USE_GPS_XSENS

    // Horizontal speed
    float fspeed = sqrt(ins_vx * ins_vx + ins_vy * ins_vy);
    if (gps.fix != GPS_FIX_3D) {
        fspeed = 0;
    }
    gps.gspeed = fspeed * 100.;
    gps.speed_3d = (uint16_t)(sqrt(ins_vx * ins_vx + ins_vy * ins_vy + ins_vz * ins_vz) * 100);

    float fcourse = atan2f((float)ins_vy, (float)ins_vx);
    gps.course = fcourse * 1e7;
#endif // USE_GPS_XSENS
}

Exemplo n.º 3

Arquivo: ins_xsens700.c Projeto: CodeMining/paparazzi

void handle_ins_msg(void)
{

#if USE_INS_MODULE
  update_state_interface();
#endif

#if USE_GPS_XSENS
  // Horizontal speed
  float fspeed = sqrt(ins_vx * ins_vx + ins_vy * ins_vy);
  if (gps.fix != GPS_FIX_3D) {
    fspeed = 0;
  }
  gps.gspeed = fspeed * 100.;
  gps.speed_3d = (uint16_t)(sqrt(ins_vx * ins_vx + ins_vy * ins_vy + ins_vz * ins_vz) * 100);

  float fcourse = atan2f((float)ins_vy, (float)ins_vx);
  gps.course = fcourse * 1e7;
#endif // USE_GPS_XSENS
}