/* 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); }
/* 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. */ }
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(¤t_lat, ¤t_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; }