diff --git a/sw/airborne/beth/overo_gcs_com.c b/sw/airborne/beth/overo_gcs_com.c index 069e3da9be..250b50a419 100644 --- a/sw/airborne/beth/overo_gcs_com.c +++ b/sw/airborne/beth/overo_gcs_com.c @@ -11,8 +11,9 @@ //bill laptop //#define GCS_HOST "10.31.4.5" +#define GCS_HOST "192.168.2.10" //auto4 -#define GCS_HOST "10.31.4.104" +//#define GCS_HOST "10.31.4.104" #define GCS_PORT 4242 #define DATALINK_PORT 4243 diff --git a/sw/airborne/beth/overo_test_uart.c b/sw/airborne/beth/overo_test_uart.c index 6a60e00cc9..b4e1497885 100644 --- a/sw/airborne/beth/overo_test_uart.c +++ b/sw/airborne/beth/overo_test_uart.c @@ -34,6 +34,7 @@ #include "gps.h" #include "messages2.h" +//#include "dl_protocol2.h" #include "airframe.h" #include "fms_periodic.h" @@ -42,6 +43,8 @@ #include "overo_gcs_com.h" #include "uart_hw.h" +#include "pprz_transport.h" + struct OveroController { int armed; @@ -52,41 +55,44 @@ static void main_exit(int sig); static void main_talk_with_tiny(void); void check_gps(void); -//make gps.c happy without incluing navigation code -uint8_t nav_utm_zone0 = 5; +//make gps.c happy without including navigation code +uint8_t nav_utm_zone0 = 31; static uint16_t foo = 0; //struct FmsSerialPort* fmssp; //int spfd; uint8_t portnum; +#ifdef GPS_CONFIGURE +static uint8_t donegpsconf = 0; +#endif +static uint8_t configgps = 0; int main(int argc, char *argv[]) { + portnum = 0; if (argc > 1) { + portnum = atoi(argv[1]); if (portnum > 10 ) { printf("Port number must be <11\n"); return -1; } - portnum = atoi(argv[1]); - } else portnum = 0; + if (argc > 2) configgps = atoi(argv[2]); + if (configgps) +#ifdef GPS_CONFIGURE + printf("Will configure GPS.\n"); +#else + printf("Rebuild with GPS configure support.\n"); +#endif + } + + printf("Using /dev/ttyUSB%d for GPS\n",portnum); - printf("Using /dev/ttyUSB%d\n",portnum); (void) signal(SIGINT, main_exit); - uart_init(); + uart_init(); gps_init(); -/* fmssp = serial_port_new(); - //speed_t speed; - - if (serial_port_open_raw(fmssp,"/dev/ttyUSB0",B9600)){ - printf("error opening USB serial port!"); - return -1; - } - - spfd = (int)fmssp->fd; -*/ /* Initalize the event library */ event_init(); @@ -96,7 +102,11 @@ int main(int argc, char *argv[]) { TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); return -1; } - + +#ifdef GPS_CONFIGURE + //periodic task is launched so we are now ready to use uart to request gps baud change... + if (configgps) gps_configure_uart(); +#endif event_dispatch(); //should never occur! printf("goodbye! (%d)\n",foo); @@ -104,20 +114,19 @@ int main(int argc, char *argv[]) { return 0; } -static uint16_t tempstatus = 0; static void main_periodic(int my_sig_num) { - - RunOnceEvery(10, {DOWNLINK_SEND_MOTOR(gcs_com.udp_transport, &tempstatus, &foo );}); - RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);}); + //RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);}); #ifdef USE_UART0 uart0_handler(); -#elif USE_UART1 +#endif +#ifdef USE_UART1 uart1_handler(); #endif + DatalinkEvent() main_talk_with_tiny(); check_gps(); @@ -125,22 +134,77 @@ static void main_periodic(int my_sig_num) { } +#include "downlink.h" +#if 0 +uint8_t downlink_nb_ovrn; +uint16_t downlink_nb_bytes; +uint16_t downlink_nb_msgs; +#define __Transport(dev, _x) dev##_x +#define _Transport(dev, _x) __Transport(dev, _x) +#define Transport(_chan, _fun) _Transport(_chan, _fun) +#define DownlinkIDsSize(_chan, _x) (_x+2) +#define DownlinkSizeOf(_chan, _x) Transport(_chan, SizeOf(DownlinkIDsSize(_chan, _x))) + +#define DownlinkCheckFreeSpace(_chan, _x) Transport(_chan, CheckFreeSpace((uint8_t)(_x))) + +#define DownlinkPutUint8ByAddr(_chan, _x) Transport(_chan, PutUint8ByAddr(_x)) +#define DownlinkPutUint8Array(_chan, _n, _x) Transport(_chan, PutUint8Array(_n, _x)) + +#define DownlinkOverrun(_chan) downlink_nb_ovrn++; +#define DownlinkCountBytes(_chan, _n) downlink_nb_bytes += _n; + +#define DownlinkStartMessage(_chan, _name, msg_id, payload_len) { \ + downlink_nb_msgs++; \ + Transport(_chan, Header(DownlinkIDsSize(_chan, payload_len))); \ + Transport(_chan, PutUint8(AC_ID)); \ + Transport(_chan, PutNamedUint8(_name, msg_id)); \ +} + +#define DownlinkEndMessage(_chan) Transport(_chan, Trailer()) + + +#define __DOWNLINK_SEND_HITL_UBX(_chan, class, id, ac_id, nb_ubx_payload, ubx_payload){ \ + if (DownlinkCheckFreeSpace(_chan, DownlinkSizeOf(_chan, 0+1+1+1+1+nb_ubx_payload*1))) {\ + DownlinkCountBytes(_chan, DownlinkSizeOf(_chan, 0+1+1+1+1+nb_ubx_payload*1)); \ + DownlinkStartMessage(_chan, "HITL_UBX", DL_HITL_UBX, 0+1+1+1+1+nb_ubx_payload*1) \ + DownlinkPutUint8ByAddr(_chan, (class)); \ + DownlinkPutUint8ByAddr(_chan, (id)); \ + DownlinkPutUint8ByAddr(_chan, (ac_id)); \ + DownlinkPutUint8Array(_chan, nb_ubx_payload, ubx_payload); \ + DownlinkEndMessage(_chan ) \ + } else \ + DownlinkOverrun(_chan ); \ +} +#endif void check_gps(void){ /* if (GpsTimeoutError) { printf("gps timeout\n"); - } -*/ + }*/ if (GpsBuffer()) { ReadGpsBuffer(); } if (gps_msg_received) { - printf("gps msg rx\n"); - /* parse and use GPS messages */ +#ifdef GPS_CONFIGURE + if (gps_configuring) + gps_configure(); + else { + if (!donegpsconf) { + printf("Finished GPS configuration.\n"); + donegpsconf=1; + } + parse_gps_msg(); + } +#else parse_gps_msg(); +#endif + printf("gps msg rx %x %x\n",ubx_class,ubx_id); + const uint8_t ac_id = 3; + //DOWNLINK_SEND_HITL_UBX(gcs_com.udp_transport, &ubx_class, &ubx_id, &ac_id, &ubx_len ,ubx_msg_buf); + DOWNLINK_SEND_HITL_UBX(PprzTransport, &ubx_class, &ubx_id, &ac_id, ubx_len ,ubx_msg_buf); gps_msg_received = FALSE; if (gps_pos_available) { printf("gps pos avail\n"); @@ -153,8 +217,6 @@ void check_gps(void){ } static void main_exit(int sig) { - printf("Initiating shutdown...\n"); - printf("Application Exiting...\n"); exit(EXIT_SUCCESS); } diff --git a/sw/airborne/beth/uart_hw.c b/sw/airborne/beth/uart_hw.c index bb1bf86499..133236d4c8 100644 --- a/sw/airborne/beth/uart_hw.c +++ b/sw/airborne/beth/uart_hw.c @@ -31,6 +31,7 @@ #include "fms_serial_port.h" + #ifdef USE_UART0 volatile uint16_t uart0_rx_insert_idx, uart0_rx_extract_idx; @@ -44,14 +45,50 @@ struct FmsSerialPort* fmssp0; int uart0_fd; extern uint8_t portnum; + +//This function will close our UART and reopen with the new baud rate +#ifdef GPS_CONFIGURE +void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) { + + //serial_port_flush_output(fmssp0); + serial_port_close(fmssp0); + fmssp0 = serial_port_new(); + + if (portnum == 0) { + printf("opening ttyUSB0 on uart0 at %d\n",GPS_BAUD); + serial_port_open_raw(fmssp0,"/dev/ttyUSB0",GPS_BAUD); + } + if (portnum == 1) { + printf("opening ttyUSB1 on uart0 at %d\n",GPS_BAUD); + serial_port_open_raw(fmssp0,"/dev/ttyUSB1",GPS_BAUD); + } + + uart0_fd = (int)fmssp0->fd; + + // initialize the transmit data queue + uart0_tx_extract_idx = 0; + uart0_tx_insert_idx = 0; + uart0_tx_running = FALSE; + + // initialize the receive data queue + uart0_rx_extract_idx = 0; + uart0_rx_insert_idx = 0; + +} +#endif + void uart0_init( void ) { fmssp0 = serial_port_new(); + + //TODO: set device name in application and pass as argument if (portnum == 0) { + printf("opening ttyUSB0 on uart0 at %d\n",UART0_BAUD); serial_port_open_raw(fmssp0,"/dev/ttyUSB0",UART0_BAUD); } if (portnum == 1) { + printf("opening ttyUSB1 on uart0 at %d\n",UART0_BAUD); serial_port_open_raw(fmssp0,"/dev/ttyUSB1",UART0_BAUD); } uart0_fd = (int)fmssp0->fd; @@ -82,6 +119,7 @@ void uart0_transmit( uint8_t data ) { else { // no, set running flag and write to output register uart0_tx_running = TRUE; write(uart0_fd,&data,1); + //printf("w %x\n",data); } } @@ -99,15 +137,16 @@ void uart0_handler(void) { // check if more data to send if (uart0_tx_insert_idx != uart0_tx_extract_idx) { write(uart0_fd,&uart0_tx_buffer[uart0_tx_extract_idx],1); + //printf("w %x\n",uart0_tx_buffer[uart0_tx_extract_idx]); uart0_tx_extract_idx++; uart0_tx_extract_idx %= UART0_TX_BUFFER_SIZE; } else { uart0_tx_running = FALSE; // clear running flag -// USART_ITConfig(USART1, USART_IT_TXE, DISABLE); } if(read(uart0_fd,&c,1) > 0){ + //printf("r %x %c\n",c,c); uint16_t temp = (uart0_rx_insert_idx + 1) % UART0_RX_BUFFER_SIZE; uart0_rx_buffer[uart0_rx_insert_idx] = c; // check for more room in queue @@ -135,7 +174,14 @@ void uart1_init( void ) { fmssp1 = serial_port_new(); - serial_port_open_raw(fmssp1,"/dev/ttyUSB1",UART1_BAUD); + if (portnum == 0) { + printf("opening ttyUSB1 on uart1 at %d\n",UART1_BAUD); + serial_port_open_raw(fmssp1,"/dev/ttyUSB1",UART1_BAUD); + } + if (portnum == 1) { + printf("opening ttyUSB0 on uart1 at %d\n",UART1_BAUD); + serial_port_open_raw(fmssp1,"/dev/ttyUSB0",UART1_BAUD); + } uart1_fd = (int)fmssp1->fd; @@ -164,6 +210,7 @@ void uart1_transmit( uint8_t data ) { } else { // no, set running flag and write to output register uart1_tx_running = TRUE; + //printf("z %x\n",data); write(uart1_fd,&data,1); } @@ -182,15 +229,16 @@ void uart1_handler(void) { // check if more data to send if (uart1_tx_insert_idx != uart1_tx_extract_idx) { write(uart1_fd,&uart1_tx_buffer[uart1_tx_extract_idx],1); + //printf("z %x\n",uart1_tx_buffer[uart1_tx_extract_idx]); uart1_tx_extract_idx++; uart1_tx_extract_idx %= UART1_TX_BUFFER_SIZE; } else { uart1_tx_running = FALSE; // clear running flag -// USART_ITConfig(USART1, USART_IT_TXE, DISABLE); } if(read(uart1_fd,&c,1) > 0){ + //printf("s %x %c\n",c,c); uint16_t temp = (uart1_rx_insert_idx + 1) % UART1_RX_BUFFER_SIZE;; uart1_rx_buffer[uart1_rx_insert_idx] = c; // check for more room in queue @@ -210,6 +258,7 @@ void uart_init( void ) #ifdef USE_UART1 uart1_init(); #endif +//TODO: add uart2 and greater #ifdef USE_UART2 uart2_init(); #endif diff --git a/sw/airborne/beth/uart_hw.h b/sw/airborne/beth/uart_hw.h index 6fdd9c983d..9e77c20b6d 100644 --- a/sw/airborne/beth/uart_hw.h +++ b/sw/airborne/beth/uart_hw.h @@ -21,22 +21,23 @@ * Boston, MA 02111-1307, USA. */ -/* - *\brief STM32 usart functions - * - */ - #ifndef UART_HW_H #define UART_HW_H #include "std.h" -/* -#define B9600 9600 +//coment to avoid redefinition +/*#define B9600 9600 #define B38400 38400 -#define B57600 57600 + #define B57600 57600 #define B115200 115200 */ +//junk for gps_configure_uart in gps_ubx.c to compile +#define UART_8N1 1 +#define UART_FIFO_8 1 +#define UART_BAUD(baud) (baud) + + #define Uart1_init uart1_init() #define Uart2_init uart2_init() #define Uart3_init uart3_init() @@ -99,5 +100,6 @@ extern uint8_t uart1_tx_buffer[UART1_TX_BUFFER_SIZE]; void uart_init( void ); +void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode); #endif /* UART_HW_H */