Ejemplo n.º 1
0
static void b2_hff_propagate_past(struct HfilterFloat *hff_past)
{
  PRINT_DBG(1, ("enter propagate past: %d\n", hff_past->lag_counter));
  /* run max MAX_PP_STEPS propagation steps */
  for (int i = 0; i < MAX_PP_STEPS; i++) {
    if (hff_past->lag_counter > 0) {
      b2_hff_get_past_accel(hff_past->lag_counter);
      PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter));
      b2_hff_propagate_x(hff_past, DT_HFILTER);
      b2_hff_propagate_y(hff_past, DT_HFILTER);
      hff_past->lag_counter--;

      if (past_save_counter > 0) {
        past_save_counter--;
        PRINT_DBG(2, ("dec past_save_counter: %d\n", past_save_counter));
      } else if (past_save_counter == SAVE_NOW) {
        /* next GPS measurement valid at this state -> save */
        PRINT_DBG(2, ("save past state\n"));
        b2_hff_rb_put_state(hff_past);
        past_save_counter = SAVING;
      } else if (past_save_counter == SAVING) {
        /* increase lag counter on if next state is already saved */
        if (hff_past == &b2_hff_rb[HFF_RB_MAXN - 1]) {
          b2_hff_rb[0].lag_counter++;
        } else {
          (hff_past + 1)->lag_counter++;
        }
      }
    }

    /* finished re-propagating the past values */
    if (hff_past->lag_counter == 0) {
      b2_hff_set_state(&b2_hff_state, hff_past);
      b2_hff_rb_drop_last();
      past_save_counter = SAVE_DONE;
      break;
    }
  }
}
Ejemplo n.º 2
0
void b2_hff_propagate(void) {
  if (b2_hff_lost_counter < b2_hff_lost_limit)
    b2_hff_lost_counter++;

#ifdef GPS_LAG
  /* continue re-propagating to catch up with the present */
  if (b2_hff_rb_last->rollback) {
	b2_hff_propagate_past(b2_hff_rb_last);
  }
#endif

  /* store body accelerations for mean computation */
  b2_hff_store_accel_body();

  /* propagate current state if it is time */
  if (b2_hff_ps_counter == HFF_PRESCALER) {
	b2_hff_ps_counter = 1;

    if (b2_hff_lost_counter < b2_hff_lost_limit) {
      /* compute float ltp mean acceleration */
      b2_hff_compute_accel_body_mean(HFF_PRESCALER);
      struct Int32Vect3 mean_accel_ltp;
      INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, ahrs.ltp_to_body_rmat, acc_body_mean);
      b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
      b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
#ifdef GPS_LAG
      b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
#endif

      /*
       * propagate current state
       */
      b2_hff_propagate_x(&b2_hff_state);
      b2_hff_propagate_y(&b2_hff_state);

      /* update ins state from horizontal filter */
      ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
      ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
      ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
      ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
      ins_ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_state.x);
      ins_ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_state.y);

#ifdef GPS_LAG
      /* increase lag counter on last saved state */
      if (b2_hff_rb_n > 0)
        b2_hff_rb_last->lag_counter++;

      /* save filter state if needed */
      if (save_counter == 0) {
        PRINT_DBG(1, ("save current state\n"));
        b2_hff_rb_put_state(&b2_hff_state);
        save_counter = -1;
      } else if (save_counter > 0) {
        save_counter--;
      }
#endif
    }
  } else {
    b2_hff_ps_counter++;
  }
}
Ejemplo n.º 3
0
void b2_hff_propagate(void)
{
  if (b2_hff_lost_counter < b2_hff_lost_limit) {
    b2_hff_lost_counter++;
  }

#ifdef GPS_LAG
  /* continue re-propagating to catch up with the present */
  if (b2_hff_rb_last->rollback) {
    b2_hff_propagate_past(b2_hff_rb_last);
  }
#endif

  /* rotate imu accel measurement to body frame and filter */
  struct Int32Vect3 acc_meas_body;
  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
  int32_rmat_transp_vmult(&acc_meas_body, body_to_imu_rmat, &imu.accel);

  struct Int32Vect3 acc_body_filtered;
  acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, acc_meas_body.x);
  acc_body_filtered.y = update_butterworth_2_low_pass_int(&filter_y, acc_meas_body.y);
  acc_body_filtered.z = update_butterworth_2_low_pass_int(&filter_z, acc_meas_body.z);

  /* propagate current state if it is time */
  if (b2_hff_ps_counter == HFF_PRESCALER) {
    b2_hff_ps_counter = 1;
    if (b2_hff_lost_counter < b2_hff_lost_limit) {
      struct Int32Vect3 filtered_accel_ltp;
      struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i();
      int32_rmat_transp_vmult(&filtered_accel_ltp, ltp_to_body_rmat, &acc_body_filtered);
      b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x);
      b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.y);
#ifdef GPS_LAG
      b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
#endif
      /*
       * propagate current state
       */
      b2_hff_propagate_x(&b2_hff_state, DT_HFILTER);
      b2_hff_propagate_y(&b2_hff_state, DT_HFILTER);

#ifdef GPS_LAG
      /* increase lag counter on last saved state */
      if (b2_hff_rb_n > 0) {
        b2_hff_rb_last->lag_counter++;
      }

      /* save filter state if needed */
      if (save_counter == 0) {
        PRINT_DBG(1, ("save current state\n"));
        b2_hff_rb_put_state(&b2_hff_state);
        save_counter = -1;
      } else if (save_counter > 0) {
        save_counter--;
      }
#endif
    }
  } else {
    b2_hff_ps_counter++;
  }
}