Exemple #1
0
void ahrs_chimu_init(void)
{
  ahrs_chimu.is_aligned = FALSE;

  // uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
  uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 };  // 50Hz attitude only + SPI
  uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
  // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
  // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI

  new_ins_attitude = 0;

  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;

  // Init
  CHIMU_Init(&CHIMU_DATA);

  // Quat Filter
  CHIMU_Checksum(quaternions, 7);
  InsSend(quaternions, 7);

  // Wait a bit (SPI send zero)
  InsSend1(0);
  InsSend1(0);
  InsSend1(0);
  InsSend1(0);
  InsSend1(0);

  // 50Hz data: attitude only
  CHIMU_Checksum(rate, 12);
  InsSend(rate, 12);
}
Exemple #2
0
void ins_init( void )
{
    uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
    uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 };	// 50Hz attitude only + SPI
    uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
    //  uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
    //  uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI


    new_ins_attitude = 0;

    ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
    ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;

    CHIMU_Init(&CHIMU_DATA);

    // Request Software version
    for (int i=0; i<7; i++) {
        InsUartSend1(ping[i]);
    }


    // Quat Filter
    for (int i=0; i<7; i++) {
        InsUartSend1(quaternions[i]);
    }


    // 50Hz
    CHIMU_Checksum(rate,12);
    InsSend(rate,12);

}
Exemple #3
0
//Frequency defined in conf *.xml
void ins_periodic_task( void ) 
{
  // Send SW Centripetal Corrections
  uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
  
  // Fill X-speed
  
  CHIMU_Checksum(centripedal,19);
  
  for (int i=0;i<19;i++)
  {
    InsSend1(centripedal[i]);
  }

  // Downlink Send
}
Exemple #4
0
void ahrs_chimu_update_gps(void)
{
  // Send SW Centripetal Corrections
  uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02,   0x00, 0x00, 0x00, 0x00,   0x00, 0x00, 0x00, 0x00,   0x00, 0x00, 0x00, 0x00,   0xc2 };

  float gps_speed = 0;

  if (gps.fix == GPS_FIX_3D) {
    gps_speed = gps.speed_3d / 100.;
  }
  gps_speed = FloatSwap(gps_speed);

  memmove(&centripedal[6], &gps_speed, 4);

  // Fill X-speed

  CHIMU_Checksum(centripedal, 19);
  InsSend(centripedal, 19);

  // Downlink Send
}
Exemple #5
0
void ahrs_chimu_update_gps(uint8_t gps_fix __attribute__((unused)), uint16_t gps_speed_3d)
{
  // Send SW Centripetal Corrections
  uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02,   0x00, 0x00, 0x00, 0x00,   0x00, 0x00, 0x00, 0x00,   0x00, 0x00, 0x00, 0x00,   0xc2 };

  float gps_speed = 0;

  if (GpsFixValid()) {
    gps_speed = gps_speed_3d / 100.;
  }
  gps_speed = FloatSwap(gps_speed);

  memmove(&centripedal[6], &gps_speed, 4);

  // Fill X-speed

  CHIMU_Checksum(centripedal, 19);
  InsSend(centripedal, 19);

  // Downlink Send
}
Exemple #6
0
void ins_init( void ) 
{
  uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 };	// 50Hz attitude only + SPI
//  uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
//  uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
  uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
  
  CHIMU_Checksum(rate,12);
  
  new_ins_attitude = 0;
  
  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
  
  CHIMU_Init(&CHIMU_DATA);  
  
  // Quat Filter
  for (int i=0;i<7;i++)
  {
    InsSend1(quaternions[i]);
  }

  // Wait a bit (SPI send zero)
  InsSend1(0);
  InsSend1(0);
  InsSend1(0);
  InsSend1(0);
  InsSend1(0);

  // 50Hz data: attitude only
  for (int i=0;i<12;i++)
  {
    InsSend1(rate[i]);
  }
  
}