C++ (Cpp) DOWNLINK_SEND_IMU_MAG примеры использования

Пример #1

Файл: ahrs_float_dcm.c Проект: jmavi/paparazzi_fix

void ahrs_dcm_update_mag(struct Int32Vect3 *mag)
{
#if USE_MAGNETOMETER
#warning MAGNETOMETER FEEDBACK NOT TESTED YET

  MAG_FLOAT_OF_BFP(mag_float, *mag);

  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;

  cos_roll = cosf(ahrs_dcm.ltp_to_imu_euler.phi);
  sin_roll = sinf(ahrs_dcm.ltp_to_imu_euler.phi);
  cos_pitch = cosf(ahrs_dcm.ltp_to_imu_euler.theta);
  sin_pitch = sinf(ahrs_dcm.ltp_to_imu_euler.theta);


  // Pitch&Roll Compensation:
  MAG_Heading_X = mag->x * cos_pitch + mag->y * sin_roll * sin_pitch + mag->z * cos_roll * sin_pitch;
  MAG_Heading_Y = mag->y * cos_roll - mag->z * sin_roll;

  /*
   *
    // Magnetic Heading
    Heading = atan2(-Head_Y,Head_X);

    // Declination correction (if supplied)
    if( declination != 0.0 )
    {
        Heading = Heading + declination;
        if (Heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
            Heading -= (2.0 * M_PI);
        else if (Heading < -M_PI)
            Heading += (2.0 * M_PI);
    }

    // Optimization for external DCM use. Calculate normalized components
    Heading_X = cos(Heading);
    Heading_Y = sin(Heading);
  */

  struct FloatVect3 ltp_mag;

  ltp_mag.x = MAG_Heading_X;
  ltp_mag.y = MAG_Heading_Y;

#if FLOAT_DCM_SEND_DEBUG
  // Downlink
  RunOnceEvery(10, DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
#endif

  // Magnetic Heading
  // MAG_Heading = atan2(mag->y, -mag->x);

#else // !USE_MAGNETOMETER
  // get rid of unused param warning...
  mag = mag;
#endif
}

Пример #2

Файл: ahrs_float_dcm.c Проект: maxcht/paparazzi

void ahrs_update_mag(void)
{
#ifdef USE_MAGNETOMETER
#warning MAGNETOMETER FEEDBACK NOT TESTED YET

  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;

  cos_roll = cos(ahrs_float.ltp_to_imu_euler.phi);
  sin_roll = sin(ahrs_float.ltp_to_imu_euler.phi);
  cos_pitch = cos(ahrs_float.ltp_to_imu_euler.theta);
  sin_pitch = sin(ahrs_float.ltp_to_imu_euler.theta);


  // Pitch&Roll Compensation:
  MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch;
  MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll;

/*
 *
  // Magnetic Heading
  Heading = atan2(-Head_Y,Head_X);

  // Declination correction (if supplied)
  if( declination != 0.0 )
  {
      Heading = Heading + declination;
      if (Heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
          Heading -= (2.0 * M_PI);
      else if (Heading < -M_PI)
          Heading += (2.0 * M_PI);
  }

  // Optimization for external DCM use. Calculate normalized components
  Heading_X = cos(Heading);
  Heading_Y = sin(Heading);
*/

  struct FloatVect3 ltp_mag;

  ltp_mag.x = MAG_Heading_X;
  ltp_mag.y = MAG_Heading_Y;

  // Downlink
  RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));

  // Magnetic Heading
  // MAG_Heading = atan2(imu.mag.y, -imu.mag.x);
#endif
}

Пример #3

Файл: imu.c Проект: Aishwaryabr/paparazzi

static void send_mag(void) {
  struct FloatVect3 mag_float;
  MAGS_FLOAT_OF_BFP(mag_float, imu.mag);
  DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice,
      &mag_float.x, &mag_float.y, &mag_float.z);
}

Пример #4

Файл: ahrs_float_dcm.c Проект: rijesha/paparazzi

              Heading += (2.0 * M_PI);
      }

      // Optimization for external DCM use. Calculate normalized components
      Heading_X = cos(Heading);
      Heading_Y = sin(Heading);
    */

#if FLOAT_DCM_SEND_DEBUG
    struct FloatVect3 ltp_mag;

    ltp_mag.x = MAG_Heading_X;
    ltp_mag.y = MAG_Heading_Y;

    // Downlink
    RunOnceEvery(10, DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
#endif

    // Magnetic Heading
    // MAG_Heading = atan2(mag->y, -mag->x);

#else // !USE_MAGNETOMETER
    // get rid of unused param warning...
    mag = mag;
#endif
}

void Normalize(void)
{
    float error = 0;
    float temporary[3][3];