Esempi in C++ (Cpp) per DOWNLINK_SEND_AHRS_EULER

Esempio n. 1

File: ins_chimu_spi.c Progetto: aibara/paparazzi

void parse_ins_msg( void )
{
  while (InsLink(ChAvailable()))
  {
    uint8_t ch = InsLink(Getch());
    
    if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
    {
      if(CHIMU_DATA.m_MsgID==0x03)
      {
	new_ins_attitude = 1;
	RunOnceEvery(25, LED_TOGGLE(3) );
	if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
	{
	  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
	}
	
	EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
	EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
      }
      else if(CHIMU_DATA.m_MsgID==0x02)
      {
	
	RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
	
      }
    }
  }
}

Esempio n. 2

File: ahrs_chimu_uart.c Progetto: Djeef/paparazzi

void parse_ins_msg(void)
{
  struct link_device *dev = InsLinkDevice;
  while (dev->char_available(dev->periph)) {
    uint8_t ch = dev->get_byte(dev->periph);

    if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
      if (CHIMU_DATA.m_MsgID == 0x03) {
        new_ins_attitude = 1;
        RunOnceEvery(25, LED_TOGGLE(3));
        if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
          CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
        }

        struct FloatEulers att = {
          CHIMU_DATA.m_attitude.euler.phi,
          CHIMU_DATA.m_attitude.euler.theta,
          CHIMU_DATA.m_attitude.euler.psi
        };
        stateSetNedToBodyEulers_f(&att);
        ahrs_chimu.is_aligned = TRUE;
#if CHIMU_DOWNLINK_IMMEDIATE
        DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi,
                                 &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
#endif

      }
    }
  }
}

Esempio n. 3

File: ahrs_chimu_spi.c Progetto: 2seasuav/paparuzzi

void parse_ins_msg(void)
{
  struct link_device *dev = InsLinkDevice;
  while (dev->char_available(dev->periph)) {
    uint8_t ch = dev->get_byte(dev->periph);

    if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
      RunOnceEvery(25, LED_TOGGLE(3));
      if (CHIMU_DATA.m_MsgID == CHIMU_Msg_3_IMU_Attitude) {
        // TODO: remove this flag as it doesn't work with multiple AHRS
        new_ins_attitude = 1;
        if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
          CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
        }

        //FIXME
        ahrs_chimu.is_aligned = true;

        if (ahrs_chimu.is_enabled) {
          struct FloatEulers att = {
            CHIMU_DATA.m_attitude.euler.phi,
            CHIMU_DATA.m_attitude.euler.theta,
            CHIMU_DATA.m_attitude.euler.psi
          };
          stateSetNedToBodyEulers_f(&att);
          struct FloatRates rates = {
            CHIMU_DATA.m_sensor.rate[0],
            CHIMU_DATA.m_attrates.euler.theta,
            0.
          }; // FIXME rate r
          stateSetBodyRates_f(&rates);
        }
      } else if (CHIMU_DATA.m_MsgID == 0x02) {
#if CHIMU_DOWNLINK_IMMEDIATE
        RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0],
                     &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
#endif
      }
    }
  }
}

Esempio n. 4

File: ins_chimu_uart.c Progetto: newtectonics/USTUAV

void parse_ins_msg( void )
{
    while (InsLink(ChAvailable())) {
        uint8_t ch = InsLink(Getch());

        if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
            if(CHIMU_DATA.m_MsgID==0x03) {
                new_ins_attitude = 1;
                RunOnceEvery(25, LED_TOGGLE(3) );
                if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
                    CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
                }

                EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
                //EstimatorSetRate(ins_p,ins_q,ins_r);

                DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);

            }
        }
    }
}

Esempio n. 5

File: ahrs_chimu_spi.c Progetto: jziesing/paparazzi

void parse_ins_msg(void)
{
  while (InsLink(ChAvailable())) {
    uint8_t ch = InsLink(Getch());

    if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
      RunOnceEvery(25, LED_TOGGLE(3));
      if (CHIMU_DATA.m_MsgID == CHIMU_Msg_3_IMU_Attitude) {
        new_ins_attitude = 1;
        if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
          CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
        }

        struct FloatEulers att = {
          CHIMU_DATA.m_attitude.euler.phi,
          CHIMU_DATA.m_attitude.euler.theta,
          CHIMU_DATA.m_attitude.euler.psi
        };
        stateSetNedToBodyEulers_f(&att);
        struct FloatRates rates = {
          CHIMU_DATA.m_sensor.rate[0],
          CHIMU_DATA.m_attrates.euler.theta,
          0.
        }; // FIXME rate r
        stateSetBodyRates_f(&rates);
        //FIXME
        ahrs_chimu.is_aligned = TRUE;
      } else if (CHIMU_DATA.m_MsgID == 0x02) {
#if CHIMU_DOWNLINK_IMMEDIATE
        RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0],
                     &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
#endif
      }
    }
  }
}