Example #1
0
/* Called by clicking a button */
void init_river_tracking( GtkWidget *widget, gpointer data )
{
    fprintf(stderr, "initializing river tracking...\n");
    /* Call function get_next_waypoint (defined in nexcwp.c)
     * which returns a waypoint. */
    Waypoint new_wp = get_next_waypoint();
    IvySendMsg("gcs MOVE_WAYPOINT %d %d %f %f %f", \
            new_wp.ac_id, 4, new_wp.lat, new_wp.lon, new_wp.alt);
}
Example #2
0
/* Called when aircraft reaches "Block 1" (the second
 * block defined in the flight plan)
 */
void start_track(IvyClientPtr app, void *data, int argc, char **argv)
{
    fprintf(stderr, ".");
    /* Call function get_next_waypoint (defined in nexcwp.c)
     * which returns a waypoint. */
    Waypoint new_wp = get_next_waypoint();

    if(CONTINUE) {
        IvySendMsg("gcs MOVE_WAYPOINT %d %d %f %f %f", \
                new_wp.ac_id, 4, new_wp.lat, new_wp.lon, new_wp.alt); //Always move waypoint 4
    }
    /* else we've reached the end (we've seen the "end" waypoint in the
     * picture), so don't send any message.
     */
}
Example #3
0
static void update_all_nav(void) {
  get_next_waypoint();

  // Check if a new GPS coordinate was received and update the position
  if (statevars.status & STATUS_GPS_FIX_AVAIL) {
    // Calculate a new gps-based heading using the previous coord (current)
    // and the newest coord (statevars)
    gps_hdg_most_recent = calc_true_bearing(current_lat, current_long, statevars.gps_lat_ddeg, statevars.gps_long_ddeg);
    current_lat = statevars.gps_lat_ddeg;
    current_long = statevars.gps_long_ddeg;
  }

  // Check if a new GPS heading and speed were received and update
  if (statevars.status & STATUS_GPS_GPRMC_RCVD) {
    // Not using the gps heading because sometimes it's horrendous. Instead,
    // we'll calculate our own using calc_true_bearing()
    // gps_hdg_most_recent = statevars.gps_ground_course_deg;
    gps_speed_most_recent = statevars.gps_ground_speed_kt * METERS_PER_SECOND_PER_KNOT;
  }

  // Calculate the number of ticks that occurred during the current iteration
  // Since the tick count is cumulative, the new tick count will always be
  // greater-than or equal to the previous tick count
  uint32_t new_tick_count = statevars.odometer_ticks;
  uint32_t tick_diff = new_tick_count - prev_tick_count;

  current_speed = calc_speed_mps(tick_diff);

  // TODO I know; we're doing another tick_diff / TICKS_PER_METER calculation.
  // But we'll optimize this later
  float distance_since_prev_iter_m = tick_diff / TICKS_PER_METER;

  // We're using calc_speed_mps() instead to allow integer-based distance diff eval
  // current_speed = calc_speed(distance_since_prev_iter_m);

  // Advance the tick count now that we're done with the previous value
  prev_tick_count = new_tick_count;

  if (current_speed == 0.0) {
    current_speed = gps_speed_most_recent;
  }

  nav_heading_deg = calc_nav_heading();

  float old_lat = current_lat;
  float old_long = current_long;
  calc_position(&current_lat, &current_long, old_lat, old_long, distance_since_prev_iter_m, nav_heading_deg);

  float waypt_true_bearing = calc_true_bearing(current_lat, current_long, waypoint_lat, waypoint_long);
  rel_bearing_deg = calc_relative_bearing(waypt_true_bearing, nav_heading_deg);

  distance_to_waypoint_m = calc_dist_to_waypoint(current_lat, current_long, waypoint_lat, waypoint_long);

  statevars.nav_heading_deg = nav_heading_deg;
  statevars.nav_latitude = current_lat;
  statevars.nav_longitude = current_long;
  statevars.nav_waypt_latitude = waypoint_lat;
  statevars.nav_waypt_longitude = waypoint_long;
  statevars.nav_rel_bearing_deg = rel_bearing_deg;
  statevars.nav_distance_to_waypt_m = distance_to_waypoint_m;
  statevars.nav_speed = current_speed;

  return;
}