示例#1
0
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]));
	
      }
    }
  }
}
示例#2
0
void xsens_event_task( void )
{
  while (Xsens1Link(ChAvailable()) && !xsens_msg_received[0]) {
    parse_xsens_msg(0, Xsens1Link(Getch()));
  }

/*
  while (Xsens2Link(ChAvailable()) && !xsens_msg_received[1])
    parse_xsens_msg(1, Xsens2Link(Getch()));
*/

  for (int i = 0; i < XSENS_COUNT; i++) {
    if (xsens_msg_received[i]) {

      // first make room
      while (xsens_msg_buf_count[i] >= XSENS_MSG_BUF) {
        // throwing away old stuff
        xsens_msg_buf_ci[i] = (xsens_msg_buf_ci[i] + 1 ) % XSENS_MSG_BUF;
        xsens_msg_buf_count[i]--;
      }

      xsens_msg_buf_pi[i] = (xsens_msg_buf_pi[i] + 1 ) % XSENS_MSG_BUF;
      xsens_msg_buf_count[i]++;

      //xsens_parse_msg(i);
      xsens_msg_received[i] = FALSE;
    }
  }
}
示例#3
0
void link_mcu_event_task( void ) {
  /* A message has been received */
  if (InterMcuBuffer()) {
    while (InterMcuLink(ChAvailable())&&!intermcu_data.msg_available)
      intermcu_parse(InterMcuLink(Getch()));
  }

  if (intermcu_data.msg_available) {
    parse_mavpilot_msg();
    intermcu_data.msg_available = FALSE;
  }
}
示例#4
0
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);

            }
        }
    }
}
示例#5
0
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
      }
    }
  }
}
示例#6
0
void digital_cam_uart_event(void)
{
  while (CameraLink(ChAvailable())) {
    parse_mora(&mora_protocol, CameraLink(Getch()));
    if (mora_protocol.msg_received) {
      switch (mora_protocol.msg_id) {
        case MORA_STATUS:
          for (int i = 0; i < MORA_STATUS_MSG_SIZE; i++) {
            mora_status_msg.bin[i] = mora_protocol.payload[i];
          }
          digital_cam_uart_status = mora_status_msg.data.shots;
          break;
        case MORA_PAYLOAD:
          for (int i = 0; i < MORA_PAYLOAD_MSG_SIZE; i++) {
            thumbs[thumb_pointer][i] = mora_protocol.payload[i];
          }
          break;
        default:
          break;
      }
      mora_protocol.msg_received = 0;
    }
  }
}
示例#7
0
void airspeed_otf_event(void) {
  while (MetLink(ChAvailable())) {
    uint8_t ch = MetLink(Getch());
    airspeed_otf_parse(ch);
  }
}
void aerocomm_init( void ) {

aerocomm_confirmation_received=TRUE;
{  uint8_t init_cpt = 120;
  while (init_cpt) {
    if (sys_time_periodic())
      init_cpt--;
  }
}

#ifdef NO_API_INIT
#warning "NO_API_INIT defined"
#else
#warning "NO_API_INIT not defined"
#endif

#ifndef NO_API_INIT
  /** Switching to AT mode (FIXME: busy waiting) */
  AerocommPrintString(AT_COMMAND_SEQUENCE);
  char c;
  while (!AerocommLink(ChAvailable())) { 
     uint8_t init_cpt = 3;
     while (init_cpt) {
       if (sys_time_periodic())
          init_cpt--;
     }
  }
  int j;
  j=0;
  while (AerocommLink(ChAvailable()) && j<4) 
  {
	c=AerocommLink(Getch()); j++;
  }

  uint16_t addr = AEROCOMM_MY_ADDR;
  char s[]=AT_SET_MY;
  s[4]=(unsigned int)(0x00);
  s[5]=(unsigned int)(0x50);
  s[6]=(unsigned int)(0x67);
  s[7]=(unsigned int)(0x00);
  s[8]=(unsigned int)(addr>> 8);
  s[9]=(unsigned int)(addr & 0xff);
  AerocommPrintString(AT_READ_MY);
  while (!AerocommLink(ChAvailable())) { 
     uint8_t init_cpt = 3;
     while (init_cpt) {
       if (sys_time_periodic())
          init_cpt--;
     }
  }
  j=0;
  while (AerocommLink(ChAvailable()) && j<4) 
  {
	c=AerocommLink(Getch()); j++;
  }

  if (c!=s[9]) for (j=0; j<10; j++)  AerocommTransportPut1Byte(s[j]);

  while (!AerocommLink(ChAvailable())) { 
     uint8_t init_cpt = 3;
     while (init_cpt) {
       if (sys_time_periodic())
          init_cpt--;
     }
  }
  j=0;
  while (AerocommLink(ChAvailable()) && j<3) 
  {
	c=AerocommLink(Getch()); j++;
  }
  AerocommPrintString(AT_RESET);
  {  uint8_t init_cpt = 60;
     while (init_cpt) {
     if (sys_time_periodic())
        init_cpt--;
     }
   }

  AerocommPrintString(AT_COMMAND_SEQUENCE);
  while (!AerocommLink(ChAvailable())) { 
     uint8_t init_cpt = 3;
     while (init_cpt) {
       if (sys_time_periodic())
          init_cpt--;
     }
  }
  j=0;
  while (AerocommLink(ChAvailable()) && j<4) 
  {
	c=AerocommLink(Getch()); j++;
  }
  AerocommPrintString(AT_AP_MODE);
  while (!AerocommLink(ChAvailable())) { 
     uint8_t init_cpt = 3;
     while (init_cpt) {
       if (sys_time_periodic())
          init_cpt--;
     }
  }
  j=0;
  while (AerocommLink(ChAvailable()) && j<2) 
  {
	c=AerocommLink(Getch()); j++;
  }
  char s1[]=AT_DEST_ADDR;
  s1[2]=(unsigned int)(0x00);
  s1[3]=(unsigned int)(0x01);
  s1[4]=(unsigned int)(0x00);
  for (j=0; j<5; j++)  AerocommTransportPut1Byte(s1[j]);
  while (!AerocommLink(ChAvailable())) { 
     uint8_t init_cpt = 3;
     while (init_cpt) {
       if (sys_time_periodic())
          init_cpt--;
     }
  }
  j=0;
  while (AerocommLink(ChAvailable()) && j<4) 
  {
	c=AerocommLink(Getch()); j++;
  }

  AerocommPrintString(AT_EXIT);
  while (!AerocommLink(ChAvailable())) { 
     uint8_t init_cpt = 3;
     while (init_cpt) {
       if (sys_time_periodic())
          init_cpt--;
     }
  }
  j=0;
  while (AerocommLink(ChAvailable()) && j<4) 
  {
	c=AerocommLink(Getch()); j++;
  }

#endif
}