コード例 #1
0
ファイル: ins.c プロジェクト: MarkGriffin/paparazzi
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 */
}
コード例 #2
0
static void compute_points_from_bungee(void)
{
  // Store init point (current position, where the plane will be released)
  VECT2_ASSIGN(init_point, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y);
  // Compute unitary 2D vector (bungee_point - init_point) = takeoff direction
  VECT2_DIFF(takeoff_dir, bungee_point, init_point);
  float_vect2_normalize(&takeoff_dir);
  // Find throttle point (the point where the throttle line and launch line intersect)
  // If TakeOff_Distance is positive, throttle point is after bungee point, before otherwise
  VECT2_SMUL(throttle_point, takeoff_dir, BUNGEE_TAKEOFF_DISTANCE);
  VECT2_SUM(throttle_point, bungee_point, throttle_point);
}
コード例 #3
0
ファイル: ins_int_extended.c プロジェクト: RockyZou/paparazzi
void ins_update_gps(void) {
#if USE_GPS
  if (gps.fix == GPS_FIX_3D) {
    if (!ins_ltp_initialised) {
      ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
      ins_ltp_def.lla.alt = gps.lla_pos.alt;
      ins_ltp_def.hmsl = gps.hmsl;
      ins_ltp_initialised = TRUE;
      stateSetLocalOrigin_i(&ins_ltp_def);
    }
    ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
    ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel);
#if 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();
#endif /* hff used */

#if !USE_HFF /* hff not 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 /* hff not used */

  }
#endif /* USE_GPS */
}
コード例 #4
0
void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) {
  if (radius == 0) {
    VECT2_COPY(navigation_target, *wp_center);
    dist2_to_wp = get_dist2_to_point(wp_center);
  }
  else {
    struct Int32Vect2 pos_diff;
    VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
    // go back to half metric precision or values are too large
    //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
    // store last qdr
    int32_t last_qdr = nav_circle_qdr;
    // compute qdr
    nav_circle_qdr = int32_atan2(pos_diff.y, pos_diff.x);
    // increment circle radians
    if (nav_circle_radians != 0) {
      int32_t angle_diff = nav_circle_qdr - last_qdr;
      INT32_ANGLE_NORMALIZE(angle_diff);
      nav_circle_radians += angle_diff;
    }
    else {
      // Smallest angle to increment at next step
      nav_circle_radians = 1;
    }

    // direction of rotation
    int8_t sign_radius = radius > 0 ? 1 : -1;
    // absolute radius
    int32_t abs_radius = abs(radius);
    // carrot_angle
    int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius);
    Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
    carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
    int32_t s_carrot, c_carrot;
    PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
    PPRZ_ITRIG_COS(c_carrot, carrot_angle);
    // compute setpoint
    VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
    INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
    VECT2_SUM(navigation_target, *wp_center, pos_diff);
  }
  nav_circle_center = *wp_center;
  nav_circle_radius = radius;
  horizontal_mode = HORIZONTAL_MODE_CIRCLE;
}
コード例 #5
0
ファイル: ins_int.c プロジェクト: hannwoei/paparazzi
void ins_reset_altitude_ref(void)
{
#if USE_GPS
  struct LlaCoor_i lla = {
    .lat = state.ned_origin_i.lla.lat,
    .lon = state.ned_origin_i.lla.lon,
    .alt = gps.lla_pos.alt
  };
  ltp_def_from_lla_i(&ins_impl.ltp_def, &lla);
  ins_impl.ltp_def.hmsl = gps.hmsl;
  stateSetLocalOrigin_i(&ins_impl.ltp_def);
#endif
  ins_impl.vf_reset = TRUE;
}

void ins_propagate(float dt)
{
  /* untilt accels */
  struct Int32Vect3 accel_meas_body;
  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
  struct Int32Vect3 accel_meas_ltp;
  int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);

  float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
  if (ins_impl.baro_initialized) {
    vff_propagate(z_accel_meas_float, dt);
    ins_update_from_vff();
  } else { // feed accel from the sensors
    // subtract -9.81m/s2 (acceleration measured due to gravity,
    // but vehicle not accelerating in ltp)
    ins_impl.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
  }

#if USE_HFF
  /* propagate horizontal filter */
  b2_hff_propagate();
  /* convert and copy result to ins_impl */
  ins_update_from_hff();
#else
  ins_impl.ltp_accel.x = accel_meas_ltp.x;
  ins_impl.ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */

  ins_ned_to_state();
}

static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure)
{
  if (!ins_impl.baro_initialized && *pressure > 1e-7) {
    // wait for a first positive value
    ins_impl.qfe = *pressure;
    ins_impl.baro_initialized = TRUE;
  }

  if (ins_impl.baro_initialized) {
    if (ins_impl.vf_reset) {
      ins_impl.vf_reset = FALSE;
      ins_impl.qfe = *pressure;
      vff_realign(0.);
      ins_update_from_vff();
    } else {
      ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe);
#if USE_VFF_EXTENDED
      vff_update_baro(ins_impl.baro_z);
#else
      vff_update(ins_impl.baro_z);
#endif
    }
    ins_ned_to_state();
  }
}

#if USE_GPS
void ins_update_gps(void)
{
  if (gps.fix == GPS_FIX_3D) {
    if (!ins_impl.ltp_initialized) {
      ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
      ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
      ins_impl.ltp_def.hmsl = gps.hmsl;
      ins_impl.ltp_initialized = TRUE;
      stateSetLocalOrigin_i(&ins_impl.ltp_def);
    }

    struct NedCoor_i gps_pos_cm_ned;
    ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
    /// @todo maybe use gps.ned_vel directly??
    struct NedCoor_i gps_speed_cm_s_ned;
    ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel);

#if INS_USE_GPS_ALT
    vff_update_z_conf((float)gps_pos_cm_ned.z / 100.0, INS_VFF_R_GPS);
#endif

#if USE_HFF
    /* horizontal gps transformed to NED in meters as float */
    struct FloatVect2 gps_pos_m_ned;
    VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
    VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);

    struct FloatVect2 gps_speed_m_s_ned;
    VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
    VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);

    if (ins_impl.hf_realign) {
      ins_impl.hf_realign = FALSE;
      const struct FloatVect2 zero = {0.0f, 0.0f};
      b2_hff_realign(gps_pos_m_ned, zero);
    }
    // run horizontal filter
    b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
    // convert and copy result to ins_impl
    ins_update_from_hff();

#else  /* hff not used */
    /* simply copy horizontal pos/speed from gps */
    INT32_VECT2_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned,
                        INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
    INT32_VECT2_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned,
                        INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif /* USE_HFF */

    ins_ned_to_state();
  }
}
#endif /* USE_GPS */


#if USE_SONAR
static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance)
{
  static float last_offset = 0.;

  /* update filter assuming a flat ground */
  if (*distance < INS_SONAR_MAX_RANGE && *distance > INS_SONAR_MIN_RANGE
#ifdef INS_SONAR_THROTTLE_THRESHOLD
      && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#endif
#ifdef INS_SONAR_BARO_THRESHOLD
      && ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
#endif
      && ins_impl.update_on_agl
      && ins_impl.baro_initialized) {
    vff_update_z_conf(-(*distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(*distance));
    last_offset = vff.offset;
  } else {
    /* update offset with last value to avoid divergence */
    vff_update_offset(last_offset);
  }
}
#endif // USE_SONAR


/** initialize the local origin (ltp_def) from flight plan position */
static void ins_init_origin_from_flightplan(void)
{

  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
  llh_nav0.lat = NAV_LAT0;
  llh_nav0.lon = NAV_LON0;
  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;

  struct EcefCoor_i ecef_nav0;
  ecef_of_lla_i(&ecef_nav0, &llh_nav0);

  ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
  ins_impl.ltp_def.hmsl = NAV_ALT0;
  stateSetLocalOrigin_i(&ins_impl.ltp_def);

}

/** copy position and speed to state interface */
static void ins_ned_to_state(void)
{
  stateSetPositionNed_i(&ins_impl.ltp_pos);
  stateSetSpeedNed_i(&ins_impl.ltp_speed);
  stateSetAccelNed_i(&ins_impl.ltp_accel);

#if defined SITL && USE_NPS
  if (nps_bypass_ins) {
    sim_overwrite_ins();
  }
#endif
}
コード例 #6
0
ファイル: ins_int.c プロジェクト: Creadid/paparazzi
void ins_reset_altitude_ref(void)
{
#if USE_GPS
  struct LlaCoor_i lla = {
    .lat = state.ned_origin_i.lla.lat,
    .lon = state.ned_origin_i.lla.lon,
    .alt = gps.lla_pos.alt
  };
  ltp_def_from_lla_i(&ins_int.ltp_def, &lla);
  ins_int.ltp_def.hmsl = gps.hmsl;
  stateSetLocalOrigin_i(&ins_int.ltp_def);
#endif
  ins_int.vf_reset = TRUE;
}

void ins_int_propagate(struct Int32Vect3 *accel, float dt)
{
  /* untilt accels */
  struct Int32Vect3 accel_meas_body;
  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
  struct Int32Vect3 accel_meas_ltp;
  int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);

  float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);

  /* Propagate only if we got any measurement during the last INS_MAX_PROPAGATION_STEPS.
   * Otherwise halt the propagation to not diverge and only set the acceleration.
   * This should only be relevant in the startup phase when the baro is not yet initialized
   * and there is no gps fix yet...
   */
  if (ins_int.propagation_cnt < INS_MAX_PROPAGATION_STEPS) {
    vff_propagate(z_accel_meas_float, dt);
    ins_update_from_vff();
  } else {
    // feed accel from the sensors
    // subtract -9.81m/s2 (acceleration measured due to gravity,
    // but vehicle not accelerating in ltp)
    ins_int.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
  }

#if USE_HFF
  /* propagate horizontal filter */
  b2_hff_propagate();
  /* convert and copy result to ins_int */
  ins_update_from_hff();
#else
  ins_int.ltp_accel.x = accel_meas_ltp.x;
  ins_int.ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */

  ins_ned_to_state();

  /* increment the propagation counter, while making sure it doesn't overflow */
  if (ins_int.propagation_cnt < 100 * INS_MAX_PROPAGATION_STEPS) {
    ins_int.propagation_cnt++;
  }
}

static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
{
  if (!ins_int.baro_initialized && pressure > 1e-7) {
    // wait for a first positive value
    ins_int.qfe = pressure;
    ins_int.baro_initialized = TRUE;
  }

  if (ins_int.baro_initialized) {
    if (ins_int.vf_reset) {
      ins_int.vf_reset = FALSE;
      ins_int.qfe = pressure;
      vff_realign(0.);
      ins_update_from_vff();
    } else {
      ins_int.baro_z = -pprz_isa_height_of_pressure(pressure, ins_int.qfe);
#if USE_VFF_EXTENDED
      vff_update_baro(ins_int.baro_z);
#else
      vff_update(ins_int.baro_z);
#endif
    }
    ins_ned_to_state();

    /* reset the counter to indicate we just had a measurement update */
    ins_int.propagation_cnt = 0;
  }
}

#if USE_GPS
void ins_int_update_gps(struct GpsState *gps_s)
{
  if (gps_s->fix < GPS_FIX_3D) {
    return;
  }

  if (!ins_int.ltp_initialized) {
    ins_reset_local_origin();
  }

  struct NedCoor_i gps_pos_cm_ned;
  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps_s->ecef_pos);

  /* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */
#ifdef INS_BODY_TO_GPS_X
  /* body2gps translation in body frame */
  struct Int32Vect3 b2g_b = {
    .x = INS_BODY_TO_GPS_X,
    .y = INS_BODY_TO_GPS_Y,
    .z = INS_BODY_TO_GPS_Z
  };
  /* rotate offset given in body frame to navigation/ltp frame using current attitude */
  struct Int32Quat q_b2n = *stateGetNedToBodyQuat_i();
  QUAT_INVERT(q_b2n, q_b2n);
  struct Int32Vect3 b2g_n;
  int32_quat_vmult(&b2g_n, &q_b2n, &b2g_b);
  /* subtract body2gps translation in ltp from gps position */
  VECT3_SUB(gps_pos_cm_ned, b2g_n);
#endif

  /// @todo maybe use gps_s->ned_vel directly??
  struct NedCoor_i gps_speed_cm_s_ned;
  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps_s->ecef_vel);

#if INS_USE_GPS_ALT
  vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
#endif
#if INS_USE_GPS_ALT_SPEED
  vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
#endif

#if USE_HFF
  /* horizontal gps transformed to NED in meters as float */
  struct FloatVect2 gps_pos_m_ned;
  VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
  VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);

  struct FloatVect2 gps_speed_m_s_ned;
  VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
  VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);

  if (ins_int.hf_realign) {
    ins_int.hf_realign = FALSE;
    const struct FloatVect2 zero = {0.0f, 0.0f};
    b2_hff_realign(gps_pos_m_ned, zero);
  }
  // run horizontal filter
  b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
  // convert and copy result to ins_int
  ins_update_from_hff();

#else  /* hff not used */
  /* simply copy horizontal pos/speed from gps */
  INT32_VECT2_SCALE_2(ins_int.ltp_pos, gps_pos_cm_ned,
                      INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
  INT32_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned,
                      INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif /* USE_HFF */

  ins_ned_to_state();

  /* reset the counter to indicate we just had a measurement update */
  ins_int.propagation_cnt = 0;
}
#else
void ins_int_update_gps(struct GpsState *gps_s __attribute__((unused))) {}
#endif /* USE_GPS */


#if USE_SONAR
static void sonar_cb(uint8_t __attribute__((unused)) sender_id, float distance)
{
  static float last_offset = 0.;

  /* update filter assuming a flat ground */
  if (distance < INS_SONAR_MAX_RANGE && distance > INS_SONAR_MIN_RANGE
#ifdef INS_SONAR_THROTTLE_THRESHOLD
      && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#endif
#ifdef INS_SONAR_BARO_THRESHOLD
      && ins_int.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
#endif
      && ins_int.update_on_agl
      && ins_int.baro_initialized) {
    vff_update_z_conf(-(distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance));
    last_offset = vff.offset;
  } else {
    /* update offset with last value to avoid divergence */
    vff_update_offset(last_offset);
  }

  /* reset the counter to indicate we just had a measurement update */
  ins_int.propagation_cnt = 0;
}
#endif // USE_SONAR


/** initialize the local origin (ltp_def) from flight plan position */
static void ins_init_origin_from_flightplan(void)
{

  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
  llh_nav0.lat = NAV_LAT0;
  llh_nav0.lon = NAV_LON0;
  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;

  struct EcefCoor_i ecef_nav0;
  ecef_of_lla_i(&ecef_nav0, &llh_nav0);

  ltp_def_from_ecef_i(&ins_int.ltp_def, &ecef_nav0);
  ins_int.ltp_def.hmsl = NAV_ALT0;
  stateSetLocalOrigin_i(&ins_int.ltp_def);

}

/** copy position and speed to state interface */
static void ins_ned_to_state(void)
{
  stateSetPositionNed_i(&ins_int.ltp_pos);
  stateSetSpeedNed_i(&ins_int.ltp_speed);
  stateSetAccelNed_i(&ins_int.ltp_accel);

#if defined SITL && USE_NPS
  if (nps_bypass_ins) {
    sim_overwrite_ins();
  }
#endif
}
コード例 #7
0
bool nav_bungee_takeoff_run(void)
{
  float cross = 0.;

  // Get current position
  struct FloatVect2 pos;
  VECT2_ASSIGN(pos, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y);

  switch (CTakeoffStatus) {
    case Launch:
      // Recalculate lines if below min speed
      if (stateGetHorizontalSpeedNorm_f() < BUNGEE_TAKEOFF_MIN_SPEED) {
        compute_points_from_bungee();
      }

      // Follow Launch Line with takeoff pitch and no throttle
      NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH);
      NavVerticalThrottleMode(0);
      // FIXME previously using altitude mode, maybe not wise without motors
      //NavVerticalAltitudeMode(bungee_point.z + BUNGEE_TAKEOFF_HEIGHT, 0.);
      nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y);

      kill_throttle = 1;

      // Find out if UAV has crossed the line
      VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point
      cross = VECT2_DOT_PRODUCT(pos, takeoff_dir);

      if (cross > 0. && stateGetHorizontalSpeedNorm_f() > BUNGEE_TAKEOFF_MIN_SPEED) {
        CTakeoffStatus = Throttle;
        kill_throttle = 0;
        nav_init_stage();
      } else {
        // If not crossed stay in this status
        break;
      }
    // Start throttle imidiatelly
    case Throttle:
      //Follow Launch Line
      NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH);
      NavVerticalThrottleMode(MAX_PPRZ * (BUNGEE_TAKEOFF_THROTTLE));
      nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y);
      kill_throttle = 0;

      if ((stateGetPositionUtm_f()->alt > bungee_point.z + BUNGEE_TAKEOFF_HEIGHT)
#if USE_AIRSPEED
          && (stateGetAirspeed_f() > BUNGEE_TAKEOFF_AIRSPEED)
#endif
          ) {
        CTakeoffStatus = Finished;
        return false;
      } else {
        return true;
      }
      break;
    default:
      // Invalid status or Finished, end function
      return false;
  }
  return true;
}