Esempi in C++ (Cpp) per MAG_FLOAT_OF_BFP

Esempio n. 1

File: ahrs_int_cmpl_quat.c Progetto: AULA-PPZ/paparazzi

static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) {
  struct FloatVect3 h_float;
  h_float.x = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.x);
  h_float.y = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.y);
  h_float.z = MAG_FLOAT_OF_BFP(ahrs_impl.mag_h.z);
  pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
                        &h_float.x, &h_float.y, &h_float.z);
}

Esempio n. 2

File: fms_spi_autopilot_msg.c Progetto: AntoineBlais/paparazzi

static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up)
{

  if (msg_up->valid.vane && vane_callback)
    vane_callback(0, msg_up->vane_angle1, msg_up->vane_angle2);

  // Fill pressure data
  if (msg_up->valid.pressure_absolute && pressure_absolute_callback)
    pressure_absolute_callback(0, msg_up->pressure_absolute);

  if (msg_up->valid.pressure_differential && pressure_differential_callback)
    pressure_differential_callback(0, (32768 + msg_up->pressure_differential));

	if (msg_up->valid.adc) {
		if(adc_callback) {
			adc_callback(msg_up->adc.channels);
		}
	}

  // Fill radio data
  if (msg_up->valid.rc && radio_control_callback) {
    radio_control.values[RADIO_ROLL] = msg_up->rc_roll;
    radio_control.values[RADIO_PITCH] = msg_up->rc_pitch;
    radio_control.values[RADIO_YAW] = msg_up->rc_yaw;
    radio_control.values[RADIO_THROTTLE] = msg_up->rc_thrust;
    radio_control.values[RADIO_MODE] = msg_up->rc_mode;
    radio_control.values[RADIO_KILL] = msg_up->rc_kill;
    radio_control.values[RADIO_GEAR] = msg_up->rc_gear;
    radio_control.values[RADIO_AUX2] = msg_up->rc_aux2;
    radio_control.values[RADIO_AUX3] = msg_up->rc_aux3;
    radio_control_callback();
  }
  // always fill status, it may change even when in the case when there is no new data
  radio_control.status = msg_up->rc_status;

  // Fill IMU data
  imuFloat.gyro.p = RATE_FLOAT_OF_BFP(msg_up->gyro.p);
  imuFloat.gyro.q = RATE_FLOAT_OF_BFP(msg_up->gyro.q);
  imuFloat.gyro.r = RATE_FLOAT_OF_BFP(msg_up->gyro.r);

  imuFloat.accel.x = ACCEL_FLOAT_OF_BFP(msg_up->accel.x);
  imuFloat.accel.y = ACCEL_FLOAT_OF_BFP(msg_up->accel.y);
  imuFloat.accel.z = ACCEL_FLOAT_OF_BFP(msg_up->accel.z);

  imuFloat.mag.x = MAG_FLOAT_OF_BFP(msg_up->mag.x);
  imuFloat.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y);
  imuFloat.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z);

	imuFloat.sample_count = msg_up->imu_tick;

  if (msg_up->valid.imu)
    rdyb_booz_imu_update(&imuFloat);
}

Esempio n. 3

File: ahrs_float_dcm.c Progetto: 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
}