Exemple #1
0
bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, float radius_inc, float segments)
{
  VECT2_COPY(nav_spiral.center, waypoints[center_wp]);    // center of the helix
  nav_spiral.center.z = waypoints[center_wp].a;
  nav_spiral.radius_start = radius_start;   // start radius of the helix
  nav_spiral.segments = segments;
  nav_spiral.radius_min = NAV_SPIRAL_MIN_CIRCLE_RADIUS;
  if (nav_spiral.radius_start < nav_spiral.radius_min)
    nav_spiral.radius_start = nav_spiral.radius_min;
  nav_spiral.radius_increment = radius_inc;     // multiplier for increasing the spiral

  struct FloatVect2 edge;
  VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);

  FLOAT_VECT2_NORM(nav_spiral.radius, edge);

  // get a copy of the current position
  struct EnuCoor_f pos_enu;
  memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));

  VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
  nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z;

  nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);

  // nav_spiral.alpha_limit denotes angle, where the radius will be increased
  nav_spiral.alpha_limit = 2*M_PI / nav_spiral.segments;
  //current position
  nav_spiral.fly_from.x = stateGetPositionEnu_f()->x;
  nav_spiral.fly_from.y = stateGetPositionEnu_f()->y;

  if(nav_spiral.dist_from_center > nav_spiral.radius)
    nav_spiral.status = SpiralOutside;
  return FALSE;
}
Exemple #2
0
void ahrs_update_accel(void) {

    /* last column of roation matrix = ltp z-axis in imu-frame */
    struct FloatVect3  c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
               RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2),
               RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)
    };

    struct FloatVect3 imu_accel_float;
    ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);

    struct FloatVect3 residual;

    if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
        /*
         * centrifugal acceleration in body frame
         * a_c_body = omega x (omega x r)
         * (omega x r) = tangential velocity in body frame
         * a_c_body = omega x vel_tangential_body
         * assumption: tangential velocity only along body x-axis
         */
        const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
        struct FloatVect3 acc_c_body;
        VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);

        /* convert centrifugal acceleration from body to imu frame */
        struct FloatVect3 acc_c_imu;
        FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);

        /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
        struct FloatVect3 corrected_gravity;
        VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu);

        /* compute the residual of gravity vector in imu frame */
        FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
    } else {
        FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2);
    }

#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
    /* heuristic on acceleration norm */
    const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float);
    const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
#else
    const float weight = 1.;
#endif

    /* compute correction */
    const float gravity_rate_update_gain = -5e-2; // -5e-2
    FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);

    const float gravity_bias_update_gain = 1e-5; // -5e-6
    FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);

    /* FIXME: saturate bias */

}
void ahrs_update_accel(void) {
  struct FloatVect3 imu_g;
  ACCELS_FLOAT_OF_BFP(imu_g, imu.accel);
  const float alpha = 0.92;
  ahrs_impl.lp_accel = alpha * ahrs_impl.lp_accel +
    (1. - alpha) *(FLOAT_VECT3_NORM(imu_g) - 9.81);
  const struct FloatVect3 earth_g = {0.,  0., -9.81 };
  const float dn = 250*fabs( ahrs_impl.lp_accel );
  struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn};
  update_state(&earth_g, &imu_g, &g_noise);
  reset_state();
}
void ahrs_update_accel(void) {

  struct FloatVect3 accel_float;
  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);

#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  float v = FLOAT_VECT3_NORM(ahrs_impl.est_ltp_speed);
  accel_float.y -=  v * ahrs_float.imu_rate.r;
  accel_float.z -= -v * ahrs_float.imu_rate.q;
#endif

  struct FloatVect3  c2 = { RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,2),
			    RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,2),
			    RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)};
  struct FloatVect3 residual;
  FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, c2);
#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
  /* heuristic on acceleration norm */
  const float acc_norm = FLOAT_VECT3_NORM(accel_float);
  const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
#else
  const float weight = 1.;
#endif
  /* compute correction */
  const float gravity_rate_update_gain = -5e-2; // -5e-2
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
#if 1
  const float gravity_bias_update_gain = 1e-5; // -5e-6
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
#else
  const float alpha = 5e-4;
  FLOAT_RATES_SCALE(ahrs_impl.gyro_bias, 1.-alpha);
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, alpha);
#endif
  /* FIXME: saturate bias */

}
void ahrs_update_accel(void) {

  struct FloatVect3 accel_float;
  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
  struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
  struct FloatVect3 residual;
  FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, (*r2));
  /* heuristic on acceleration norm */
  const float acc_norm = FLOAT_VECT3_NORM(accel_float);
  const float weight = Chop(1.-2*fabs(1-acc_norm/9.81), 0., 1.);
  //const float weight = 1.;
  /* compute correction */
  const float gravity_rate_update_gain = 5e-2;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
  const float gravity_bias_update_gain = -5e-6;
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
  /* FIXME: saturate bias */

}
Exemple #6
0
void aos_compute_sensors(void) {

  struct FloatRates gyro;
  RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias);
  //  printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r));

  float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise);

  RATES_BFP_OF_REAL(imu.gyro, gyro);
  RATES_BFP_OF_REAL(imu.gyro_prev, gyro);

  struct FloatVect3 g_ltp = {0., 0., 9.81};
  struct FloatVect3 accelero_ltp;
  VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp);
  struct FloatVect3 accelero_imu;
  FLOAT_QUAT_VMULT(accelero_imu, aos.ltp_to_imu_quat, accelero_ltp);

  float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise);
  ACCELS_BFP_OF_REAL(imu.accel, accelero_imu);
  
  
  struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
  struct FloatVect3 h_imu;
  FLOAT_QUAT_VMULT(h_imu, aos.ltp_to_imu_quat, h_earth);
  MAGS_BFP_OF_REAL(imu.mag, h_imu);

#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
#if AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ
  VECT3_COPY(ahrs_impl.est_ltp_speed, aos.ltp_vel);
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
  ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel));
#endif
#endif


}
Exemple #7
0
bool nav_spiral_run(void)
{
  struct EnuCoor_f pos_enu = *stateGetPositionEnu_f();

  VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
  nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);

  float DistanceStartEstim;
  float CircleAlpha;

  switch (nav_spiral.status) {
    case SpiralOutside:
      //flys until center of the helix is reached an start helix
      nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y);
      // center reached?
      if (nav_approaching_xy(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.fly_from.x, nav_spiral.fly_from.y, 0)) {
        // nadir image
#ifdef DIGITAL_CAM
        dc_send_command(DC_SHOOT);
#endif
        nav_spiral.status = SpiralStartCircle;
      }
      break;
    case SpiralStartCircle:
      // Starts helix
      // storage of current coordinates
      // calculation needed, State switch to SpiralCircle
      nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start);
      if (nav_spiral.dist_from_center >= nav_spiral.radius_start) {
        VECT2_COPY(nav_spiral.last_circle, pos_enu);
        nav_spiral.status = SpiralCircle;
        // Start helix
#ifdef DIGITAL_CAM
        dc_Circle(360 / nav_spiral.segments);
#endif
      }
      break;
    case SpiralCircle: {
      nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start);
      // Trigonometrische Berechnung des bereits geflogenen Winkels alpha
      // equation:
      // alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start)
      // if alphamax already reached, increase radius.

      //DistanceStartEstim = |Starting position angular - current positon|
      struct FloatVect2 pos_diff;
      VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu);
      DistanceStartEstim = float_vect2_norm(&pos_diff);
      CircleAlpha = (2.0 * asin(DistanceStartEstim / (2 * nav_spiral.radius_start)));
      if (CircleAlpha >= nav_spiral.alpha_limit) {
        VECT2_COPY(nav_spiral.last_circle, pos_enu);
        nav_spiral.status = SpiralInc;
      }
      break;
    }
    case SpiralInc:
      // increasing circle radius as long as it is smaller than max helix radius
      if (nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius) {
        nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment;
#ifdef DIGITAL_CAM
        if (dc_cam_tracing) {
          // calculating Cam angle for camera alignment
          nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z;
          dc_cam_angle = atan(nav_spiral.radius_start / nav_spiral.trans_current.z) * 180  / M_PI;
        }
#endif
      } else {
        nav_spiral.radius_start = nav_spiral.radius;
#ifdef DIGITAL_CAM
        // Stopps DC
        dc_stop();
#endif
      }
      nav_spiral.status = SpiralCircle;
      break;
    default:
      break;
  }

  NavVerticalAutoThrottleMode(0.); /* No pitch */
  NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */

  return true;
}
void ahrs_update_accel(void) {

  /* last column of roation matrix = ltp z-axis in imu-frame */
  struct FloatVect3  c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2),
                            RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2),
                            RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)};

  struct FloatVect3 imu_accel_float;
  ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);

  struct FloatVect3 residual;

  struct FloatVect3 pseudo_gravity_measurement;

  if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
    /*
     * centrifugal acceleration in body frame
     * a_c_body = omega x (omega x r)
     * (omega x r) = tangential velocity in body frame
     * a_c_body = omega x vel_tangential_body
     * assumption: tangential velocity only along body x-axis
     */
    const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
    struct FloatVect3 acc_c_body;
    VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body);

    /* convert centrifugal acceleration from body to imu frame */
    struct FloatVect3 acc_c_imu;
    FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);

    /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
    VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);

  } else {
    VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
  }

  FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);

  /* FIR filtered pseudo_gravity_measurement */
  #define FIR_FILTER_SIZE 8
  static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
  VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1);
  VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
  VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);

  if (ahrs_impl.gravity_heuristic_factor) {
    /* heuristic on acceleration (gravity estimate) norm */
    /* Factor how strongly to change the weight.
     * e.g. for gravity_heuristic_factor 30:
     * <0.66G = 0, 1G = 1.0, >1.33G = 0
     */

    const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81;
    ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
    Bound(ahrs_impl.weight, 0.15, 1.0);
  }
  else {
    ahrs_impl.weight = 1.0;
  }

  /* Complementary filter proportional gain.
   * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY
   */
  const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega *
    ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81);
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain);

  /* Complementary filter integral gain
   * Correct the gyro bias.
   * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
   */
  const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega *
    ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81);
  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain);

  /* FIXME: saturate bias */

}
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) {

  /* cmd_x is positive to north = negative pitch
   * cmd_y is positive to east = positive roll
   *
   * orientation vector describing simultaneous rotation of roll/pitch
   */
  const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0};
  /* quaternion from that orientation vector */
  struct FloatQuat q_rp;
  float_quat_of_orientation_vect(&q_rp, &ov);

  /* as rotation matrix */
  struct FloatRMat R_rp;
  float_rmat_of_quat(&R_rp, &q_rp);
  /* body x-axis (before heading command) is first column */
  struct FloatVect3 b_x;
  VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]);
  /* body z-axis (thrust vect) is last column */
  struct FloatVect3 thrust_vect;
  VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]);

  /// @todo optimize yaw angle calculation

  /*
   * Instead of using the psi setpoint angle to rotate around the body z-axis,
   * calculate the real angle needed to align the projection of the body x-axis
   * onto the horizontal plane with the psi setpoint.
   *
   * angle between two vectors a and b:
   * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
   * where the normal n is the thrust vector (i.e. both a and b lie in that plane)
   */

  // desired heading vect in earth x-y plane
  const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0};

  /* projection of desired heading onto body x-y plane
   * b = v - dot(v,n)*n
   */
  float dot = VECT3_DOT_PRODUCT(psi_vect, thrust_vect);
  struct FloatVect3 dotn;
  VECT3_SMUL(dotn, thrust_vect, dot);

  // b = v - dot(v,n)*n
  struct FloatVect3 b;
  VECT3_DIFF(b, psi_vect, dotn);
  dot = VECT3_DOT_PRODUCT(b_x, b);
  struct FloatVect3 cross;
  VECT3_CROSS_PRODUCT(cross, b_x, b);
  // norm of the cross product
  float nc = FLOAT_VECT3_NORM(cross);
  // angle = atan2(norm(cross(a,b)), dot(a,b))
  float yaw2 = atan2(nc, dot) / 2.0;

  // negative angle if needed
  // sign(dot(cross(a,b), n)
  float dot_cross_ab = VECT3_DOT_PRODUCT(cross, thrust_vect);
  if (dot_cross_ab < 0) {
    yaw2 = -yaw2;
  }

  /* quaternion with yaw command */
  struct FloatQuat q_yaw;
  QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));

  /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
  float_quat_comp(quat, &q_rp, &q_yaw);
  float_quat_normalize(quat);
  float_quat_wrap_shortest(quat);

}