コード例 #1
0
ファイル: ahrs_infrared.c プロジェクト: DuinoPilot/paparazzi
void ahrs_update_infrared(void) {
  float phi  = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
  float theta  = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;

  if (theta < -M_PI_2) theta += M_PI;
  else if (theta > M_PI_2) theta -= M_PI;

  if (phi >= 0) phi *= infrared.correction_right;
  else phi *= infrared.correction_left;

  if (theta >= 0) theta *= infrared.correction_up;
  else theta *= infrared.correction_down;

  struct FloatEulers att = { phi, theta, heading };
  stateSetNedToBodyEulers_f(&att);

}
コード例 #2
0
ファイル: ahrs_infrared.c プロジェクト: DimaRU/paparazzi
void ahrs_infrared_periodic(void)
{
  float phi  = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
  float theta  = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;

  if (theta < -M_PI_2) { theta += M_PI; }
  else if (theta > M_PI_2) { theta -= M_PI; }

  if (phi >= 0) { phi *= infrared.correction_right; }
  else { phi *= infrared.correction_left; }

  if (theta >= 0) { theta *= infrared.correction_up; }
  else { theta *= infrared.correction_down; }

  struct FloatEulers att = { phi, theta, heading };
  stateSetNedToBodyEulers_f(&att);
}
コード例 #3
0
ファイル: ahrs_chimu_spi.c プロジェクト: 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
      }
    }
  }
}
コード例 #4
0
ファイル: ahrs_float_dcm.c プロジェクト: F34140r/paparazzi
/*
 * Compute body orientation and rates from imu orientation and rates
 */
static inline void set_body_orientation_and_rates(void) {

  struct FloatRates body_rate;
  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate);
  stateSetBodyRates_f(&body_rate);

  struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
  FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler);
  FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);

  // Some stupid lines of code for neutrals
  struct FloatEulers ltp_to_body_euler;
  FLOAT_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
  ltp_to_body_euler.phi -= ins_roll_neutral;
  ltp_to_body_euler.theta -= ins_pitch_neutral;
  stateSetNedToBodyEulers_f(&ltp_to_body_euler);

  // should be replaced at the end by:
  //   stateSetNedToBodyRMat_f(&ltp_to_body_rmat);

}
コード例 #5
0
ファイル: ahrs_chimu_spi.c プロジェクト: 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
      }
    }
  }
}
コード例 #6
0
ファイル: ins_vn100.c プロジェクト: AE4317group07/paparazzi
void vn100_event_task(void)
{
  if (vn100_trans.status == SPITransSuccess) {
    parse_ins_msg();
#ifndef INS_VN100_READ_ONLY
    // Update estimator
    // FIXME Use a proper rotation matrix here
    struct FloatEulers att = {
      ins_eulers.phi - ins_roll_neutral,
      ins_eulers.theta - ins_pitch_neutral,
      ins_eulers.psi
    };
    stateSetNedToBodyEulers_f(&att);
    stateSetBodyRates_f(&ins_rates);
#endif
    //uint8_t s = 4+VN100_REG_QMR_SIZE;
    //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input);
    vn100_trans.status = SPITransDone;
  }
  if (vn100_trans.status == SPITransFailed) {
    vn100_trans.status = SPITransDone;
    // FIXME retry config if not done ?
  }
}
コード例 #7
0
void ahrs_propagate(void) {
  struct NedCoor_f accel;
  struct FloatRates body_rates;
  struct FloatEulers eulers;

  // realign all the filter if needed
  // a complete init cycle is required
  if (ins_impl.reset) {
    ins_impl.reset = FALSE;
    ins.status = INS_UNINIT;
    ahrs.status = AHRS_UNINIT;
    init_invariant_state();
  }

  // fill command vector
  struct Int32Rates gyro_meas_body;
  INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.body_to_imu_rmat, imu.gyro);
  RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
  struct Int32Vect3 accel_meas_body;
  INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
  ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);

  // update correction gains
  error_output(&ins_impl);

  // propagate model
  struct inv_state new_state;
  runge_kutta_4_float((float*)&new_state,
      (float*)&ins_impl.state, INV_STATE_DIM,
      (float*)&ins_impl.cmd, INV_COMMAND_DIM,
      invariant_model, dt);
  ins_impl.state = new_state;

  // normalize quaternion
  FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);

  // set global state
  FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
#if INS_UPDATE_FW_ESTIMATOR
  // Some stupid lines of code for neutrals
  eulers.phi -= ins_roll_neutral;
  eulers.theta -= ins_pitch_neutral;
  stateSetNedToBodyEulers_f(&eulers);
#else
  stateSetNedToBodyQuat_f(&ins_impl.state.quat);
#endif
  RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
  stateSetBodyRates_f(&body_rates);
  stateSetPositionNed_f(&ins_impl.state.pos);
  stateSetSpeedNed_f(&ins_impl.state.speed);
  // untilt accel and remove gravity
  FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel);
  FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
  FLOAT_VECT3_ADD(accel, A);
  stateSetAccelNed_f(&accel);

  //------------------------------------------------------------//

  RunOnceEvery(3,{
      DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice,
        &ins_impl.state.quat.qi,
        &eulers.phi,
        &eulers.theta,
        &eulers.psi,
        &ins_impl.state.speed.x,
        &ins_impl.state.speed.y,
        &ins_impl.state.speed.z,
        &ins_impl.state.pos.x,
        &ins_impl.state.pos.y,
        &ins_impl.state.pos.z,
        &ins_impl.state.bias.p,
        &ins_impl.state.bias.q,
        &ins_impl.state.bias.r,
        &ins_impl.state.as,
        &ins_impl.state.hb,
        &ins_impl.meas.baro_alt,
        &ins_impl.meas.pos_gps.z)
      });

#if LOG_INVARIANT_FILTER
  if (pprzLogFile.fs != NULL) {
    if (!log_started) {
      // log file header
      sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
      log_started = TRUE;
    }
    else {
      sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
          ins_impl.cmd.rates.p,
          ins_impl.cmd.rates.q,
          ins_impl.cmd.rates.r,
          ins_impl.cmd.accel.x,
          ins_impl.cmd.accel.y,
          ins_impl.cmd.accel.z,
          ins_impl.meas.pos_gps.x,
          ins_impl.meas.pos_gps.y,
          ins_impl.meas.pos_gps.z,
          ins_impl.meas.speed_gps.x,
          ins_impl.meas.speed_gps.y,
          ins_impl.meas.speed_gps.z,
          ins_impl.meas.mag.x,
          ins_impl.meas.mag.y,
          ins_impl.meas.mag.z,
          ins_impl.meas.baro_alt,
          ins_impl.state.quat.qi,
          ins_impl.state.quat.qx,
          ins_impl.state.quat.qy,
          ins_impl.state.quat.qz,
          ins_impl.state.bias.p,
          ins_impl.state.bias.q,
          ins_impl.state.bias.r,
          ins_impl.state.speed.x,
          ins_impl.state.speed.y,
          ins_impl.state.speed.z,
          ins_impl.state.pos.x,
          ins_impl.state.pos.y,
          ins_impl.state.pos.z,
          ins_impl.state.hb,
          ins_impl.state.as);
    }
  }
#endif
}
コード例 #8
0
ファイル: ahrs_ardrone2.c プロジェクト: Djeef/paparazzi
void ahrs_ardrone2_propagate(void)
{
  int l;

  //Recieve the main packet
  l = at_com_recieve_navdata(buffer);
  navdata_t *main_packet = (navdata_t *) &buffer;

#ifdef ARDRONE2_DEBUG
  if (l < 0) {
    printf("errno = %d\n", errno);
  }
#endif

  //When this isn't a valid packet return
  if (l < 0 || main_packet->header != NAVDATA_HEADER) {
    return;
  }

#ifdef ARDRONE2_DEBUG
  printf("Read %d\n", l);
  dump(buffer, l);
#endif

  //Set the state
  ahrs_ardrone2.state = main_packet->ardrone_state;

  //Init the option
  navdata_option_t *navdata_option = (navdata_option_t *) & (main_packet->options[0]);
  bool_t full_read = FALSE;

  //The possible packets
  navdata_demo_t *navdata_demo;
  navdata_gps_t *navdata_gps;
  navdata_phys_measures_t *navdata_phys_measures;

  //Read the navdata until packet is fully readed
  while (!full_read && navdata_option->size > 0) {
#ifdef ARDRONE2_DEBUG
    printf("tag = %d\n", navdata_option->tag);
#endif
    //Check the tag for the right option
    switch (navdata_option->tag) {
      case 0: //NAVDATA_DEMO
        navdata_demo = (navdata_demo_t *) navdata_option;

        //Set the AHRS state
        ahrs_ardrone2.control_state = navdata_demo->ctrl_state >> 16;
        ahrs_ardrone2.eulers.phi = navdata_demo->phi;
        ahrs_ardrone2.eulers.theta = navdata_demo->theta;
        ahrs_ardrone2.eulers.psi = navdata_demo->psi;
        ahrs_ardrone2.speed.x = navdata_demo->vx / 1000;
        ahrs_ardrone2.speed.y = navdata_demo->vy / 1000;
        ahrs_ardrone2.speed.z = navdata_demo->vz / 1000;
        ahrs_ardrone2.altitude = navdata_demo->altitude / 10;
        ahrs_ardrone2.battery = navdata_demo->vbat_flying_percentage;

        //Set the ned to body eulers
        struct FloatEulers angles;
        angles.theta = navdata_demo->theta / 180000.*M_PI;
        angles.psi = navdata_demo->psi / 180000.*M_PI;
        angles.phi = navdata_demo->phi / 180000.*M_PI;
        stateSetNedToBodyEulers_f(&angles);

        //Update the electrical supply
        electrical.vsupply = navdata_demo->vbat_flying_percentage;
        break;
      case 3: //NAVDATA_PHYS_MEASURES
        navdata_phys_measures = (navdata_phys_measures_t *) navdata_option;

        //Set the AHRS accel state
        INT32_VECT3_SCALE_2(ahrs_ardrone2.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
        break;
#ifdef USE_GPS_ARDRONE2
      case 27: //NAVDATA_GPS
# ifdef ARDRONE2_DEBUG
        dump(navdata_option, navdata_option->size);
# endif
        navdata_gps = (navdata_gps_t *) navdata_option;

        // Send the data to the gps parser
        gps_ardrone2_parse(navdata_gps);
        break;
#endif
      case 0xFFFF: //CHECKSUM
        //TODO: Check the checksum
        full_read = TRUE;
        break;
      default:
#ifdef ARDRONE2_DEBUG
        printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
#endif
        break;
    }
    navdata_option = (navdata_option_t *)((uint32_t)navdata_option + navdata_option->size);
  }
}
コード例 #9
0
ファイル: ahrs_gx3.c プロジェクト: SeungWan/paparazzi
void gx3_packet_read_message(void) {
  ahrs_impl.gx3_accel.x     = bef(&ahrs_impl.gx3_packet.msg_buf[1]);
  ahrs_impl.gx3_accel.y     = bef(&ahrs_impl.gx3_packet.msg_buf[5]);
  ahrs_impl.gx3_accel.z     = bef(&ahrs_impl.gx3_packet.msg_buf[9]);
  ahrs_impl.gx3_rate.p      = bef(&ahrs_impl.gx3_packet.msg_buf[13]);
  ahrs_impl.gx3_rate.q      = bef(&ahrs_impl.gx3_packet.msg_buf[17]);
  ahrs_impl.gx3_rate.r      = bef(&ahrs_impl.gx3_packet.msg_buf[21]);
  ahrs_impl.gx3_rmat.m[0]   = bef(&ahrs_impl.gx3_packet.msg_buf[25]);
  ahrs_impl.gx3_rmat.m[1]   = bef(&ahrs_impl.gx3_packet.msg_buf[29]);
  ahrs_impl.gx3_rmat.m[2]   = bef(&ahrs_impl.gx3_packet.msg_buf[33]);
  ahrs_impl.gx3_rmat.m[3]   = bef(&ahrs_impl.gx3_packet.msg_buf[37]);
  ahrs_impl.gx3_rmat.m[4]   = bef(&ahrs_impl.gx3_packet.msg_buf[41]);
  ahrs_impl.gx3_rmat.m[5]   = bef(&ahrs_impl.gx3_packet.msg_buf[45]);
  ahrs_impl.gx3_rmat.m[6]   = bef(&ahrs_impl.gx3_packet.msg_buf[49]);
  ahrs_impl.gx3_rmat.m[7]   = bef(&ahrs_impl.gx3_packet.msg_buf[53]);
  ahrs_impl.gx3_rmat.m[8]   = bef(&ahrs_impl.gx3_packet.msg_buf[57]);
  ahrs_impl.gx3_time    = (uint32_t)(ahrs_impl.gx3_packet.msg_buf[61] << 24 |
                                     ahrs_impl.gx3_packet.msg_buf[62] << 16 | ahrs_impl.gx3_packet.msg_buf[63] << 8 | ahrs_impl.gx3_packet.msg_buf[64]);
  ahrs_impl.gx3_chksm   = GX3_CHKSM(ahrs_impl.gx3_packet.msg_buf);

  ahrs_impl.gx3_freq = 62500.0 / (float)(ahrs_impl.gx3_time - ahrs_impl.gx3_ltime);
  ahrs_impl.gx3_ltime = ahrs_impl.gx3_time;

  // Acceleration
  VECT3_SMUL(ahrs_impl.gx3_accel, ahrs_impl.gx3_accel, 9.80665); // Convert g into m/s2
  ACCELS_BFP_OF_REAL(imu.accel, ahrs_impl.gx3_accel); // for backwards compatibility with fixed point interface
  imuf.accel = ahrs_impl.gx3_accel;

  // Rates
  struct FloatRates body_rate;
  imuf.gyro = ahrs_impl.gx3_rate;
  /* compute body rates */
  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imuf.body_to_imu);
  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, imuf.gyro);
  /* Set state */
  stateSetBodyRates_f(&body_rate);

  // Attitude
  struct FloatRMat ltp_to_body_rmat;
  FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, *body_to_imu_rmat);

#if AHRS_USE_GPS_HEADING && USE_GPS
  struct FloatEulers ltp_to_body_eulers;
  float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
  float course_f = (float)DegOfRad(gps.course / 1e7);
  if (course_f > 180.0) {
    course_f -= 360.0;
  }
  ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
#else // !AHRS_USE_GPS_HEADING
#ifdef IMU_MAG_OFFSET
  struct FloatEulers ltp_to_body_eulers;
  float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
  ltp_to_body_eulers.psi -= ahrs_impl.mag_offset;
  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
#else
  stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
#endif // IMU_MAG_OFFSET
#endif // !AHRS_USE_GPS_HEADING
}