Ejemplo n.º 1
0
void ins_update_gps(void) {
#ifdef USE_GPS
  if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
    if (!ins_ltp_initialised) {
      ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos);
      ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
      ins_ltp_def.hmsl = booz_gps_state.hmsl;
      ins_ltp_initialised = TRUE;
    }
    ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &booz_gps_state.ecef_pos);
    ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &booz_gps_state.ecef_vel);
#ifdef USE_HFF
    VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
    VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
    VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
    VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
    if (ins_hf_realign) {
      ins_hf_realign = FALSE;
#ifdef SITL
      struct FloatVect2 true_pos, true_speed;
      VECT2_COPY(true_pos, fdm.ltpprz_pos);
      VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
      b2_hff_realign(true_pos, true_speed);
#else
      const struct FloatVect2 zero = {0.0, 0.0};
      b2_hff_realign(ins_gps_pos_m_ned, zero);
#endif
    }
    b2_hff_update_gps();
#ifndef USE_VFF /* vff not used */
    ins_ltp_pos.z =  (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
    ins_ltp_speed.z =  (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
#endif /* vff not used */
#endif /* hff used */


#ifndef USE_HFF /* hff not used */
#ifndef USE_VFF /* neither hf nor vf used */
    INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
    INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#else /* only vff used */
    INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
    INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif

#ifdef USE_GPS_LAG_HACK
    VECT2_COPY(d_pos, ins_ltp_speed);
    INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
    VECT2_ADD(ins_ltp_pos, d_pos);
#endif
#endif /* hff not used */

    INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
    INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
    INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
  }
#endif /* USE_GPS */
}
Ejemplo n.º 2
0
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {

  VECT2_ADD(gh_pos_ref, gh_speed_ref);
  VECT2_ADD(gh_speed_ref, gh_accel_ref);

  // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
  struct Int32Vect2 speed;
  INT32_VECT2_RSHIFT(speed, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
  VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA);
  INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC);
  // compute pos error in pos_sp resolution
  struct Int32Vect2 pos_err;
  INT32_VECT2_RSHIFT(pos_err, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC));
  VECT2_DIFF(pos_err, pos_err, pos_sp);
  // convert to accel resolution
  INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC));
  // compute the "pos part" of accel
  struct Int32Vect2 pos;
  VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2));
  INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC);
  // sum accel
  VECT2_SUM(gh_accel_ref, speed, pos);

  /* Compute route reference before saturation */
  // use metric precision or values are too large
  INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x);
  /* Compute North and East route components */
  PPRZ_ITRIG_SIN(s_route_ref, route_ref);
  PPRZ_ITRIG_COS(c_route_ref, route_ref);
  c_route_ref = abs(c_route_ref);
  s_route_ref = abs(s_route_ref);
  /* Compute maximum acceleration*/
  gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
  gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
  /* Compute maximum speed*/
  gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
  gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
  /* restore gh_speed_ref range (Q14.17) */
  INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));

 /* Saturate accelerations */
  if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
    gh_accel_ref.x = -gh_max_accel_ref.x;
  }
  else if (gh_accel_ref.x >=  gh_max_accel_ref.x) {
    gh_accel_ref.x =  gh_max_accel_ref.x;
  }
  if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
    gh_accel_ref.y = -gh_max_accel_ref.y;
  }
  else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
    gh_accel_ref.y = gh_max_accel_ref.y;
  }

  /* Saturate speed and adjust acceleration accordingly */
  if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
    gh_speed_ref.x = -gh_max_speed_ref.x;
    if (gh_accel_ref.x < 0)
      gh_accel_ref.x = 0;
  }
  else if (gh_speed_ref.x >=  gh_max_speed_ref.x) {
    gh_speed_ref.x =  gh_max_speed_ref.x;
    if (gh_accel_ref.x > 0)
      gh_accel_ref.x = 0;
  }
  if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
    gh_speed_ref.y = -gh_max_speed_ref.y;
    if (gh_accel_ref.y < 0)
      gh_accel_ref.y = 0;
  }
  else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
    gh_speed_ref.y = gh_max_speed_ref.y;
    if (gh_accel_ref.y > 0)
      gh_accel_ref.y = 0;
  }
}
Ejemplo n.º 3
0
/**
 * main navigation routine.
 * This is called periodically evaluates the current
 * Position and stage and navigates accordingly.
 *
 * @returns TRUE until the survey is finished
 */
bool_t nav_survey_zamboni_run(void)
{
  // retain altitude
  NavVerticalAutoThrottleMode(0.0);
  NavVerticalAltitudeMode(zs.altitude, 0.0);

  //go from center of field to end of field - (before starting the syrvey)
  if (zs.stage == Z_ENTRY) {
    nav_route_xy(zs.wp_center.x, zs.wp_center.y, zs.seg_end.x, zs.seg_end.y);
    if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.wp_center.x, zs.wp_center.y, CARROT)) {
      zs.stage = Z_TURN1;
      NavVerticalAutoThrottleMode(0.0);
      nav_init_stage();
    }
  }

  //Turn from stage to return
  else if (zs.stage == Z_TURN1) {
    nav_circle_XY(zs.turn_center1.x, zs.turn_center1.y, zs.turnradius1);
    if (NavCourseCloseTo(zs.return_angle + zs.pre_leave_angle)){
      // && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT
      //calculate SEG-points for the next flyover
      VECT2_ADD(zs.seg_start, zs.sweep_width);
      VECT2_ADD(zs.seg_end, zs.sweep_width);

      zs.stage = Z_RET;
      nav_init_stage();
#ifdef DIGITAL_CAM
      LINE_START_FUNCTION;
#endif
    }
  }

  //fly the segment until seg_end is reached
  else if (zs.stage == Z_RET) {
    nav_route_xy(zs.ret_start.x, zs.ret_start.y, zs.ret_end.x, zs.ret_end.y);
    if (nav_approaching_xy(zs.ret_end.x, zs.ret_end.y, zs.ret_start.x, zs.ret_start.y, 0)) {
      zs.current_laps = zs.current_laps + 1;
#ifdef DIGITAL_CAM
      //dc_stop();
      LINE_STOP_FUNCTION;
#endif
      zs.stage = Z_TURN2;
    }
  }

  //turn from stage to return
  else if (zs.stage == Z_TURN2) {
    nav_circle_XY(zs.turn_center2.x, zs.turn_center2.y, zs.turnradius2);
    if (NavCourseCloseTo(zs.flight_angle + zs.pre_leave_angle)) {
      //zs.current_laps = zs.current_laps + 1;
      zs.stage = Z_SEG;
      nav_init_stage();
#ifdef DIGITAL_CAM
      LINE_START_FUNCTION;
#endif
    }

    //return
  } else if (zs.stage == Z_SEG) {
    nav_route_xy(zs.seg_start.x, zs.seg_start.y, zs.seg_end.x, zs.seg_end.y);
    if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, 0)) {

      // calculate the rest of the points for the next fly-over
      VECT2_ADD(zs.ret_start, zs.sweep_width);
      VECT2_ADD(zs.ret_end, zs.sweep_width);
      zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x)/2;
      zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y)/2;
      zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2;
      zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2;

      zs.stage = Z_TURN1;
      nav_init_stage();
#ifdef DIGITAL_CAM
      //dc_stop();
      LINE_STOP_FUNCTION;
#endif
    }
  }
  if (zs.current_laps >= zs.total_laps) {
#ifdef DIGITAL_CAM
    LINE_STOP_FUNCTION;
#endif
    return FALSE;
  }
  else {
    return TRUE;
  }
}
Ejemplo n.º 4
0
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
/* WARNING: SPEED SATURATION UNTESTED */
  VECT2_ADD(gh_pos_ref, gh_speed_ref);
  VECT2_ADD(gh_speed_ref, gh_accel_ref);

  // compute speed error
  struct Int32Vect2 speed_err;
  INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
  VECT2_DIFF(speed_err, gh_speed_ref, speed_err);
  // convert to accel resolution
  INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
  // compute accel from speed_sp
  VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_THAU);
  INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_THAU_FRAC);

  /* Compute route reference before saturation */
  // use metric precision or values are too large
  INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x);
  /* Compute North and East route components */
  PPRZ_ITRIG_SIN(s_route_ref, route_ref);
  PPRZ_ITRIG_COS(c_route_ref, route_ref);
  c_route_ref = abs(c_route_ref);
  s_route_ref = abs(s_route_ref);
  /* Compute maximum acceleration*/
  gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
  gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
  /* Compute maximum speed*/
  gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
  gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
  /* restore gh_speed_ref range (Q14.17) */
  INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));

  /* Saturate accelerations */
  if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
    gh_accel_ref.x = -gh_max_accel_ref.x;
  }
  else if (gh_accel_ref.x >=  gh_max_accel_ref.x) {
    gh_accel_ref.x =  gh_max_accel_ref.x;
  }
  if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
    gh_accel_ref.y = -gh_max_accel_ref.y;
  }
  else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
    gh_accel_ref.y = gh_max_accel_ref.y;
  }

  /* Saturate speed and adjust acceleration accordingly */
  if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
    gh_speed_ref.x = -gh_max_speed_ref.x;
    if (gh_accel_ref.x < 0)
      gh_accel_ref.x = 0;
  }
  else if (gh_speed_ref.x >=  gh_max_speed_ref.x) {
    gh_speed_ref.x =  gh_max_speed_ref.x;
    if (gh_accel_ref.x > 0)
      gh_accel_ref.x = 0;
  }
  if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
    gh_speed_ref.y = -gh_max_speed_ref.y;
    if (gh_accel_ref.y < 0)
      gh_accel_ref.y = 0;
  }
  else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
    gh_speed_ref.y = gh_max_speed_ref.y;
    if (gh_accel_ref.y > 0)
      gh_accel_ref.y = 0;
  }
}