[paparazzi-commits] [5914] Changes required for overo gps passthrough to twog.

[Paul Cox]

Revision: 5914
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5914
Author:   paulcox
Date:     2010-09-22 10:42:12 +0000 (Wed, 22 Sep 2010)
Log Message:
-----------
Changes required for overo gps passthrough to twog.

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c
    paparazzi3/trunk/sw/airborne/beth/overo_test_uart.c
    paparazzi3/trunk/sw/airborne/beth/uart_hw.c
    paparazzi3/trunk/sw/airborne/beth/uart_hw.h

Modified: paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c   2010-09-21 22:49:33 UTC 
(rev 5913)
+++ paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c   2010-09-22 10:42:12 UTC 
(rev 5914)
@@ -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
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_test_uart.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_test_uart.c 2010-09-21 22:49:33 UTC 
(rev 5913)
+++ paparazzi3/trunk/sw/airborne/beth/overo_test_uart.c 2010-09-22 10:42:12 UTC 
(rev 5914)
@@ -34,6 +34,7 @@
 #include "gps.h"
 
 #include "messages2.h"
+//#include "dl_protocol2.h"
 #include "airframe.h"
 
 #include "fms_periodic.h"
@@ -42,7 +43,9 @@
 
 #include "overo_gcs_com.h"
 #include "uart_hw.h"
+#include "pprz_transport.h"
 
+
 struct OveroController {
   int armed;
 } controller;
@@ -52,41 +55,44 @@
 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 @@
     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 @@
   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 @@
 
 }
 
+#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 @@
 }
 
 static void main_exit(int sig) {
-  printf("Initiating shutdown...\n");
-
   printf("Application Exiting...\n");
   exit(EXIT_SUCCESS);
 }

Modified: paparazzi3/trunk/sw/airborne/beth/uart_hw.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/uart_hw.c 2010-09-21 22:49:33 UTC (rev 
5913)
+++ paparazzi3/trunk/sw/airborne/beth/uart_hw.c 2010-09-22 10:42:12 UTC (rev 
5914)
@@ -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 @@
 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 @@
   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 @@
     // 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 @@
 
   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 @@
   }
   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 @@
     // 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 @@
 #ifdef USE_UART1 
   uart1_init();
 #endif
+//TODO: add uart2 and greater
 #ifdef USE_UART2 
   uart2_init();
 #endif

Modified: paparazzi3/trunk/sw/airborne/beth/uart_hw.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/uart_hw.h 2010-09-21 22:49:33 UTC (rev 
5913)
+++ paparazzi3/trunk/sw/airborne/beth/uart_hw.h 2010-09-22 10:42:12 UTC (rev 
5914)
@@ -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 @@
 
 
 void uart_init( void );
+void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode);
 
 #endif /* UART_HW_H */