void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so)
{
  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
  nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
  survey_orientation = so;

  if (survey_orientation == NS) {
    survey_from.x = survey_to.x = Min(Max(stateGetPositionEnu_f()->x, nav_survey_west + grid / 2.),
                                      nav_survey_east - grid / 2.);
    if (stateGetPositionEnu_f()->y > nav_survey_north || (stateGetPositionEnu_f()->y > nav_survey_south
                                                          && stateGetHorizontalSpeedDir_f() > M_PI / 2. && stateGetHorizontalSpeedDir_f() < 3 * M_PI / 2)) {
      survey_to.y = nav_survey_south;
      survey_from.y = nav_survey_north;
    } else {
      survey_from.y = nav_survey_south;
      survey_to.y = nav_survey_north;
    }
  } else { /* survey_orientation == WE */
    survey_from.y = survey_to.y = Min(Max(stateGetPositionEnu_f()->y, nav_survey_south + grid / 2.),
                                      nav_survey_north - grid / 2.);
    if (stateGetPositionEnu_f()->x > nav_survey_east || (stateGetPositionEnu_f()->x > nav_survey_west
        && stateGetHorizontalSpeedDir_f() > M_PI)) {
      survey_to.x = nav_survey_west;
      survey_from.x = nav_survey_east;
    } else {
      survey_from.x = nav_survey_west;
      survey_to.x = nav_survey_east;
    }
  }
  nav_survey_shift = grid;
  survey_uturn = FALSE;
  LINE_START_FUNCTION;
}
Example #2
0
bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) {

  if (chemo_sensor > last_plume_value) {
    /* Move the circle in this direction */
    float x = stateGetPositionEnu_f()->x - waypoints[plume].x;
    float y = stateGetPositionEnu_f()->y - waypoints[plume].y;
    waypoints[c].x = waypoints[plume].x + ALPHA * x;
    waypoints[c].y = waypoints[plume].y + ALPHA * y;
    //    DownlinkSendWp(c);
    /* Turn in the right direction */
    float dir_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
    float dir_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
    float pvect = dir_x*y - dir_y*x;
    sign = (pvect > 0 ? -1 : 1);
    /* Reduce the radius */
    radius = sign * (DEFAULT_CIRCLE_RADIUS+(MAX_CHEMO-chemo_sensor)/(float)MAX_CHEMO*(MAX_RADIUS-DEFAULT_CIRCLE_RADIUS));


    /* Store this plume */
    waypoints[plume].x = stateGetPositionEnu_f()->x;
    waypoints[plume].y = stateGetPositionEnu_f()->y;
    // DownlinkSendWp(plume);
    last_plume_value = chemo_sensor;
  }

  NavCircleWaypoint(c, radius);
  return TRUE;
}
Example #3
0
/** Update the RELEASE location with the actual ground speed and altitude */
unit_t bomb_update_release( uint8_t wp_target ) {

  bomb_z = stateGetPositionEnu_f()->z - waypoints[wp_target].a;
  bomb_x = 0.;
  bomb_y = 0.;

  bomb_vx = (*stateGetHorizontalSpeedNorm_f()) * sin((*stateGetHorizontalSpeedDir_f()));
  bomb_vy = (*stateGetHorizontalSpeedNorm_f()) * cos((*stateGetHorizontalSpeedDir_f()));
  bomb_vz = 0.;

  integrate(wp_target);

  return 0;
}
Example #4
0
/** Update the RELEASE location with the actual ground speed and altitude */
unit_t nav_drop_update_release( uint8_t wp_target ) {

  nav_drop_z = stateGetPositionUtm_f()->alt - waypoints[wp_target].a;
  nav_drop_x = 0.;
  nav_drop_y = 0.;

  nav_drop_vx = (*stateGetHorizontalSpeedNorm_f()) * sin((*stateGetHorizontalSpeedDir_f()));
  nav_drop_vy = (*stateGetHorizontalSpeedNorm_f()) * cos((*stateGetHorizontalSpeedDir_f()));
  nav_drop_vz = 0.;

  integrate(wp_target);

  return 0;
}
Example #5
0
void dc_send_shot_position(void)
{
  // angles in decideg
  int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f);
  int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f);
  int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi*10.0f);
  // course in decideg
  int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10;
  // ground speed in cm/s
  uint16_t speed = (*stateGetHorizontalSpeedNorm_f()) * 10;
  int16_t photo_nr = -1;

  if (dc_photo_nr < DC_IMAGE_BUFFER) {
    dc_photo_nr++;
    photo_nr = dc_photo_nr;
  }

  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
                        &photo_nr,
                        &stateGetPositionLla_i()->lat,
                        &stateGetPositionLla_i()->lon,
                        &stateGetPositionLla_i()->alt,
                        &gps.hmsl,
                        &phi,
                        &theta,
                        &psi,
                        &course,
                        &speed,
                        &gps.tow);
}
Example #6
0
File: cam.c Project: AxSt/paparazzi
/** Computes the right angles from target_x, target_y, target_alt */
void cam_target( void ) {
#ifdef TEST_CAM
  vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z,
         test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir,
         cam_target_x, cam_target_y, cam_target_alt,
         &cam_pan_c, &cam_tilt_c);
#else
  struct EnuCoor_f* pos = stateGetPositionEnu_f();
  struct FloatEulers* att = stateGetNedToBodyEulers_f();
  vPoint(pos->x, pos->y, pos->z,
         att->phi, att->theta, *stateGetHorizontalSpeedDir_f(),
         cam_target_x, cam_target_y, cam_target_alt,
         &cam_pan_c, &cam_tilt_c);
#endif
  cam_angles();
}
Example #7
0
void dc_send_shot_position(void)
{
  // angles in decideg
  int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f);
  int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
  int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
  // course in decideg
  int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
  // ground speed in cm/s
  uint16_t speed = stateGetHorizontalSpeedNorm_f() * 10;
  int16_t photo_nr = -1;

  if (dc_photo_nr < DC_IMAGE_BUFFER) {
    dc_photo_nr++;
    photo_nr = dc_photo_nr;
  }

#if DC_SHOT_EXTRA_DL
  // send a message on second datalink first
  // (for instance an embedded CPU)
  DOWNLINK_SEND_DC_SHOT(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
                        &photo_nr,
                        &stateGetPositionLla_i()->lat,
                        &stateGetPositionLla_i()->lon,
                        &stateGetPositionLla_i()->alt,
                        &gps.hmsl,
                        &phi,
                        &theta,
                        &psi,
                        &course,
                        &speed,
                        &gps.tow);
#endif
  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
                        &photo_nr,
                        &stateGetPositionLla_i()->lat,
                        &stateGetPositionLla_i()->lon,
                        &stateGetPositionLla_i()->alt,
                        &gps.hmsl,
                        &phi,
                        &theta,
                        &psi,
                        &course,
                        &speed,
                        &gps.tow);
}
Example #8
0
//static inline void fly_to_xy(float x, float y) {
void fly_to_xy(float x, float y) {
  struct EnuCoor_f* pos = stateGetPositionEnu_f();
  desired_x = x;
  desired_y = y;
  if (nav_mode == NAV_MODE_COURSE) {
    h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y);
    if (h_ctl_course_setpoint < 0.)
      h_ctl_course_setpoint += 2 * M_PI;
    lateral_mode = LATERAL_MODE_COURSE;
  } else {
    float diff = atan2f(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f());
    NormRadAngle(diff);
    BoundAbs(diff,M_PI/2.);
    float s = sinf(diff);
    float speed = *stateGetHorizontalSpeedNorm_f();
    h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) );
    BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
    lateral_mode = LATERAL_MODE_ROLL;
  }
}
/**
 * \brief
 *
 */
void h_ctl_course_loop ( void ) {
  static float last_err;

  // Ground path error
  float err = h_ctl_course_setpoint - (*stateGetHorizontalSpeedDir_f());
  NormRadAngle(err);

  float d_err = err - last_err;
  last_err = err;
  NormRadAngle(d_err);

  float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
  Bound(speed_depend_nav, 0.66, 1.5);

  h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank
    + h_ctl_course_pgain * speed_depend_nav * err
    + h_ctl_course_dgain * d_err;

  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
/**
 * \brief
 *
 */
void h_ctl_course_loop(void)
{
  static float last_err;

  // Ground path error
  float err = stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint;
  NormRadAngle(err);

#ifdef STRONG_WIND
  // Usefull path speed
  const float reference_advance = (NOMINAL_AIRSPEED / 2.);
  float advance = cos(err) * stateGetHorizontalSpeedNorm_f() / reference_advance;

  if (
    (advance < 1.)  &&                          // Path speed is small
    (stateGetHorizontalSpeedNorm_f() < reference_advance)  // Small path speed is due to wind (small groundspeed)
  ) {
    /*
    // rough crabangle approximation
    float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
    float wind_dir = atan2(wind_east,wind_north);

    float wind_course = h_ctl_course_setpoint - wind_dir;
    NormRadAngle(wind_course);

    estimator_hspeed_dir = estimator_psi;

    float crab = sin(wind_dir-estimator_psi) * atan2(wind_mod,NOMINAL_AIRSPEED);
    //crab = estimator_hspeed_mod - estimator_psi;
    NormRadAngle(crab);
    */

    // Heading error
    float herr = stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);
    NormRadAngle(herr);

    if (advance < -0.5) {            //<! moving in the wrong direction / big > 90 degree turn
      err = herr;
    } else if (advance < 0.) {       //<!
      err = (-advance) * 2. * herr;
    } else {
      err = advance * err;
    }

    // Reset differentiator when switching mode
    //if (h_ctl_course_heading_mode == 0)
    //  last_err = err;
    //h_ctl_course_heading_mode = 1;
  }
  /*  else
      {
      // Reset differentiator when switching mode
      if (h_ctl_course_heading_mode == 1)
      last_err = err;
      h_ctl_course_heading_mode = 0;
      }
  */
#endif //STRONG_WIND

  float d_err = err - last_err;
  last_err = err;

  NormRadAngle(d_err);

#ifdef H_CTL_COURSE_SLEW_INCREMENT
  /* slew severe course changes (i.e. waypoint moves, block changes or perpendicular routes) */
  static float h_ctl_course_slew_rate = 0.;
  float nav_angle_saturation = h_ctl_roll_max_setpoint / h_ctl_course_pgain; /* heading error corresponding to max_roll */
  float half_nav_angle_saturation = nav_angle_saturation / 2.;
  if (autopilot.launch) {  /* prevent accumulator run-up on the ground */
    if (err > half_nav_angle_saturation) {
      h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
      err = Min(err, (half_nav_angle_saturation + h_ctl_course_slew_rate));
      h_ctl_course_slew_rate += h_ctl_course_slew_increment;
    } else if (err < -half_nav_angle_saturation) {
      h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
      err = Max(err, (-half_nav_angle_saturation + h_ctl_course_slew_rate));
      h_ctl_course_slew_rate -= h_ctl_course_slew_increment;
    } else {
      h_ctl_course_slew_rate = 0.;
    }
  }
#endif

  float speed_depend_nav = stateGetHorizontalSpeedNorm_f() / NOMINAL_AIRSPEED;
  Bound(speed_depend_nav, 0.66, 1.5);

  float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);



#if defined(AGR_CLIMB) && !USE_AIRSPEED
  /** limit navigation during extreme altitude changes */
  if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */
    if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE ||
        v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_BLENDED) {
      BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */
      /* altitude: z-up is positive -> positive error -> too low */
      if (v_ctl_altitude_error > 0) {
        nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
                    (AGR_BLEND_START - AGR_BLEND_END));
        Bound(nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
      } else {
        nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
                    (AGR_BLEND_START - AGR_BLEND_END));
        Bound(nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
      }
      cmd *= nav_ratio;
    }
  }
#endif

  float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank;

#ifdef H_CTL_ROLL_SLEW
  float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
  BoundAbs(diff_roll, h_ctl_roll_slew);
  h_ctl_roll_setpoint += diff_roll;
#else
  h_ctl_roll_setpoint = roll_setpoint;
#endif

  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
Example #11
0
int formation_flight(void)
{

  static uint8_t _1Hz   = 0;
  uint8_t nb = 0, i;
  float hspeed_dir = stateGetHorizontalSpeedDir_f();
  float ch = cosf(hspeed_dir);
  float sh = sinf(hspeed_dir);
  form_n = 0.;
  form_e = 0.;
  form_a = 0.;
  form_speed = stateGetHorizontalSpeedNorm_f();
  form_speed_n = form_speed * ch;
  form_speed_e = form_speed * sh;

  if (AC_ID == leader_id) {
    stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east;
    stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north;
  }
  // set info for this AC
  set_ac_info(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, hspeed_dir,
            stateGetPositionUtm_f()->alt, form_speed, stateGetSpeedEnu_f()->z, gps.tow);

  // broadcast info
  uint8_t ac_id = AC_ID;
  uint8_t status = formation[the_acs_id[AC_ID]].status;
  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status);
  if (++_1Hz >= 4) {
    _1Hz = 0;
    DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &ac_id, &form_mode,
                                    &formation[the_acs_id[AC_ID]].east,
                                    &formation[the_acs_id[AC_ID]].north,
                                    &formation[the_acs_id[AC_ID]].alt);
  }
  if (formation[the_acs_id[AC_ID]].status != ACTIVE) { return FALSE; } // AC not ready

  // get leader info
  struct ac_info_ * leader = get_ac_info(leader_id);
  if (formation[the_acs_id[leader_id]].status == UNSET ||
      formation[the_acs_id[leader_id]].status == IDLE) {
    // leader not ready or not in formation
    return FALSE;
  }

  // compute slots in the right reference frame
  struct slot_ form[NB_ACS];
  float cr = 0., sr = 1.;
  if (form_mode == FORM_MODE_COURSE) {
    cr = cosf(leader->course);
    sr = sinf(leader->course);
  }
  for (i = 0; i < NB_ACS; ++i) {
    if (formation[i].status == UNSET) { continue; }
    form[i].east  = formation[i].east * sr - formation[i].north * cr;
    form[i].north = formation[i].east * cr + formation[i].north * sr;
    form[i].alt = formation[i].alt;
  }

  // compute control forces
  for (i = 0; i < NB_ACS; ++i) {
    if (the_acs[i].ac_id == AC_ID) { continue; }
    struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
    float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
    if (delta_t > FORM_CARROT) {
      // if AC not responding for too long
      formation[i].status = LOST;
      continue;
    } else {
      // compute control if AC is ACTIVE and around the same altitude (maybe not so usefull)
      formation[i].status = ACTIVE;
      if (ac->alt > 0 && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox) {
        form_e += (ac->east  + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x)
          - (form[i].east - form[the_acs_id[AC_ID]].east);
        form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y)
          - (form[i].north - form[the_acs_id[AC_ID]].north);
        form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
        form_speed += ac->gspeed;
        //form_speed_e += ac->gspeed * sinf(ac->course);
        //form_speed_n += ac->gspeed * cosf(ac->course);
        ++nb;
      }
    }
  }
  uint8_t _nb = Max(1, nb);
  form_n /= _nb;
  form_e /= _nb;
  form_a /= _nb;
  form_speed = form_speed / (nb + 1) - stateGetHorizontalSpeedNorm_f();
  //form_speed_e = form_speed_e / (nb+1) - stateGetHorizontalSpeedNorm_f() * sh;
  //form_speed_n = form_speed_n / (nb+1) - stateGetHorizontalSpeedNorm_f() * ch;

  // set commands
  NavVerticalAutoThrottleMode(0.);

  // altitude loop
  float alt = 0.;
  if (AC_ID == leader_id) {
    alt = nav_altitude;
  } else {
    alt = leader->alt - form[the_acs_id[leader_id]].alt;
  }
  alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a;
  flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT);

  // carrot
  if (AC_ID != leader_id) {
    float dx = form[the_acs_id[AC_ID]].east - form[the_acs_id[leader_id]].east;
    float dy = form[the_acs_id[AC_ID]].north - form[the_acs_id[leader_id]].north;
    desired_x = leader->east  + NOMINAL_AIRSPEED * form_carrot * sinf(leader->course) + dx;
    desired_y = leader->north + NOMINAL_AIRSPEED * form_carrot * cosf(leader->course) + dy;
    // fly to desired
    fly_to_xy(desired_x, desired_y);
    desired_x = leader->east  + dx;
    desired_y = leader->north + dy;
    // lateral correction
    //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
    //float diff_course = leader->course - hspeed_dir;
    //NormRadAngle(diff_course);
    //h_ctl_roll_setpoint += coef_form_course * diff_course;
    //h_ctl_roll_setpoint += coef_form_course * diff_heading;
  }
  //BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);

  // speed loop
  if (nb > 0) {
    float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
    cruise += coef_form_pos * (form_n * ch + form_e * sh) + coef_form_speed * form_speed;
    Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
    v_ctl_auto_throttle_cruise_throttle = cruise;
  }
  return TRUE;
}
Example #12
0
void airborne_ant_point_periodic(void)
{
  float airborne_ant_pan_servo = 0;

  static VECTOR svPlanePosition,
         Home_Position,
         Home_PositionForPlane,
         Home_PositionForPlane2;

  static MATRIX smRotation;

  svPlanePosition.fx = stateGetPositionEnu_f()->y;
  svPlanePosition.fy = stateGetPositionEnu_f()->x;
  svPlanePosition.fz = stateGetPositionUtm_f()->alt;

  Home_Position.fx = WaypointY(WP_HOME);
  Home_Position.fy = WaypointX(WP_HOME);
  Home_Position.fz = waypoints[WP_HOME].a;

  /* distance between plane and object */
  vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);

  /* yaw */
  smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f());
  smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f());
  smRotation.fx3 = 0.;
  smRotation.fy1 = -smRotation.fx2;
  smRotation.fy2 = smRotation.fx1;
  smRotation.fy3 = 0.;
  smRotation.fz1 = 0.;
  smRotation.fz2 = 0.;
  smRotation.fz3 = 1.;

  vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane);


  /*
   * This is for one axis pan antenna mechanisms. The default is to
   * circle clockwise so view is right. The pan servo neutral makes
   * the antenna look to the right with 0� given, 90� is to the back and
   * -90� is to the front.
   *
   *
   *
   *   plane front
   *
   *                  90
                      ^
   *                  I
   *             135  I  45�
   *                \ I /
   *                 \I/
   *        180-------I------- 0�
   *                 /I\
   *                / I \
   *            -135  I  -45�
   *                  I
   *                -90
   *             plane back
   *
   *
   */

  /* fPan =   0  -> antenna looks along the wing
             90  -> antenna looks in flight direction
            -90  -> antenna looks backwards
  */
  /* fixed to the plane*/
  airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy)));

  // I need to avoid oscillations around the 180 degree mark.
  if (airborne_ant_pan > 0 && airborne_ant_pan <= RadOfDeg(175)) { ant_pan_positive = 1; }
  if (airborne_ant_pan < 0 && airborne_ant_pan >= RadOfDeg(-175)) { ant_pan_positive = 0; }

  if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0) {
    airborne_ant_pan = RadOfDeg(-180);

  } else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive) {
    airborne_ant_pan = RadOfDeg(180);
    ant_pan_positive = 0;
  }

#ifdef ANT_PAN_NEUTRAL
  airborne_ant_pan = airborne_ant_pan - RadOfDeg(ANT_PAN_NEUTRAL);
  if (airborne_ant_pan > 0) {
    airborne_ant_pan_servo = MAX_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MAX - ANT_PAN_NEUTRAL)));
  } else {
    airborne_ant_pan_servo = MIN_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MIN - ANT_PAN_NEUTRAL)));
  }
#endif

  airborne_ant_pan_servo = TRIM_PPRZ(airborne_ant_pan_servo);

#ifdef COMMAND_ANT_PAN
  ap_state->commands[COMMAND_ANT_PAN] = airborne_ant_pan_servo;
#endif


  return;
}
Example #13
0
/* conflicts detection and monitoring */
void tcas_periodic_task_1Hz( void ) {
  // no TCAS under security_height
  if (stateGetPositionEnu_f()->z < GROUND_ALT + SECURITY_HEIGHT) {
    uint8_t i;
    for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM;
    return;
  }
  // test possible conflicts
  float tau_min = tcas_tau_ta;
  uint8_t ac_id_close = AC_ID;
  uint8_t i;
  float vx = (*stateGetHorizontalSpeedNorm_f()) * sinf((*stateGetHorizontalSpeedDir_f()));
  float vy = (*stateGetHorizontalSpeedNorm_f()) * cosf((*stateGetHorizontalSpeedDir_f()));
  for (i = 2; i < NB_ACS; i++) {
    if (the_acs[i].ac_id == 0) continue; // no AC data
    uint32_t dt = gps.tow - the_acs[i].itow;
    if (dt > 3*TCAS_DT_MAX) {
      tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
      continue;
    }
    if (dt > TCAS_DT_MAX) continue; // lost com but keep current status
    float dx = the_acs[i].east - stateGetPositionEnu_f()->x;
    float dy = the_acs[i].north - stateGetPositionEnu_f()->y;
    float dz = the_acs[i].alt - stateGetPositionEnu_f()->z;
    float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course);
    float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course);
    float dvz = stateGetSpeedEnu_f()->z - the_acs[i].climb;
    float scal = dvx*dx + dvy*dy + dvz*dz;
    float ddh = dx*dx + dy*dy;
    float ddv = dz*dz;
    float tau = TCAS_HUGE_TAU;
    if (scal > 0.) tau = (ddh + ddv) / scal;
    // monitor conflicts
    uint8_t inside = TCAS_IsInside();
    //enum tcas_resolve test_dir = RA_NONE;
    switch (tcas_acs_status[i].status) {
      case TCAS_RA:
        if (tau >= TCAS_HUGE_TAU && !inside) {
          tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
          tcas_acs_status[i].resolve = RA_NONE;
          DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
        }
        break;
      case TCAS_TA:
        if (tau < tcas_tau_ra || inside) {
          tcas_acs_status[i].status = TCAS_RA; // TA -> RA
          // Downlink alert
          //test_dir = tcas_test_direction(the_acs[i].ac_id);
          //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
          break;
        }
        if (tau > tcas_tau_ta && !inside)
          tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
          tcas_acs_status[i].resolve = RA_NONE;
          DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
        break;
      case TCAS_NO_ALARM:
        if (tau < tcas_tau_ta || inside) {
          tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
          // Downlink warning
          DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
        }
        if (tau < tcas_tau_ra || inside) {
          tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ?
          // Downlink alert
          //test_dir = tcas_test_direction(the_acs[i].ac_id);
          //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);
        }
        break;
    }
    // store closest AC
    if (tau < tau_min) {
      tau_min = tau;
      ac_id_close = the_acs[i].ac_id;

    }
  }
  // set current conflict mode
  if (tcas_status == TCAS_RA && tcas_ac_RA != AC_ID && tcas_acs_status[the_acs_id[tcas_ac_RA]].status == TCAS_RA) {
    ac_id_close = tcas_ac_RA; // keep RA until resolved
  }
  tcas_status = tcas_acs_status[the_acs_id[ac_id_close]].status;
  // at least one in conflict, deal with closest one
  if (tcas_status == TCAS_RA) {
    tcas_ac_RA = ac_id_close;
    tcas_resolve = tcas_test_direction(tcas_ac_RA);
    uint8_t ac_resolve = tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve;
    if (ac_resolve != RA_NONE) { // first resolution, no message received
      if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
        if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
        else tcas_resolve = RA_CLIMB;
      }
      tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming level flight for now
    }
    else { // second resolution or message received
      if (ac_resolve != RA_LEVEL) { // message received
        if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
          if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
          else tcas_resolve = RA_CLIMB;
        }
      }
      else { // no message
        if (tcas_resolve == RA_CLIMB && the_acs[the_acs_id[tcas_ac_RA]].climb > 1.0) tcas_resolve = RA_DESCEND; // revert resolve
        else if (tcas_resolve == RA_DESCEND && the_acs[the_acs_id[tcas_ac_RA]].climb < -1.0) tcas_resolve = RA_CLIMB; // revert resolve
      }
    }
    // Downlink alert
    DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&tcas_ac_RA,&tcas_resolve);
  }
  else tcas_ac_RA = AC_ID; // no conflict
#ifdef TCAS_DEBUG
  if (tcas_status == TCAS_RA) DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel, DefaultDevice,&ac_id_close,&tau_min);
#endif
}
Example #14
0
/* D is the current position */
bool snav_init(uint8_t a, float desired_course_rad, float radius)
{
  wp_a = a;
  radius = fabs(radius);

  float da_x = WaypointX(wp_a) - stateGetPositionEnu_f()->x;
  float da_y = WaypointY(wp_a) - stateGetPositionEnu_f()->y;

  /* D_CD orthogonal to current course, CD on the side of A */
  float u_x = cos(M_PI_2 - stateGetHorizontalSpeedDir_f());
  float u_y = sin(M_PI_2 - stateGetHorizontalSpeedDir_f());
  d_radius = - Sign(u_x * da_y - u_y * da_x) * radius;
  wp_cd.x = stateGetPositionEnu_f()->x + d_radius * u_y;
  wp_cd.y = stateGetPositionEnu_f()->y - d_radius * u_x;
  wp_cd.a = WaypointAlt(wp_a);

  /* A_CA orthogonal to desired course, CA on the side of D */
  float desired_u_x = cos(M_PI_2 - desired_course_rad);
  float desired_u_y = sin(M_PI_2 - desired_course_rad);
  a_radius = Sign(desired_u_x * da_y - desired_u_y * da_x) * radius;
  u_a_ca_x = desired_u_y;
  u_a_ca_y = - desired_u_x;
  wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
  wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
  wp_ca.a = WaypointAlt(wp_a);

  /* Unit vector along CD-CA */
  u_x = wp_ca.x - wp_cd.x;
  u_y = wp_ca.y - wp_cd.y;
  float cd_ca = sqrt(u_x * u_x + u_y * u_y);

  /* If it is too close in reverse direction, set CA on the other side */
  if (a_radius * d_radius < 0 && cd_ca < 2 * radius) {
    a_radius = -a_radius;
    wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
    wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
    u_x = wp_ca.x - wp_cd.x;
    u_y = wp_ca.y - wp_cd.y;
    cd_ca = sqrt(u_x * u_x + u_y * u_y);
  }

  u_x /= cd_ca;
  u_y /= cd_ca;

  if (a_radius * d_radius > 0) {
    /* Both arcs are in the same direction */
    /* CD_TD orthogonal to CD_CA */
    wp_td.x = wp_cd.x - d_radius * u_y;
    wp_td.y = wp_cd.y + d_radius * u_x;

    /* CA_TA also orthogonal to CD_CA */
    wp_ta.x = wp_ca.x - a_radius * u_y;
    wp_ta.y = wp_ca.y + a_radius * u_x;
  } else {
    /* Arcs are in reverse directions: trigonemetric puzzle :-) */
    float alpha = atan2(u_y, u_x) + acos(d_radius / (cd_ca / 2));
    wp_td.x = wp_cd.x + d_radius * cos(alpha);
    wp_td.y = wp_cd.y + d_radius * sin(alpha);

    wp_ta.x = wp_ca.x + a_radius * cos(alpha);
    wp_ta.y = wp_ca.y + a_radius * sin(alpha);
  }
  qdr_td = M_PI_2 - atan2(wp_td.y - wp_cd.y, wp_td.x - wp_cd.x);
  qdr_a = M_PI_2 - atan2(WaypointY(wp_a) - wp_ca.y, WaypointX(wp_a) - wp_ca.x);
  wp_td.a = wp_cd.a;
  wp_ta.a = wp_ca.a;
  ground_speed_timer = 0;

  return false;
}
Example #15
0
// GENERIC TRAJECTORY CONTROLLER
void gvf_control_2D(float ke, float kn, float e,
                    struct gvf_grad *grad, struct gvf_Hess *hess)
{
  struct FloatEulers *att = stateGetNedToBodyEulers_f();
  float ground_speed = stateGetHorizontalSpeedNorm_f();
  float course = stateGetHorizontalSpeedDir_f();
  float px_dot = ground_speed * sinf(course);
  float py_dot = ground_speed * cosf(course);
  int s = gvf_control.s;

  // gradient Phi
  float nx = grad->nx;
  float ny = grad->ny;

  // tangent to Phi
  float tx = s * grad->ny;
  float ty = -s * grad->nx;

  // Hessian
  float H11 = hess->H11;
  float H12 = hess->H12;
  float H21 = hess->H21;
  float H22 = hess->H22;

  // Calculation of the desired angular velocity in the vector field
  float pdx_dot = tx - ke * e * nx;
  float pdy_dot = ty - ke * e * ny;

  float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
  float md_x = pdx_dot / norm_pd_dot;
  float md_y = pdy_dot / norm_pd_dot;

  float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
  float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;

  float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
                        + ((-ke * e * H12) + s * H22) * py_dot;
  float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
                        - (s * H12 + (ke * e * H22)) * py_dot;

  float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
  float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;

  float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
                       / norm_pd_dot;

  float md_dot_x =  md_y * md_dot_const;
  float md_dot_y = -md_x * md_dot_const;

  float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);

  float mr_x = sinf(course);
  float mr_y = cosf(course);

  float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);

  // Coordinated turn
  if (autopilot_get_mode() == AP_MODE_AUTO2) {
    h_ctl_roll_setpoint =
      -atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
    BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);

    lateral_mode = LATERAL_MODE_ROLL;
  }
}
Example #16
0
int potential_task(void)
{

  uint8_t i;

  float ch = cosf(stateGetHorizontalSpeedDir_f());
  float sh = sinf(stateGetHorizontalSpeedDir_f());
  potential_force.east = 0.;
  potential_force.north = 0.;
  potential_force.alt = 0.;

  // compute control forces
  int8_t nb = 0;
  for (i = 0; i < NB_ACS; ++i) {
    if (the_acs[i].ac_id == AC_ID) { continue; }
    struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
    float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
    // if AC not responding for too long, continue, else compute force
    if (delta_t > CARROT) { continue; }
    else {
      float sha = sinf(ac->course);
      float cha = cosf(ac->course);
      float de = ac->east  + sha * delta_t - stateGetPositionEnu_f()->x;
      if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) { continue; }
      float dn = ac->north + cha * delta_t - stateGetPositionEnu_f()->y;
      if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) { continue; }
      float da = ac->alt + ac->climb * delta_t - stateGetPositionUtm_f()->alt;
      if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) { continue; }
      float dist = sqrtf(de * de + dn * dn + da * da);
      if (dist == 0.) { continue; }
      float dve = stateGetHorizontalSpeedNorm_f() * sh - ac->gspeed * sha;
      float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac->gspeed * cha;
      float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb;
      float scal = dve * de + dvn * dn + dva * da;
      if (scal < 0.) { continue; } // No risk of collision
      float d3 = dist * dist * dist;
      potential_force.east += scal * de / d3;
      potential_force.north += scal * dn / d3;
      potential_force.alt += scal * da / d3;
      ++nb;
    }
  }
  if (nb == 0) { return true; }
  //potential_force.east /= nb;
  //potential_force.north /= nb;
  //potential_force.alt /= nb;

  // set commands
  NavVerticalAutoThrottleMode(0.);

  // carrot
  float dx = -force_pos_gain * potential_force.east;
  float dy = -force_pos_gain * potential_force.north;
  desired_x += dx;
  desired_y += dy;
  // fly to desired
  fly_to_xy(desired_x, desired_y);

  // speed loop
  float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
  cruise += -force_speed_gain * (potential_force.north * ch + potential_force.east * sh);
  Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
  potential_force.speed = cruise;
  v_ctl_auto_throttle_cruise_throttle = cruise;

  // climb loop
  potential_force.climb = -force_climb_gain * potential_force.alt;
  BoundAbs(potential_force.climb, V_CTL_ALTITUDE_MAX_CLIMB);
  NavVerticalClimbMode(potential_force.climb);

  DOWNLINK_SEND_POTENTIAL(DefaultChannel, DefaultDevice, &potential_force.east, &potential_force.north,
                          &potential_force.alt, &potential_force.speed, &potential_force.climb);

  return true;
}