diff --git a/.gitignore b/.gitignore index 08da500ec8..d0ac9401fc 100644 --- a/.gitignore +++ b/.gitignore @@ -82,7 +82,7 @@ /sw/ground_segment/tmtc/ivy2udp /sw/ground_segment/tmtc/server /sw/ground_segment/tmtc/diadec -/sw/ground_segment/misc/ivy2serial +/sw/ground_segment/tmtc/ivy_serial_bridge /sw/ground_segment/tmtc/GSM/SMS_GS # /sw/ground_segment/joystick diff --git a/conf/modules/mcp355x.xml b/conf/modules/mcp355x.xml new file mode 100644 index 0000000000..f68b84c6fa --- /dev/null +++ b/conf/modules/mcp355x.xml @@ -0,0 +1,17 @@ + + + +
+ +
+ + + + + + + + + + + diff --git a/conf/telemetry/default_fixedwing_imu_9k6.xml b/conf/telemetry/default_fixedwing_imu_9k6.xml index 02087f6deb..aa1442c0b8 100644 --- a/conf/telemetry/default_fixedwing_imu_9k6.xml +++ b/conf/telemetry/default_fixedwing_imu_9k6.xml @@ -3,19 +3,19 @@ - + - - + + - + - - + + @@ -23,14 +23,14 @@ - + - + - + diff --git a/sw/airborne/modules/adcs/mcp355x.h b/sw/airborne/modules/adcs/mcp355x.h new file mode 100644 index 0000000000..aef4b3e25a --- /dev/null +++ b/sw/airborne/modules/adcs/mcp355x.h @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * + * 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. + * + */ + +/* driver for MCP3550/1/3 (Module wrapper) + */ + +#ifndef MCP355X_MODULE_H +#define MCP355X_MODULE_H + +#include "peripherals/mcp355x.h" + +#endif diff --git a/sw/airborne/peripherals/mcp355x.c b/sw/airborne/peripherals/mcp355x.c new file mode 100644 index 0000000000..386f1aae59 --- /dev/null +++ b/sw/airborne/peripherals/mcp355x.c @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * + * 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. + * + */ + +/* driver for MCP3550/1/3 + */ + +#include "peripherals/mcp355x.h" +#include "mcu_periph/spi.h" + +bool_t mcp355x_data_available; +int32_t mcp355x_data; +uint8_t mcp355x_spi_buf[4]; + +void mcp355x_init(void) { + mcp355x_data_available = FALSE; + mcp355x_data = 0; + + SpiClrCPOL(); + SpiClrCPHA(); +} + +void mcp355x_read(void) { + spi_buffer_length = 4; + spi_buffer_input = mcp355x_spi_buf; + SpiSelectSlave0(); + SpiStart(); +} + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +void mcp355x_event(void) { + static uint32_t filtered = 0; + if (spi_message_received) { + spi_message_received = FALSE; + if ((mcp355x_spi_buf[0]>>4) == 0) { + //mcp355x_data = (int32_t)(((uint32_t)mcp355x_spi_buf[0]<<16) | ((uint32_t)mcp355x_spi_buf[1]<<8) | (mcp355x_spi_buf[2])); + mcp355x_data = (int32_t)( + ((uint32_t)mcp355x_spi_buf[0]<<17) | + ((uint32_t)mcp355x_spi_buf[1]<<9) | + ((uint32_t)mcp355x_spi_buf[2]<<1) | + (mcp355x_spi_buf[3]>>7)); + filtered = (5*filtered + mcp355x_data) / (6); + DOWNLINK_SEND_DEBUG(DefaultChannel,4,mcp355x_spi_buf); + DOWNLINK_SEND_BARO_RAW(DefaultChannel,&mcp355x_data,&filtered); + } + } +} + diff --git a/sw/airborne/peripherals/mcp355x.h b/sw/airborne/peripherals/mcp355x.h new file mode 100644 index 0000000000..cbc94e021f --- /dev/null +++ b/sw/airborne/peripherals/mcp355x.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * + * 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. + * + */ + +/* driver for MPC3550/1/3 + */ + +#ifndef MCP355X_H +#define MCP355X_H + +#include "std.h" + +extern bool_t mcp355x_data_available; +extern int32_t mcp355x_data; + +extern void mcp355x_init(void); +extern void mcp355x_read(void); +extern void mcp355x_event(void); + +#endif + diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 03ac26bc1b..9018f8cb2a 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -549,7 +549,10 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id let settings_file = Http.file_of_url settings_url in let settings_xml = try - ExtXml.parse_file ~noprovedtd:true settings_file + if String.compare "replay" settings_file <> 0 then + ExtXml.parse_file ~noprovedtd:true settings_file + else + Xml.Element("empty", [], []) with exc -> prerr_endline (Printexc.to_string exc); Xml.Element("empty", [], []) diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile index d2c1539164..35ff00d8c9 100644 --- a/sw/ground_segment/misc/Makefile +++ b/sw/ground_segment/misc/Makefile @@ -7,7 +7,7 @@ else endif -all: davis2ivy kestrel2ivy ivy2serial +all: davis2ivy kestrel2ivy clean: rm *.o davis2ivy kestrel2ivy @@ -18,9 +18,5 @@ davis2ivy: davis2ivy.o kestrel2ivy: kestrel2ivy.o g++ -o kestrel2ivy kestrel2ivy.o $(LIBRARYS) -livy -ivy2serial: ivy2serial.o - g++ -o ivy2serial ivy2serial.o $(LIBRARYS) -livy - - %.o : %.c gcc -c -O2 -Wall -I /opt/local/include/ $< diff --git a/sw/ground_segment/misc/ivy2serial.c b/sw/ground_segment/misc/ivy2serial.c deleted file mode 100644 index 523dc58acb..0000000000 --- a/sw/ground_segment/misc/ivy2serial.c +++ /dev/null @@ -1,382 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -////////////////////////////////////////////////////////////////////////////////// -// SETTINGS -////////////////////////////////////////////////////////////////////////////////// - -// Serial Repeat Rate -long delay = 1000; - -////////////////////////////////////////////////////////////////////////////////// -// local_uav DATA -////////////////////////////////////////////////////////////////////////////////// - - -struct _uav_type_ -{ - // Header - unsigned char header; - - // Data - unsigned char ac_id; - short int phi, theta, psi, speed; - int utm_east,utm_north,utm_z; - unsigned char utm_zone; - unsigned char pprz_mode; - float desired_alt; - unsigned char block; - - // Footer - unsigned char footer; -} -__attribute__((packed)) - -local_uav, remote_uav; - -volatile unsigned char new_ivy_data = 0; -volatile unsigned char new_serial_data = 0; - -////////////////////////////////////////////////////////////////////////////////// -// IVY Reader -////////////////////////////////////////////////////////////////////////////////// - - -static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[]) -{ -/* - - - - - -*/ - - local_uav.phi = (short int) (atof(argv[0]) * 1000.0); - local_uav.psi = (short int) (atof(argv[1]) * 1000.0); - local_uav.theta = (short int) (atof(argv[2]) * 1000.0); - - //printf("ATTITUDE ac=%d phi=%d theta=%d psi=%d ",local_uav.ac_id, local_uav.phi, local_uav.theta, local_uav.psi); -} - -static void on_Desired(IvyClientPtr app, void *user_data, int argc, char *argv[]) -{ -/* - - - - - - - - - -*/ - - local_uav.desired_alt = atof(argv[5]); -} - -static void on_Gps(IvyClientPtr app, void *user_data, int argc, char *argv[]) -{ -/* - - - - - - - - - - - - - -*/ - - local_uav.utm_east = atoi(argv[1]); - local_uav.utm_north = atoi(argv[2]); - local_uav.utm_z = atoi(argv[4]); - local_uav.utm_zone = atoi(argv[9]); - local_uav.speed = atoi(argv[5]); - - //printf("ATTITUDE ac=%d phi=%d theta=%d psi=%d ",local_uav.ac_id, local_uav.phi, local_uav.theta, local_uav.psi); - //printf("GPS ac=%d %d %d %d %d\n",local_uav.ac_id, local_uav.utm_east, local_uav.utm_north, local_uav.utm_z, local_uav.utm_zone); - - new_ivy_data = 1; -} - -////////////////////////////////////////////////////////////////////////////////// -// IVY Writer -////////////////////////////////////////////////////////////////////////////////// - -void send_ivy(void) -{ - float phi,theta,psi,z,zdot; - - if (new_serial_data == 0) - return; - - new_serial_data = 0; - - phi = ((float) remote_uav.phi) / 1000.0f; - theta = ((float) remote_uav.theta) / 1000.0f; - psi = ((float) remote_uav.psi) / 1000.0f; - - IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", remote_uav.ac_id); - - IvySendMsg("%d ATTITUDE %f %f %f\n", remote_uav.ac_id, phi, psi, theta); - -/* - remote_uav.utm_east = local_uav.utm_east; - remote_uav.utm_north = local_uav.utm_north + 5000; - remote_uav.utm_z = local_uav.utm_z + 1000; - remote_uav.utm_zone = local_uav.utm_zone; - remote_uav.speed = local_uav.speed * 4; - remote_uav.psi += 30.; -*/ - -/* - - - - - - - - - - - - - -*/ - - IvySendMsg("%d GPS 3 %d %d 0 %d %d 0 0 0 %d 0\n", remote_uav.ac_id, remote_uav.utm_east, remote_uav.utm_north, remote_uav.utm_z, remote_uav.speed, remote_uav.utm_zone); - -/* - - - - - - - -*/ - - IvySendMsg("%d FBW_STATUS 2 0 1 81 0 \n", remote_uav.ac_id); - - z = ((float)remote_uav.utm_z) / 1000.0f; - zdot = 0.0f; - IvySendMsg("%d ESTIMATOR %f %f \n", remote_uav.ac_id, z, zdot); - -/* - - - - - - - - - -*/ - IvySendMsg("%d DESIRED 0 0 0 0 0 %f 0 \n", remote_uav.ac_id, remote_uav.desired_alt); - - printf("IVY %d\n",remote_uav.ac_id); -} - -////////////////////////////////////////////////////////////////////////////////// -// SERIAL PORT -////////////////////////////////////////////////////////////////////////////////// - -// pointer -int fd; - -/// Open -void open_port(const char* device) { - fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY); - if (fd == -1) { - fprintf(stderr, "open_port: unable to open device %s - ", device); - perror(NULL); - exit(EXIT_FAILURE); - } - // setup connection options - struct termios options; - - // get the current options - tcgetattr(fd, &options); - - // set local mode, enable receiver, set comm. options: - // 8 data bits, 1 stop bit, no parity, 9600 Baud - options.c_cflag = CLOCAL | CREAD | CS8 | B9600; - - // write options back to port - tcsetattr(fd, TCSANOW, &options); -} - -unsigned char* buf_tx = (unsigned char*) &local_uav; -unsigned char* buf_rx = (unsigned char*) &remote_uav; - -void send_port(void) -{ - int bytes; - int i = 0; - - if (new_ivy_data == 0) - return; - - new_ivy_data = 0; - - - local_uav.header = '@'; - local_uav.footer = 0; - // Checksum - for (i=0;i<(sizeof(local_uav)-1);i++) - { - local_uav.footer += buf_tx[i]; - printf("%x ", buf_tx[i]); - } - bytes = write(fd, &local_uav, sizeof(local_uav)); - printf("SENT: %d (%d bytes)\n",local_uav.ac_id, bytes); -} - -void read_port(void) -{ - int i; - static int counter = 0; - int readsize = sizeof(remote_uav) - counter; - int bytes = read(fd, buf_rx + counter, readsize); - unsigned char crc = 0; - - // printf("READ: %d bytes",bytes); - - if (bytes <= 0) - return; - - counter += bytes; - - if (counter >= sizeof(remote_uav)) - { - if (buf_rx[0] != '@') - { - printf("Protocol Error\n"); - } - for (i=0;i<(sizeof(remote_uav)-1);i++) - { - crc += buf_rx[i]; - printf("%x ", buf_rx[i]); - } - if (buf_rx[sizeof(remote_uav)-1] != crc) - { - printf("Checksum Error\n"); - } - printf("RECEIVED %d (%d bytes)\n",remote_uav.ac_id, counter); - counter -= sizeof(remote_uav); - - new_serial_data = 1; - remote_uav.ac_id = 6; - - send_ivy(); - } -} - -void close_port(void) -{ - close(fd); -} - -////////////////////////////////////////////////////////////////////////////////// -// TIMER -////////////////////////////////////////////////////////////////////////////////// - -// Timer -void handle_timer (TimerId id, void *data, unsigned long delta) -{ - static unsigned char dispatch = 0; - - // Every Time - read_port(); - - // One out of 2 - if (dispatch > 0) - { - send_port(); - dispatch = 0; - } - else - { - dispatch ++; - } -} - -TimerId tid; - -/// Handler for Ctrl-C, exits the main loop -void sigint_handler(int sig) { - printf("\nCLEAN STOP\n"); - IvyStop(); - TimerRemove(tid); - close_port(); -} - -////////////////////////////////////////////////////////////////////////////////// -// MAIN -////////////////////////////////////////////////////////////////////////////////// - -int main ( int argc, char** argv) -{ - int s = sizeof(local_uav); - - if (argc < 3) - { - printf("Use: ivy2serial ac_id serial_device\n"); - return -1; - } - - local_uav.ac_id = atoi(argv[1]); - - printf("Listening to AC=%d, \nSending Size of Data = %d \n",local_uav.ac_id, s); - - // make Ctrl-C stop the main loop and clean up properly - signal(SIGINT, sigint_handler); - - // Open Serial or Die - open_port(argv[2]); - - // Init UAV - remote_uav.ac_id = 6; - - remote_uav.phi = 1000; - remote_uav.theta = 200; - remote_uav.psi = -3140; - - - // create timer (Ivy) - tid = TimerRepeatAfter (0, delay / 2, handle_timer, 0); - - - IvyInit ("IVY <-> Serial", "IVY <-> Serial READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_Desired, NULL, "^%d DESIRED (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)",local_uav.ac_id); - IvyBindMsg(on_Attitude, NULL, "^%d ATTITUDE (\\S*) (\\S*) (\\S*)", local_uav.ac_id); - IvyBindMsg(on_Gps, NULL, "^%d GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)",local_uav.ac_id); - IvyStart("127.255.255.255"); - - IvyMainLoop (); - - return 0; -} - diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile index 72824e30df..93bb2bbe3f 100644 --- a/sw/ground_segment/tmtc/Makefile +++ b/sw/ground_segment/tmtc/Makefile @@ -35,7 +35,7 @@ include ../../../conf/Makefile.local CONF = ../../../conf VAR = ../../../var -all: link server messages settings dia diadec $(VAR)/boa.conf ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp +all: link server messages settings dia diadec $(VAR)/boa.conf ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy_serial_bridge clean: rm -f link server messages settings dia diadec *.bak *~ core *.o .depend *.opt *.out *.cm* ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp @@ -151,6 +151,9 @@ c_ivy_client_example_2: c_ivy_client_example_2.c c_ivy_client_example_3: c_ivy_client_example_3.c $(CC) $(GTK_CFLAGS) -o $@ $< $(GTK_LDFLAGS) +ivy_serial_bridge: ivy_serial_bridge.c + $(CC) $(GTK_CFLAGS) -o $@ $< $(GTK_LDFLAGS) + # # Dependencies diff --git a/sw/ground_segment/tmtc/ivy_serial_bridge.c b/sw/ground_segment/tmtc/ivy_serial_bridge.c new file mode 100644 index 0000000000..5dade2b7fb --- /dev/null +++ b/sw/ground_segment/tmtc/ivy_serial_bridge.c @@ -0,0 +1,851 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +//#include + + +#define Dprintf(X, ... ) +//#define Dprintf printf + +////////////////////////////////////////////////////////////////////////////////// +// SETTINGS +////////////////////////////////////////////////////////////////////////////////// + +// Serial Repeat Rate +long delay = 1000; + + +GtkWidget *status_ivy; +GtkWidget *status_serial; +GtkWidget *status; + +char status_str[256]; +char status_ivy_str[256]; +char status_serial_str[256]; +char *port = ""; + +long int count_ivy = 0; +long int count_serial = 0; + +long int rx_bytes = 0; +long int tx_bytes = 0; +long int rx_error = 0; + +char modem_id[32] = ""; +int power_level = 4; + +////////////////////////////////////////////////////////////////////////////////// +// local_uav DATA +////////////////////////////////////////////////////////////////////////////////// + + +struct _uav_type_ +{ + // Header + unsigned char header; + + // Data + unsigned char ac_id; + short int phi, theta, psi, speed; + int utm_east,utm_north,utm_z; + unsigned char utm_zone; + unsigned char pprz_mode; + float desired_alt; + float climb; + unsigned char block; + + // Footer + unsigned char footer; +} +__attribute__((packed)) + +local_uav, remote_uav; + +volatile unsigned char new_ivy_data = 0; +volatile unsigned char new_serial_data = 0; + +////////////////////////////////////////////////////////////////////////////////// +// IVY Reader +////////////////////////////////////////////////////////////////////////////////// + + +static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[]) +{ +/* + + + + + +*/ + + local_uav.phi = (short int) (atof(argv[0]) * 1000.0); + local_uav.psi = (short int) (atof(argv[1]) * 1000.0); + local_uav.theta = (short int) (atof(argv[2]) * 1000.0); + + //Dprintf("ATTITUDE ac=%d phi=%d theta=%d psi=%d ",local_uav.ac_id, local_uav.phi, local_uav.theta, local_uav.psi); +} + +static void on_Estimator(IvyClientPtr app, void *user_data, int argc, char *argv[]) +{ +/* + + + + +*/ + + local_uav.climb = atof(argv[1]); +} + +static void on_Navigation(IvyClientPtr app, void *user_data, int argc, char *argv[]) +{ +/* + + + + + + + + + + +*/ + + local_uav.block = atoi(argv[0]); +} + +static void on_Desired(IvyClientPtr app, void *user_data, int argc, char *argv[]) +{ +/* + + + + + + + + + +*/ + + local_uav.desired_alt = atof(argv[5]); +} + +static void on_Gps(IvyClientPtr app, void *user_data, int argc, char *argv[]) +{ +/* + + + + + + + + + + + + + +*/ + + local_uav.utm_east = atoi(argv[1]); + local_uav.utm_north = atoi(argv[2]); + local_uav.utm_z = atoi(argv[4]); + local_uav.utm_zone = atoi(argv[9]); + local_uav.speed = atoi(argv[5]); + + //Dprintf("ATTITUDE ac=%d phi=%d theta=%d psi=%d ",local_uav.ac_id, local_uav.phi, local_uav.theta, local_uav.psi); + //Dprintf("GPS ac=%d %d %d %d %d\n",local_uav.ac_id, local_uav.utm_east, local_uav.utm_north, local_uav.utm_z, local_uav.utm_zone); + + new_ivy_data = 1; +} + +////////////////////////////////////////////////////////////////////////////////// +// IVY Writer +////////////////////////////////////////////////////////////////////////////////// + +void send_ivy(void) +{ + float phi,theta,psi,z,zdot; + + if (new_serial_data == 0) + return; + + new_serial_data = 0; + + phi = ((float) remote_uav.phi) / 1000.0f; + theta = ((float) remote_uav.theta) / 1000.0f; + psi = ((float) remote_uav.psi) / 1000.0f; + + IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", remote_uav.ac_id); + + IvySendMsg("%d ATTITUDE %f %f %f\n", remote_uav.ac_id, phi, psi, theta); + +/* + remote_uav.utm_east = local_uav.utm_east; + remote_uav.utm_north = local_uav.utm_north + 5000; + remote_uav.utm_z = local_uav.utm_z + 1000; + remote_uav.utm_zone = local_uav.utm_zone; + remote_uav.speed = local_uav.speed * 4; + remote_uav.psi += 30.; +*/ + +/* + + + + + + + + + + + + + +*/ + + IvySendMsg("%d GPS 3 %d %d 0 %d %d 0 0 0 %d 0\n", remote_uav.ac_id, remote_uav.utm_east, remote_uav.utm_north, remote_uav.utm_z, remote_uav.speed, remote_uav.utm_zone); + +/* + + + + + + + +*/ + + IvySendMsg("%d FBW_STATUS 2 0 1 81 0 \n", remote_uav.ac_id); + + z = ((float)remote_uav.utm_z) / 1000.0f; + zdot = remote_uav.climb; + IvySendMsg("%d ESTIMATOR %f %f \n", remote_uav.ac_id, z, zdot); + +/* + + + + + + + + + +*/ + IvySendMsg("%d DESIRED 0 0 0 0 0 %f 0 \n", remote_uav.ac_id, remote_uav.desired_alt); + +/* + + + + + + + + + + +*/ + + IvySendMsg("%d NAVIGATION %d 0 0 0 0 0 0 0 \n", remote_uav.ac_id, remote_uav.block); + +/* + + + + + + + + + + +*/ + + // IvySendMsg("%d BAT 0 81 0 %ld 0 0 0 0\n", remote_uav.ac_id, count_serial); + +/* + + + + + + + + +*/ + + // IvySendMsg("%d PPRZ_MODE 0 0 0 0 0 0\n", remote_uav.ac_id); + +/* // Needed to show any NAV info like current block number + + + + + +*/ + + static int delayer = 10; + delayer++; + if (delayer > 5) + { + IvySendMsg("%d NAVIGATION_REF %d %d %d\n", remote_uav.ac_id, remote_uav.utm_east, remote_uav.utm_north, remote_uav.utm_zone); + delayer = 0; + } + + + + + count_serial++; + + sprintf(status_serial_str, "Read %d from '%s': forwarding to IVY [%ld] {Rx=%ld} {Err=%ld}", remote_uav.ac_id, port, count_serial, rx_bytes, rx_error); + gtk_label_set_text( GTK_LABEL(status_serial), status_serial_str ); +} + +////////////////////////////////////////////////////////////////////////////////// +// SERIAL PORT +////////////////////////////////////////////////////////////////////////////////// + +// pointer +int fd = 0; + +/// Open +void open_port(const char* device) { + fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd == -1) { + fprintf(stderr, "open_port: unable to open device %s - ", device); + perror(NULL); + exit(EXIT_FAILURE); + } + // setup connection options + struct termios options; + + // get the current options + tcgetattr(fd, &options); + + // set local mode, enable receiver, set comm. options: + // 8 data bits, 1 stop bit, no parity, 9600 Baud + options.c_cflag = CLOCAL | CREAD | CS8 | B9600; + + // write options back to port + tcsetattr(fd, TCSANOW, &options); +} + +unsigned char* buf_tx = (unsigned char*) &local_uav; +unsigned char* buf_rx = (unsigned char*) &remote_uav; + +void send_port(void) +{ + int bytes; + int i = 0; + + if (new_ivy_data == 0) + return; + + new_ivy_data = 0; + + + local_uav.header = '@'; + local_uav.footer = 0; + // Checksum + for (i=0;i<(sizeof(local_uav)-1);i++) + { + local_uav.footer += buf_tx[i]; + Dprintf("%x ", buf_tx[i]); + } + bytes = write(fd, &local_uav, sizeof(local_uav)); + + tx_bytes += bytes; + + Dprintf("SENT: %d (%d bytes)\n",local_uav.ac_id, bytes); + + count_ivy++; + + sprintf(status_ivy_str, "Received %d from IVY: forwarding to '%s' [%ld] {Tx=%ld}", local_uav.ac_id, port, count_ivy, tx_bytes); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); +} + +int set_power_level = -1; +int get_power_level = -1; + +int handle_api(void) +{ + static int step = 0; + int bytes; + int i=0; + char buff[32]; + + // ATPL4 = power level 4 + // ATMT0 = zero retry on broadcast + // ATRR = mac retry on NACK + + // ATDH = 0x00000000 + // ATDL = 0x0000FFFF + + // read only: + // Serial High + // Serial Low + + // ATDC = duty cycle status (percent 0 - 100) + // ATDB = Signal Strength + + + // ATWR = write to flash + // ATCN = exit command mode + + + step++; + + if (step > 35) + { + step = 35; + return 1; + } + + switch(step) + { + case 1: + sprintf(buff,"+++"); + bytes = write(fd, buff, strlen(buff)); + printf("+++ (bytes=%d %d)\n",bytes, fd); + + buff[strlen(buff)] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + + break; + case 8: + sprintf(buff,"ATPL\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATPL\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 10: + sprintf(buff,"ATPL%d\r", power_level); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATPL%d\n",power_level); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 12: + sprintf(buff,"ATPL\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATPL\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 14: + sprintf(buff,"ATDH\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATDH\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 16: + sprintf(buff,"ATDL\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATDL\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 18: + sprintf(buff,"ATSH\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATSH\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 20: + sprintf(buff,"ATSL\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATSL\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 22: + sprintf(buff,"ATMT0\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATMT0\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 24: + sprintf(buff,"ATRR0\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATRR0\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 26: + sprintf(buff,"ATDH00000000\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATDH00000000\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 28: + sprintf(buff,"ATDL0000FFFF\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATDL0000FFFF\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 30: + sprintf(buff,"ATWR\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATWR\n"); + + buff[strlen(buff) - 1] = 0; + strcpy(status_ivy_str,buff); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + break; + case 32: + sprintf(buff,"ATCN\r"); + write(fd, (unsigned char*) buff, strlen(buff)); + printf("ATCN\n"); + printf("Quit API\n"); + modem_id[16]=0; + sprintf(buff, ", XB_ADDR='%s', ", modem_id); + strcat(status_str,buff); + sprintf(buff, "ATPL=%d", power_level); + strcat(status_str,buff); + gtk_label_set_text( GTK_LABEL(status), status_str ); + break; + case 34: + sprintf(status_ivy_str, "---"); + gtk_label_set_text( GTK_LABEL(status_ivy), status_ivy_str ); + sprintf(status_serial_str, "---"); + gtk_label_set_text( GTK_LABEL(status_serial), status_serial_str ); + + break; + case 7: + case 9: + case 11: + case 13: + case 15: + case 17: + case 19: + case 21: + case 23: + case 25: + case 27: + case 29: + case 31: + case 33: + bytes = read(fd, (unsigned char*) buff, 32); + printf("Bytes %d %d\n",bytes, fd); + if ((bytes > 1) && (bytes < 10)) + { + buff[bytes-1] = 0; + strcpy(status_serial_str, buff); + gtk_label_set_text( GTK_LABEL(status_serial), status_serial_str ); + } + for (i=0;i= sizeof(remote_uav)) + { + if (buf_rx[0] != '@') + { + int head = 0; + for (head=0; head= 0) && (handle_api() == 0)) + return TRUE; + + // Every Time + read_port(); + + // One out of 4 + if (dispatch > 2) + { + send_port(); + dispatch = 0; + } + else + { + dispatch ++; + } + return TRUE; +} + +////////////////////////////////////////////////////////////////////////////////// +// MAIN +////////////////////////////////////////////////////////////////////////////////// + +gint delete_event( GtkWidget *widget, + GdkEvent *event, + gpointer data ) +{ + g_print ("CLEAN STOP\n"); + + close_port(); + IvyStop(); + + exit(0); + + return(FALSE); // false = delete window, FALSE = keep active +} + + +int main ( int argc, char** argv) +{ + int s = sizeof(local_uav); + + gtk_init(&argc, &argv); + + if (argc < 3) + { + printf("Use: ivy2serial ac_id serial_device\n"); + printf("or\n"); + printf("Use: ivy2serial ac_id serial_device xbee_power_level [0-4] to configure the xbee as broadcast, no retries\n"); + return -1; + } + + if (argc == 4) + { + printf("Programming XBee Modem\n"); + power_level = (int) (argv[3][0]) - (int) '0'; + if (power_level < 0) + power_level = 0; + else if (power_level > 4) + power_level = 4; + printf("Set Power Level To: '%d'\n", power_level); + } + else + { + power_level = -1; + } + + local_uav.ac_id = atoi(argv[1]); + + sprintf(status_str, "Listening to AC=%d, Serial Data Size = %d",local_uav.ac_id, s); + sprintf(status_ivy_str, "---"); + sprintf(status_serial_str, "---"); + printf("%s\n",status_str); + + // Open Serial or Die + port = argv[2]; + open_port(port); + + // Init UAV + remote_uav.ac_id = 6; + + remote_uav.phi = 1000; + remote_uav.theta = 200; + remote_uav.psi = -3140; + + // Start IVY + IvyInit ("IVY <-> Serial", "IVY <-> Serial READY", NULL, NULL, NULL, NULL); + IvyBindMsg(on_Desired, NULL, "^%d DESIRED (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)",local_uav.ac_id); + IvyBindMsg(on_Estimator, NULL, "^%d ESTIMATOR (\\S*) (\\S*)",local_uav.ac_id); + IvyBindMsg(on_Navigation, NULL, "^%d NAVIGATION (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)",local_uav.ac_id); + IvyBindMsg(on_Attitude, NULL, "^%d ATTITUDE (\\S*) (\\S*) (\\S*)", local_uav.ac_id); + IvyBindMsg(on_Gps, NULL, "^%d GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)",local_uav.ac_id); + IvyStart("127.255.255.255"); + + // Add Timer + gtk_timeout_add(delay / 4, timeout_callback, NULL); + + // GTK Window + GtkWidget *window = gtk_window_new (GTK_WINDOW_TOPLEVEL); + gtk_window_set_title (GTK_WINDOW (window), "IVY_Serial_Bridge"); + + gtk_signal_connect (GTK_OBJECT (window), "delete_event", + GTK_SIGNAL_FUNC (delete_event), NULL); + + GtkWidget *box = gtk_vbox_new(TRUE, 1); + gtk_container_add (GTK_CONTAINER (window), box); + + GtkWidget *hbox = gtk_hbox_new(FALSE, 1); + gtk_container_add (GTK_CONTAINER (box), hbox); + status = gtk_label_new( "Status:" ); + gtk_box_pack_start(GTK_BOX(hbox), status, FALSE, FALSE, 1); + gtk_label_set_justify( (GtkLabel*) status, GTK_JUSTIFY_LEFT ); + status = gtk_label_new( status_str ); + gtk_box_pack_start(GTK_BOX(hbox), status, FALSE, FALSE, 1); + gtk_label_set_justify( (GtkLabel*) status, GTK_JUSTIFY_LEFT ); + + hbox = gtk_hbox_new(FALSE, 1); + gtk_container_add (GTK_CONTAINER (box), hbox); + status_ivy = gtk_label_new( "IVY->SERIAL:" ); + gtk_box_pack_start(GTK_BOX(hbox), status_ivy, FALSE, FALSE, 1); + gtk_label_set_justify( (GtkLabel*) status_ivy, GTK_JUSTIFY_LEFT ); + status_ivy = gtk_label_new( status_ivy_str ); + gtk_box_pack_start(GTK_BOX(hbox), status_ivy, FALSE, FALSE, 1); + gtk_label_set_justify( (GtkLabel*) status_ivy, GTK_JUSTIFY_LEFT ); + + hbox = gtk_hbox_new(FALSE, 1); + gtk_container_add (GTK_CONTAINER (box), hbox); + status_serial = gtk_label_new( "SERIAL->IVY:" ); + gtk_box_pack_start(GTK_BOX(hbox), status_serial, FALSE, FALSE, 1); + gtk_label_set_justify( (GtkLabel*) status_serial, GTK_JUSTIFY_LEFT ); + status_serial = gtk_label_new( status_serial_str ); + gtk_label_set_justify( GTK_LABEL(status_serial), GTK_JUSTIFY_LEFT ); + gtk_box_pack_start(GTK_BOX(hbox), status_serial, FALSE, FALSE, 1); + + + gtk_widget_show_all(window); + + gtk_main(); + + // Clean up + fprintf(stderr,"Stopping\n"); + + + return 0; +} + diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index 82964a1be8..34cc714840 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -599,7 +599,8 @@ let send_config = fun http _asker args -> let fp = prefix ("var" // ac_name // "flight_plan.xml") and af = prefix ("conf" // ExtXml.attrib conf "airframe") and rc = prefix ("conf" // ExtXml.attrib conf "radio") - and settings = prefix ("var" // ac_name // "settings.xml") in + and settings = if not _is_replayed then prefix ("var" // ac_name // + "settings.xml") else "file://replay" in let col = try Xml.attrib conf "gui_color" with _ -> new_color () in let ac_name = try Xml.attrib conf "name" with _ -> "" in [ "ac_id", Pprz.String ac_id;