diff --git a/sw/in_progress/river_tracking/river_tracking-Makefile.mk b/sw/in_progress/river_tracking/river_tracking-Makefile.mk index 4d6650c152..22e2d3a104 100644 --- a/sw/in_progress/river_tracking/river_tracking-Makefile.mk +++ b/sw/in_progress/river_tracking/river_tracking-Makefile.mk @@ -19,6 +19,6 @@ main : src/river_track.c $(CC) src/river_track.c -o bin/river_track $(INCLUDES) clean : - rm -f *.o + cd $(RTHOME) rm -f *.o cd $(RTSOURCE); rm -f *.o; cd $(RTBIN); rm -f river_track; diff --git a/sw/in_progress/river_tracking/src/nextwp.c b/sw/in_progress/river_tracking/src/nextwp.c index 4d6f057377..2b7ffabdb9 100644 --- a/sw/in_progress/river_tracking/src/nextwp.c +++ b/sw/in_progress/river_tracking/src/nextwp.c @@ -4,16 +4,17 @@ next waypoint. #include "waypoint.h" +extern float ADD; Waypoint get_next_waypoint() { Waypoint next_wp; /* Insert all sorts of opencv magic here */ - next_wp.ac_id = 1; next_wp.wp = 3; next_wp.alt = 1420.; - next_wp.lat = 41.820258; - next_wp.lon = -111.98964; + next_wp.lat = 41.823364 + ADD; + next_wp.lon = -111.988800 + ADD; + ADD+=.00004; return next_wp; } diff --git a/sw/in_progress/river_tracking/src/river_track.c b/sw/in_progress/river_tracking/src/river_track.c index 46f8e752c1..36744b5d97 100644 --- a/sw/in_progress/river_tracking/src/river_track.c +++ b/sw/in_progress/river_tracking/src/river_track.c @@ -5,15 +5,25 @@ #include #include +#include #include #include #include -//#include is what the previous line should read when compiling -//with pkg-config --cflags --libs gtk+-2.0 +/*#include is what the previous line should read when compiling +with pkg-config --cflags --libs gtk+-2.0 */ #include "nextwp.c" #include "waypoint.h" + +/* Some global variables */ +float END_LAT = 0.0; //latitude of the "end" waypoint (the fifth waypoint defined in flight plan) +float END_LON = 0.0; //longitude of the "end" waypoint (the fifth waypoint defined in flight plan) +int CONTINUE = 1; //When this variable is set to 0 river_track will stop moving the waypoint around +float ADD = 0; //just to move the waypoint around for fun + + + /* callback associated to "Hello" messages */ void textCallback(IvyClientPtr app, void *data, int argc, char **argv) { @@ -22,14 +32,53 @@ void textCallback(IvyClientPtr app, void *data, int argc, char **argv) } -void start_track( GtkWidget *widget, gpointer data ) +/* Sets the global variables END_LAT & END_LON to the GPS coordinates + * of the end waypoint (the fifth waypoint defined in the flight plan). + */ +void set_end(IvyClientPtr app, void *data, int argc, char **argv) { - fprintf(stderr, "starting river tracking...\n"); + const char *arg = (argc < 1) ? "" : argv[0]; + + char *current; + current = strtok(arg, " "); + current = strtok(NULL, " "); + current = strtok(NULL, " "); + current = strtok(NULL, " "); + END_LAT = atof(current); + current = strtok(NULL, " "); + END_LON = atof(current); +// fprintf(stderr, "END_LAT=%f. END_LON=%f.", END_LAT, END_LON); +} + +/* 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, new_wp.wp, new_wp.lat, new_wp.lon, new_wp.alt); + 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. + */ } @@ -40,13 +89,13 @@ static void destroy( GtkWidget *widget, gpointer data ) } -int main( int argc, char *argv[] ) { +int main( int argc, char *argv[] ) +{ GtkWidget *window; GtkWidget *button; GtkWidget *box1; char *bus=getenv("IVYBUS"); - char *tosend="foo"; gtk_init(&argc, &argv); window = gtk_window_new(GTK_WINDOW_TOPLEVEL); @@ -57,14 +106,14 @@ int main( int argc, char *argv[] ) { gtk_container_add (GTK_CONTAINER (window), box1); //first button... - button = gtk_button_new_with_label("start river tracking"); - g_signal_connect(G_OBJECT(button), "clicked", G_CALLBACK(start_track), &tosend); + button = gtk_button_new_with_label("(Re)define region of interest"); + g_signal_connect(G_OBJECT(button), "clicked", G_CALLBACK(init_river_tracking), 0); gtk_box_pack_start(GTK_BOX(box1), button, TRUE, TRUE, 0); gtk_widget_show (button); //second button... button = gtk_button_new_with_label("Quit"); - g_signal_connect(G_OBJECT(button), "clicked", G_CALLBACK(destroy), &tosend); + g_signal_connect(G_OBJECT(button), "clicked", G_CALLBACK(destroy), 0); gtk_box_pack_start(GTK_BOX (box1), button, TRUE, TRUE, 0); gtk_widget_show (button); @@ -72,7 +121,9 @@ int main( int argc, char *argv[] ) { gtk_widget_show(window); IvyInit("river_track", "river_track READY", NULL, NULL, NULL, NULL); - IvyBindMsg(textCallback,&tosend,"^river_track hello(.*)"); + IvyBindMsg(textCallback,0,"^river_track hello(.*)"); + IvyBindMsg(start_track,0,"(NAV_STATUS 1 1 +.*)"); + IvyBindMsg(set_end,0,"(WAYPOINT_MOVED 1 5 +.*)"); IvyStart(bus); gtk_main();