[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4328] Added /sw/in_progress/ antenna_track folder a
From: |
Mark Griffin |
Subject: |
[paparazzi-commits] [4328] Added /sw/in_progress/ antenna_track folder and source for a Pololu servo based antenna tracker. |
Date: |
Thu, 12 Nov 2009 21:09:32 +0000 |
Revision: 4328
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4328
Author: markgriffin
Date: 2009-11-12 21:09:32 +0000 (Thu, 12 Nov 2009)
Log Message:
-----------
Added /sw/in_progress/antenna_track folder and source for a Pololu servo based
antenna tracker.
Added Paths:
-----------
paparazzi3/trunk/sw/in_progress/antenna_track/
paparazzi3/trunk/sw/in_progress/antenna_track/ant_track_Pololu.c
Added: paparazzi3/trunk/sw/in_progress/antenna_track/ant_track_Pololu.c
===================================================================
--- paparazzi3/trunk/sw/in_progress/antenna_track/ant_track_Pololu.c
(rev 0)
+++ paparazzi3/trunk/sw/in_progress/antenna_track/ant_track_Pololu.c
2009-11-12 21:09:32 UTC (rev 4328)
@@ -0,0 +1,344 @@
+/*
+ * Paparazzi $Id: ant_track.c 2009-10-30 12:14:20Z griffin $
+ *
+ * Copyright (C) 2009 - Pascal Brisset, Antoine Drouin
+ *
+ * Modified by: Mark Griffin and Todd Sandercock
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+#include <gtk/gtk.h>
+#include <stdio.h>
+#include <math.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <Ivy/ivy.h>
+#include <Ivy/ivyglibloop.h>
+
+#include <termios.h>
+#include <unistd.h> /* UNIX standard function definitions */
+#include <fcntl.h> /* File control definitions */
+#include <errno.h> /* Error number definitions */
+
+#define MANUAL 0
+#define AUTO 1
+
+static double gps_pos_x;
+static double gps_pos_y;
+static double gps_alt;
+static double home_alt;
+static double ant_azim;
+static double ant_elev;
+static int mode;
+static int home_found;
+
+int fd; /* File descriptor for the port */
+
+double hfov = 170., vfov = 170.;
+double hnp = 270., vnp = 30.;
+double hlim1 = 30, hlim2 = 200;
+double vlim1 = 30, vlim2 = 200;
+
+unsigned char startByte = 0x80, deviceId = 0x01, command = 0x03, commandinit =
+ 0x00, psiServo = 0x01, thetaServo = 0x02, speed = 0x00,
speedCmd = 0x01;
+unsigned char datainit = 0x40;
+unsigned char data1 = 19, data2 = 68;
+
+GtkWidget *azim_scale;
+GtkWidget *elev_scale;
+
+void on_mode_changed(GtkRadioButton *radiobutton, gpointer user_data) {
+ mode
+ =
gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radiobutton)) ? MANUAL
+ : AUTO;
+ //IvySendMsg("1ME RAW_DATALINK 80 SETTING;0;0;%d", mode);
+ //g_message("Mode changed to %d" , mode);
+}
+
+#define GLADE_HOOKUP_OBJECT(component,widget,name) \
+ g_object_set_data_full (G_OBJECT (component), name, \
+ gtk_widget_ref (widget), (GDestroyNotify) gtk_widget_unref)
+
+#define GLADE_HOOKUP_OBJECT_NO_REF(component,widget,name) \
+ g_object_set_data (G_OBJECT (component), name, widget)
+
+GtkWidget* build_gui(void) {
+ GtkWidget *window1;
+ GtkWidget *vbox1;
+ GtkWidget *vbox2;
+ GtkWidget *table1;
+ GtkWidget *label1;
+ GtkWidget *label2;
+ GtkWidget *label3;
+ GtkWidget *label4;
+ GtkWidget *radiobutton1;
+ GSList *radiobutton1_group = NULL;
+ GtkWidget *radiobutton2;
+ GtkWidget *entry1;
+
+ window1 = gtk_window_new(GTK_WINDOW_TOPLEVEL);
+ gtk_window_set_title(GTK_WINDOW (window1), "tracking antenna");
+
+ vbox1 = gtk_vbox_new(FALSE, 0);
+ gtk_widget_show(vbox1);
+ gtk_container_add(GTK_CONTAINER (window1), vbox1);
+
+ vbox2 = gtk_vbox_new(FALSE, 0);
+ gtk_widget_show(vbox2);
+ gtk_box_pack_start(GTK_BOX (vbox1), vbox2, TRUE, TRUE, 0);
+
+ table1 = gtk_table_new(4, 3, FALSE);
+ gtk_widget_show(table1);
+ gtk_box_pack_start(GTK_BOX (vbox2), table1, TRUE, TRUE, 0);
+ gtk_table_set_col_spacings(GTK_TABLE (table1), 5);
+
+ label1 = gtk_label_new("Azimuth");
+ gtk_widget_show(label1);
+ gtk_table_attach(GTK_TABLE (table1), label1, 0, 1, 1, 2,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0),
0, 0);
+ gtk_misc_set_alignment(GTK_MISC (label1), 0, 0.5);
+
+ label2 = gtk_label_new("Elevation");
+ gtk_widget_show(label2);
+ gtk_table_attach(GTK_TABLE (table1), label2, 0, 1, 2, 3,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0),
0, 0);
+ gtk_misc_set_alignment(GTK_MISC (label2), 0, 0.5);
+
+ label3 = gtk_label_new("Id");
+ gtk_widget_show(label3);
+ gtk_table_attach(GTK_TABLE (table1), label3, 0, 1, 3, 4,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0),
0, 0);
+ gtk_misc_set_alignment(GTK_MISC (label3), 0, 0.5);
+
+ label4 = gtk_label_new("mode");
+ gtk_widget_show(label4);
+ gtk_table_attach(GTK_TABLE (table1), label4, 0, 1, 0, 1,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0),
0, 0);
+ gtk_misc_set_alignment(GTK_MISC (label4), 0, 0.5);
+
+ radiobutton1 = gtk_radio_button_new_with_mnemonic(NULL, "manual");
+ gtk_widget_show(radiobutton1);
+ gtk_table_attach(GTK_TABLE (table1), radiobutton1, 1, 2, 0, 1,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0),
0, 0);
+ gtk_radio_button_set_group(GTK_RADIO_BUTTON (radiobutton1),
+ radiobutton1_group);
+ radiobutton1_group = gtk_radio_button_get_group(
+ GTK_RADIO_BUTTON (radiobutton1));
+
+ radiobutton2 = gtk_radio_button_new_with_mnemonic(NULL, "tracking");
+ gtk_widget_show(radiobutton2);
+ gtk_table_attach(GTK_TABLE (table1), radiobutton2, 2, 3, 0, 1,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0),
0, 0);
+ gtk_radio_button_set_group(GTK_RADIO_BUTTON (radiobutton2),
+ radiobutton1_group);
+ radiobutton1_group = gtk_radio_button_get_group(
+ GTK_RADIO_BUTTON (radiobutton2));
+
+ azim_scale = gtk_hscale_new(
+ GTK_ADJUSTMENT (gtk_adjustment_new (144.7, 0, 360, 1,
1, 1)));
+ gtk_widget_show(azim_scale);
+ gtk_table_attach(GTK_TABLE (table1), azim_scale, 1, 3, 1, 2,
+ (GtkAttachOptions) (GTK_EXPAND | GTK_FILL),
+ (GtkAttachOptions) (GTK_FILL), 0, 0);
+ gtk_range_set_update_policy(GTK_RANGE (azim_scale), GTK_UPDATE_DELAYED);
+
+ elev_scale = gtk_hscale_new(
+ GTK_ADJUSTMENT (gtk_adjustment_new (32.3, 0, 90, 1, 1,
1)));
+ gtk_widget_show(elev_scale);
+ gtk_table_attach(GTK_TABLE (table1), elev_scale, 1, 3, 2, 3,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions)
(GTK_FILL), 0, 0);
+
+ entry1 = gtk_entry_new();
+ gtk_widget_show(entry1);
+ gtk_table_attach(GTK_TABLE (table1), entry1, 1, 3, 3, 4,
+ (GtkAttachOptions) (GTK_EXPAND | GTK_FILL),
(GtkAttachOptions) (0),
+ 0, 0);
+
+ g_signal_connect ((gpointer) radiobutton1, "toggled",
+ G_CALLBACK (on_mode_changed),
+ NULL);
+
+ /* Store pointers to all widgets, for use by lookup_widget(). */
+ GLADE_HOOKUP_OBJECT_NO_REF (window1, window1, "window1");
+ GLADE_HOOKUP_OBJECT (window1, vbox1, "vbox1");
+ GLADE_HOOKUP_OBJECT (window1, vbox2, "vbox2");
+ GLADE_HOOKUP_OBJECT (window1, table1, "table1");
+ GLADE_HOOKUP_OBJECT (window1, label1, "label1");
+ GLADE_HOOKUP_OBJECT (window1, label2, "label2");
+ GLADE_HOOKUP_OBJECT (window1, label3, "label3");
+ GLADE_HOOKUP_OBJECT (window1, label4, "label4");
+ GLADE_HOOKUP_OBJECT (window1, radiobutton1, "radiobutton1");
+ GLADE_HOOKUP_OBJECT (window1, radiobutton2, "radiobutton2");
+ GLADE_HOOKUP_OBJECT (window1, entry1, "entry1");
+
+ return window1;
+}
+
+/* jump here when a GPS message is received */
+void on_GPS_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]) {
+ if (home_found == 0) {
+ if (atof(argv[0]) == 3) { /* wait until we have a valid GPS fix
*/
+ home_alt = atof(argv[4]) / 100.; /* get the altitude */
+ home_found = 1;
+ }
+ }
+ gps_alt = atof(argv[4]) / 100.;
+}
+
+void send_pos(int vert, int hori) {
+ // Split the value into two 7 bit numbers
+ data1 = vert / 128;
+ data2 = vert % 128;
+
+ // Command buffer for "Set Position, 8-bit (2 data bytes)" from Pololu
servo controller data sheet
+ // Send vertical servo
+ char buffer1[] = { startByte, deviceId, command, psiServo, data1, data2
};
+
+ write(fd, buffer1, 6);
+
+ // Split the value into two 7 bit numbers
+ data1 = hori / 128;
+ data2 = hori % 128;
+
+ // Command buffer for "Set Position, 8-bit (2 data bytes)" from Pololu
servo controller data sheet
+ // Send horizontal servo
+ char buffer2[] = { startByte, deviceId, command, thetaServo, data1,
data2 };
+
+ write(fd, buffer2, 6);
+}
+
+/* jump here when a NAVIGATION message is received */
+void on_NAV_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]) {
+ double hpos, vpos;
+ double hservo, vservo;
+
+ if (mode == AUTO) {
+ gps_pos_x = atof(argv[2]);
+ gps_pos_y = atof(argv[3]);
+ /* calculate azimuth */
+ ant_azim = atan2(gps_pos_x, gps_pos_y) * 180. / M_PI;
+
+ if (ant_azim < 0)
+ ant_azim += 360.;
+
+ /* calculate elevation */
+ ant_elev = atan2((gps_alt - home_alt), sqrt(atof(argv[5]))) *
180.
+ / M_PI;
+ if (ant_elev < 0)
+ ant_elev = 0.;
+
+ gtk_range_set_value(azim_scale, ant_azim);
+ gtk_range_set_value(elev_scale, ant_elev);
+ }
+
+ // The magic is done here
+
+ // First take the horizontal angle relative to the neutral point "hnp"
+ hpos = ant_azim - hnp;
+
+ // Keep the range between (-180,180). this is done so that it
consistently swaps sides
+ if (hpos < -180) {
+ hpos += 360;
+ } else if (hpos > 180) {
+ hpos -= 360;
+ }
+
+ // keep the range within the field of view "hfov"
+ if (hpos > (hfov / 2)) {
+ hpos = hfov / 2;
+ } else if (-hpos > (hfov / 2)) {
+ hpos = -hfov / 2;
+ }
+
+ // Take the vertical angle relative to the neutral point "vnp"
+ vpos = ant_elev - vnp;
+
+ // keep within the field of view "vfov"
+ if (vpos > (vfov / 2)) {
+ vpos = vfov / 2;
+ } else if (-vpos > (vfov / 2)) {
+ vpos = -vfov / 2;
+ }
+
+ // make outputs relative to limits for the Pololu board
+
+ vservo = (((vpos + (vfov / 2)) / vfov) * (vlim2 - vlim1)) + vlim1;
+ hservo = (((hpos + (hfov / 2)) / hfov) * (hlim2 - hlim1)) + hlim1;
+
+ /*g_message("home_alt %f gps_alt %f azim %f elev %f", home_alt,
gps_alt, ant_azim, ant_elev); */
+
+ // Send servo position.
+ send_pos(vservo, hservo);
+
+ g_message("vservo %f hservo %f", vservo, hservo);
+}
+
+int open_port(void) {
+ struct termios options;
+
+ // would probably be good to set the port up as an arg.
+ fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);
+ if (fd == -1) {
+ perror("open_port: Unable to open /dev/ttyUSB0 - ");
+ } else
+ fcntl(fd, F_SETFL, 0);
+
+ tcgetattr(fd, &options);
+
+ // Set the baud rates to 19200. This can be between 2,000 to 40,000
+
+ cfsetispeed(&options, B19200);
+ cfsetospeed(&options, B19200);
+
+ options.c_cflag |= (CLOCAL | CREAD);
+
+ tcsetattr(fd, TCSANOW, &options);
+
+ // Send initialisation to the pololu board. By changing "speed" the
speed of the servo can be changed
+ // if "speed" is nonzero then 1 is the slowest 127 is the fastest. 0 =
no speed restriction
+ char buffer[] = { startByte, deviceId, speedCmd, psiServo, speed,
+ startByte, deviceId, speedCmd, thetaServo, speed};
+
+ write(fd, buffer, 10);
+
+ return (fd);
+}
+
+int main(int argc, char** argv) {
+
+ gtk_init(&argc, &argv);
+
+ GtkWidget* window = build_gui();
+ gtk_widget_show_all(window);
+
+ open_port();
+
+ IvyInit("AntennaTracker", "AntennaTracker READY", NULL, NULL, NULL,
NULL);
+ IvyBindMsg(
+ on_GPS_STATUS,
+ NULL,
+ "^\\S* GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)
(\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
+ IvyBindMsg(on_NAV_STATUS, NULL,
+ "^\\S* NAVIGATION (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)
(\\S*) (\\S*) (\\S*)");
+ IvyStart("127.255.255.255");
+ gtk_main();
+ return 0;
+}
+
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4328] Added /sw/in_progress/ antenna_track folder and source for a Pololu servo based antenna tracker.,
Mark Griffin <=