C++ (Cpp) utm_of_lla_i Examples

Example #1

File: gps.c Project: enacuavlab/paparazzi

struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
{
  struct UtmCoor_f utm = {.east = 0., .north=0., .alt=0., .zone=zone};

  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
    /* A real UTM position is available, use the correct zone */
    UTM_FLOAT_OF_BFP(utm, gps_s->utm_pos);
  } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT))
  {
    /* Recompute UTM coordinates in this zone */
    struct UtmCoor_i utm_i;
    utm_i.zone = zone;
    utm_of_lla_i(&utm_i, &gps_s->lla_pos);
    UTM_FLOAT_OF_BFP(utm, utm_i);

    /* set utm.alt in hsml */
    if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
      utm.alt = gps_s->hmsl/1000.;
    } else {
      utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon)/1000.;
    }
  }

  return utm;
}

struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone)
{
  struct UtmCoor_i utm = {.east = 0, .north=0, .alt=0, .zone=zone};

  if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
    // A real UTM position is available, use the correct zone
    UTM_COPY(utm, gps_s->utm_pos);
  }
  else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)){
    /* Recompute UTM coordinates in zone */
    utm_of_lla_i(&utm, &gps_s->lla_pos);

    /* set utm.alt in hsml */
    if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
      utm.alt = gps_s->hmsl;
    } else {
      utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon);
    }
  }

  return utm;
}

Example #2

File: swarm_potential.c Project: coppolam/paparazzi

int swarm_potential_task(void)
{
  struct EnuCoor_f speed_sp = {.x = 0., .y = 0., .z = 0.};

  // compute desired velocity
  int8_t nb = 0;
  uint8_t i;

  if (gps.fix == 0) {return 1;}
  struct UtmCoor_i my_pos;
  my_pos.zone = 0;
  utm_of_lla_i(&my_pos, &gps.lla_pos);    // TODO update height to hmsl

  for (i = 0; i < ti_acs_idx; i++) {
    if (ti_acs[i].ac_id == 0 || ti_acs[i].ac_id == AC_ID) { continue; }
    struct ac_info_ * ac = get_ac_info(ti_acs[i].ac_id);
    //float delta_t = ABS((int32_t)(gps.tow - ac->itow) / 1000.);
    // if AC not responding for too long, continue, else compute force
    //if(delta_t > 5) { continue; }

    float de = (ac->utm.east - my_pos.east) / 100.; // + sha * delta_t
    float dn = (ac->utm.north - my_pos.north) / 100.; // cha * delta_t
    float da = (ac->utm.alt - my_pos.alt) / 1000.; // ac->climb * delta_t   // currently wrong reference in other aircraft
    float dist2 = de * de + dn * dn;// + da * da;
    if (dist2 == 0.) {continue;}

    float dist = sqrtf(dist2);

    // potential force equation: x^2 - d0^3/x
    float force = dist2 - TARGET_DIST3 / dist;

    potential_force.east = (de * force) / dist;
    potential_force.north = (dn * force) / dist;
    potential_force.alt = (da * force) / dist;

    // set carrot
    // include speed of other vehicles for swarm cohesion
    speed_sp.x += force_hor_gain * potential_force.east;// + ac->gspeed * sinf(ac->course);
    speed_sp.y += force_hor_gain * potential_force.north;// + ac->gspeed * cosf(ac->course);
    speed_sp.z += force_climb_gain * potential_force.alt;// + ac->climb;

    nb++;

    //debug
    //potential_force.east  = de;
    //potential_force.north = dn;
    //potential_force.alt   = da;
  }

  // add waypoint force to get vehicle to waypoint
  if (use_waypoint) {
    struct EnuCoor_f my_enu = *stateGetPositionEnu_f();

    float de = waypoint_get_x(SP_WP) - my_enu.x;
    float dn = waypoint_get_y(SP_WP) - my_enu.y;
    float da = waypoint_get_alt(SP_WP) - my_enu.z;

    float dist2 = de * de + dn * dn;// + da * da;
    if (dist2 > 0.01) {   // add deadzone of 10cm from goal
      float dist = sqrtf(dist2);
      float force = 2 * dist2;

      // higher attractive potential to get to goal when close by
      if (dist < 1.) {
        force = 2 * dist;
      }

      potential_force.east  = force * de / dist;
      potential_force.north = force * dn / dist;
      potential_force.alt   = force * da / dist;

      speed_sp.x += force_hor_gain * potential_force.east;
      speed_sp.y += force_hor_gain * potential_force.north;
      speed_sp.z += force_climb_gain * potential_force.alt;

      potential_force.east  = de;
      potential_force.north = dn;
      potential_force.alt   = force;
      potential_force.speed   = speed_sp.x;
      potential_force.climb   = speed_sp.y;
    }
  }

  //potential_force.speed = speed_sp.x;
  //potential_force.climb = speed_sp.y;

  // limit commands
#ifdef GUIDANCE_H_REF_MAX_SPEED
  BoundAbs(speed_sp.x, GUIDANCE_H_REF_MAX_SPEED);
  BoundAbs(speed_sp.y, GUIDANCE_H_REF_MAX_SPEED);
  BoundAbs(speed_sp.z, GUIDANCE_H_REF_MAX_SPEED);
#endif

  potential_force.east  = speed_sp.x;
  potential_force.north = speed_sp.y;
  potential_force.alt   = speed_sp.z;

  autopilot_guided_move_ned(speed_sp.y, speed_sp.x, 0, 0);    // speed in enu

  return 1;
}