mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 04:45:37 +08:00
obsolete
This commit is contained in:
MarkGriffin
@@ -1,344 +0,0 @@
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user