[paparazzi-commits] [5915] Changes to accomodate ubx gps data passthrough from overo to twog.

[Paul Cox]

Revision: 5915
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5915
Author:   paulcox
Date:     2010-09-22 10:49:07 +0000 (Wed, 22 Sep 2010)
Log Message:
-----------
Changes to accomodate ubx gps data passthrough from overo to twog.
Overo forwards ublox messages to tiny via HITL code already present.
Overo receives ExtraPprz messages from twog.
See sw/airborne/beth/overo_test_uart.c for more info.

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/estimator.c
    paparazzi3/trunk/sw/airborne/gps.c
    paparazzi3/trunk/sw/airborne/gps_ubx.c
    paparazzi3/trunk/sw/airborne/gps_ubx.h
    paparazzi3/trunk/sw/airborne/main_ap.c

Modified: paparazzi3/trunk/sw/airborne/estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/estimator.c    2010-09-22 10:42:12 UTC (rev 
5914)
+++ paparazzi3/trunk/sw/airborne/estimator.c    2010-09-22 10:49:07 UTC (rev 
5915)
@@ -34,8 +34,10 @@
 #include "ap_downlink.h"
 #include "gps.h"
 #include "nav.h"
+#ifdef EXTRA_DOWNLINK_DEVICE
+#include "core/extra_pprz_dl.h"
+#endif
 
-
 /* position in meters */
 float estimator_x;
 float estimator_y;
@@ -227,5 +229,8 @@
   estimator_psi = atan2f(w_ve, w_vn);
   if (estimator_psi < 0.)
     estimator_psi += 2 * M_PI;
+#ifdef EXTRA_DOWNLINK_DEVICE    
+    
DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
+#endif    
 }
 

Modified: paparazzi3/trunk/sw/airborne/gps.c
===================================================================
--- paparazzi3/trunk/sw/airborne/gps.c  2010-09-22 10:42:12 UTC (rev 5914)
+++ paparazzi3/trunk/sw/airborne/gps.c  2010-09-22 10:49:07 UTC (rev 5915)
@@ -31,7 +31,9 @@
 
 #include "gps.h"
 #include "latlong.h"
+#ifndef FMS_PERIODIC_FREQ
 #include "sys_time.h"
+#endif
 #include "airframe.h"
 #include "periodic.h"
 
@@ -45,9 +47,17 @@
 #define DOWNLINK_GPS_DEVICE DOWNLINK_AP_DEVICE
 #endif
 
+
+
+#ifdef FMS_PERIODIC_FREQ
+#include "messages2.h"
+#include "beth/overo_gcs_com.h"
+#define DefaultChannel gcs_com.udp_transport
+#else
 #define DOWNLINK_DEVICE DOWNLINK_GPS_DEVICE
 #include "messages.h"
 #include "downlink.h"
+#endif
 
 uint16_t last_gps_msg_t;       /** cputime of the last gps message */
 bool_t gps_verbose_downlink;

Modified: paparazzi3/trunk/sw/airborne/gps_ubx.c
===================================================================
--- paparazzi3/trunk/sw/airborne/gps_ubx.c      2010-09-22 10:42:12 UTC (rev 
5914)
+++ paparazzi3/trunk/sw/airborne/gps_ubx.c      2010-09-22 10:49:07 UTC (rev 
5915)
@@ -30,6 +30,12 @@
 #include <string.h> 
 #include <math.h>
 
+#ifdef FMS_PERIODIC_FREQ
+//for printf
+#include <stdio.h>
+//for baudrate
+#include "fms_serial_port.h"
+#endif 
 
 #include "flight_plan.h"
 #include "uart.h"
@@ -83,7 +89,7 @@
 uint8_t gps_mode;
 volatile bool_t gps_msg_received;
 bool_t gps_pos_available;
-uint8_t ubx_id, ubx_class;
+uint8_t ubx_id, ubx_class,ubx_len;
 int32_t gps_lat, gps_lon;
 uint16_t gps_reset;
 
@@ -109,7 +115,6 @@
 #define GOT_CHECKSUM1 8
 
 static uint8_t  ubx_status;
-static uint16_t ubx_len;
 static uint8_t  ubx_msg_idx;
 static uint8_t ck_a, ck_b;
 uint8_t send_ck_a, send_ck_b;
@@ -153,10 +158,23 @@
    change the uart rate. */
 #if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2
 void gps_configure_uart ( void ) {
+#ifdef FMS_PERIODIC_FREQ
+  UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, 
UBX_PROTO_MASK, 0x0, 0x0);
+  uint8_t loop=0;
+  while (GpsUartRunning) {
+    //doesn't work unless some printfs are used, so :
+    if (loop<9) { 
+      printf("."); loop++;  
+    } else {
+      printf("\b"); loop--;
+    }
+  }
+#else
   UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, GPS_BAUD, UBX_PROTO_MASK, 
UBX_PROTO_MASK, 0x0, 0x0);
+  while (GpsUartRunning); /* FIXME */
+#endif
 
-  while (GpsUartRunning) ; /* FIXME */
-  GpsUartInitParam( UART_BAUD(GPS_BAUD),  UART_8N1, UART_FIFO_8);
+  GpsUartInitParam( GPS_BAUD,  UART_8N1, UART_FIFO_8);
 }
 #endif
 
@@ -166,15 +184,18 @@
 }
 #endif
 
+#define IGNORED 0
+#define RESERVED 0
 
-
 #ifdef USER_GPS_CONFIGURE
 #include USER_GPS_CONFIGURE
 #else 
 static bool_t user_gps_configure(bool_t cpt) {
   switch (cpt) {
   case 0:
-    UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 
0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 
0x00);
+    //New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no 
longer available
+    //UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 
0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 
0x00);
+    UbxSend_CFG_NAV5(0x05, NAV5_DYN_AIRBORNE_2G, NAV5_3D_ONLY, IGNORED, 
IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, 
RESERVED, RESERVED, RESERVED, RESERVED);
     break;
   case 1:
     UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0);

Modified: paparazzi3/trunk/sw/airborne/gps_ubx.h
===================================================================
--- paparazzi3/trunk/sw/airborne/gps_ubx.h      2010-09-22 10:42:12 UTC (rev 
5914)
+++ paparazzi3/trunk/sw/airborne/gps_ubx.h      2010-09-22 10:49:07 UTC (rev 
5915)
@@ -35,7 +35,7 @@
 
 extern uint16_t gps_reset;
 
-extern uint8_t ubx_id, ubx_class;
+extern uint8_t ubx_id, ubx_class,ubx_len;
 #define UBX_MAX_PAYLOAD 255
 extern uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD];
 

Modified: paparazzi3/trunk/sw/airborne/main_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/main_ap.c      2010-09-22 10:42:12 UTC (rev 
5914)
+++ paparazzi3/trunk/sw/airborne/main_ap.c      2010-09-22 10:49:07 UTC (rev 
5915)
@@ -499,6 +499,9 @@
 #endif
 #endif
     estimator_propagate_state();
+#ifdef EXTRA_DOWNLINK_DEVICE
+    
DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
+#endif    
     navigation_task();
     break;
   case 1:
@@ -874,7 +877,7 @@
 #endif /* UGEAR*/
 
 #ifdef GPS
-#ifndef HITL /** else comes through the datalink */
+#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the 
datalink */
   if (GpsBuffer()) {
     ReadGpsBuffer();
   }
@@ -1008,5 +1011,4 @@
   }
 
   modules_event_task();
-
 } /* event_task_ap() */