[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6203] Make osam ugear a module
|
From: |
Martin Mueller |
|
Subject: |
[paparazzi-commits] [6203] Make osam ugear a module |
|
Date: |
Fri, 22 Oct 2010 21:51:02 +0000 |
Revision: 6203
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6203
Author: mmm
Date: 2010-10-22 21:51:01 +0000 (Fri, 22 Oct 2010)
Log Message:
-----------
Make osam ugear a module
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/osam_xsens_twog.xml
paparazzi3/trunk/doc/OSAM_IMU/readme_pprz_osam_imu.txt
paparazzi3/trunk/sw/airborne/infrared.c
paparazzi3/trunk/sw/airborne/main_ap.c
Added Paths:
-----------
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/gps_ugear.makefile
paparazzi3/trunk/conf/modules/ins_osam_ugear.xml
paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/osam_imu_ugear.c
paparazzi3/trunk/sw/airborne/osam_imu_ugear.h
Modified: paparazzi3/trunk/conf/airframes/osam_xsens_twog.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/osam_xsens_twog.xml 2010-10-21 21:18:45 UTC
(rev 6202)
+++ paparazzi3/trunk/conf/airframes/osam_xsens_twog.xml 2010-10-22 21:51:01 UTC
(rev 6203)
@@ -2,6 +2,51 @@
<airframe name="OSAMUGEAR">
+ <firmware name="fixedwing">
+ <target name="ap" board="tiny_2.11">
+ <define name="AGR_CLIMB"/>
+ <define name="LOITER_TRIM"/>
+ <define name="ALT_KALMAN"/>
+ <define name="WIND_INFO"/>
+ <define name="UGEAR"/>
+ <define name="UGEAR_LED" value="2"/>
+ <define name="UGEAR_LINK" value="Uart0"/>
+ <define name="USE_UART0"/>
+ <define name="UART0_BAUD" value="B115200"/>
+ </target>
+
+ <target name="sim" board="pc">
+ <define name="AGR_CLIMB"/>
+ <define name="LOITER_TRIM"/>
+ <define name="ALT_KALMAN"/>
+ </target>
+
+ <subsystem name="radio_control" type="ppm"/>
+
+ <!-- Communication -->
+ <subsystem name="telemetry" type="transparent">
+ <param name="MODEM_BAUD" value="B38400"/>
+ </subsystem>
+
+ <!-- Actuators are automatically chosen according to board-->
+ <subsystem name="control"/>
+ <!-- Sensors -->
+ <!--subsystem name="attitude" type="infrared"/-->
+ <subsystem name="gps" type="ugear"/>
+ <subsystem name="navigation"/>
+ </firmware>
+
+ <firmware name="setup">
+ <target name="tunnel" board="tiny_2.11"/>
+ <target name="usb_tunnel_0" board="tiny_2.11"/>
+ <target name="usb_tunnel_1" board="tiny_2.11"/>
+ <target name="setup_actuators" board="tiny_2.11"/>
+ </firmware>
+
+ <!-- modules -->
+ <modules>
+ <load name="ins_osam_ugear.xml"/>
+ </modules>
<!-- commands section -->
<servos>
<servo name="AILEVON_RIGHT" no="3" min="1900" neutral="1450" max="1000"/>
@@ -72,6 +117,12 @@
<define name="CORRECTION_LEFT" value="1.0"/>
<define name="CORRECTION_RIGHT" value="1.0"/>
</section>
+
+ <section name="INS" prefix="INS_">
+ <define name="ROLL_NEUTRAL_DEFAULT" value="-5.73" unit="deg"/>
+ <define name="PITCH_NEUTRAL_DEFAULT" value="5.73" unit="deg"/>
+ </section>
+
<!--
<section name="GYRO" prefix="GYRO_">
<define name="ADC_ROLL_NEUTRAL" value="497"/>
@@ -83,7 +134,7 @@
</section>
-->
<section name="BAT">
- <define name="MILLIAMP_PER_PERCENT" value="0.86"/>
+ <define name="MILLIAMP_AT_FULL_THROTTLE" value="10000."/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
</section>
@@ -159,56 +210,20 @@
<define name="YAW_RESPONSE_FACTOR" value="0.5"/>
</section>
-->
- <makefile>
-CONFIG=\"tiny_2_1.h\"
-include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
+ <!--makefile>
-FLASH_MODE=IAP
-
-ap.CFLAGS += -DFBW -DAP -DBOARD_CONFIG=$(CONFIG) -DLED -DTIME_LED=1
-ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c
main_ap.c main.c
-
-ap.srcs += commands.c
-
-ap.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017
-ap.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c
-
-# 72MHz
-ap.CFLAGS += -DRADIO_CONTROL
-ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
-
# Maxstream
ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport
-DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1
-DDATALINK=PPRZ -DUART1_BAUD=B38400
ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c pprz_transport.c
-ap.CFLAGS += -DINTER_MCU
-ap.srcs += inter_mcu.c
-
-ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3
-ap.srcs += $(SRC_ARCH)/adc_hw.c
-
-#ap.CFLAGS += -DGYRO -DADXRS150
-#ap.srcs += gyro.c
-
# Commented out the following two lines for adding stuffs about communication
with Gumstix+MNAV Haiyang 20080507
#ap.CFLAGS += -DGPS -DGPS_LED=2 -DUBX -DUSE_UART1 -DGPS_LINK=Uart1
-DUART1_BAUD=B38400
#ap.srcs += gps_ubx.c gps.c latlong.c
-ap.CFLAGS += -DUGEAR -DUGEAR_LED=2 -DUSE_UART0 -DUGEAR_LINK=Uart0
-DUART0_BAUD=B115200 -DDOWNLINK_GPS_1Hz
-ap.srcs += osam_imu_ugear.c gps.c latlong.c
#ap.CFLAGS += -DINFRARED -DIMUIR 20080821
ap.CFLAGS += -DINFRARED
ap.srcs += infrared.c estimator.c
-ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DWIND_INFO
-ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
-ap.srcs += nav_line.c nav_survey_rectangle.c
-
-# Config for SITL simulation
-include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_survey_rectangle.c nav_line.c
-
- </makefile>
+ </makefile-->
</airframe>
Added: paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/gps_ugear.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/gps_ugear.makefile
(rev 0)
+++ paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/gps_ugear.makefile
2010-10-22 21:51:01 UTC (rev 6203)
@@ -0,0 +1,5 @@
+# OSAM UGEAR
+
+#ap.CFLAGS += -DGPS
+
+$(TARGET).srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c
Added: paparazzi3/trunk/conf/modules/ins_osam_ugear.xml
===================================================================
--- paparazzi3/trunk/conf/modules/ins_osam_ugear.xml
(rev 0)
+++ paparazzi3/trunk/conf/modules/ins_osam_ugear.xml 2010-10-22 21:51:01 UTC
(rev 6203)
@@ -0,0 +1,13 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ins_osam_ugear" dir="ins">
+ <header>
+ <file name="ins_osam_ugear.h"/>
+ </header>
+ <init fun="ugear_init()"/>
+ <event fun="ugear_event()"/>
+ <makefile target="ap">
+ <file name="ins_osam_ugear.c"/>
+ </makefile>
+</module>
+
Modified: paparazzi3/trunk/doc/OSAM_IMU/readme_pprz_osam_imu.txt
===================================================================
--- paparazzi3/trunk/doc/OSAM_IMU/readme_pprz_osam_imu.txt 2010-10-21
21:18:45 UTC (rev 6202)
+++ paparazzi3/trunk/doc/OSAM_IMU/readme_pprz_osam_imu.txt 2010-10-22
21:51:01 UTC (rev 6203)
@@ -6,38 +6,13 @@
1. Brief
This file is to explain what we have done to make the pprz compatible with IMU
(xsens-mtig and microstrain-gx2).
Haiyang Chao on 20080912 for version 1
+adapted to new build config 20101022
2. New Airborne files added
-~/paparazzi3/sw/airborne/osam_imu_ugear.h
-~/paparazzi3/sw/airborne/osam_imu_ugear.c
+~/paparazzi3/sw/airborne/modules/ins/ins_osam_ugear.h
+~/paparazzi3/sw/airborne/modules/ins/ins_osam_ugear.c
3. Airborne Files modified (all modifications we made are triggered only if
UGEAR is defined)
-~/paparazzi3/sw/airborne/main_ap.c
------Line 122------------------------
-#ifdef UGEAR
-#include "osam_imu_ugear.h"
-#endif
-
-#ifdef XSENS
-#include "xsens_ins.h"
-#endif
------Line 733------------------------
-#ifdef UGEAR
- if (UgearBuffer()){
- ReadUgearBuffer();
- }
- if (ugear_msg_received){
- parse_ugear_msg();
- ugear_msg_received = FALSE;
- if (gps_pos_available){
- gps_downlink();
- gps_verbose_downlink = !launch;
- UseGpsPos(estimator_update_state_gps);
- gps_pos_available = FALSE;
- }
- }
-#endif /* UGEAR*/
------------------------------
~/paparazzi3/sw/airborne/ap_downlink.h
-----Line 165------------------------
#if defined UGEAR
@@ -51,20 +26,6 @@
#elif defined UGEAR
#define GPS_NB_CHANNELS 16
-----------------------------
-~/paparazzi3/sw/airborne/infrared.c
------Line 42------------------------
-#ifdef UGEAR
-#include "osam_imu_ugear.h"
-#endif
------Line 239------------------------
-#if defined UGEAR
- #if !(defined IMUIR)
- ugear_debug3 = 333;
- estimator_phi = (float)ugear_phi/10000 - ir_roll_neutral;
- estimator_theta = (float)ugear_theta/10000 - ir_pitch_neutral;
- #endif
-#endif
------------------------------
4. Configuration files changed
~/paparazzi3/conf/message.xml
@@ -83,4 +44,5 @@
~/paparazzi3/conf/telemetry/osam_imu.xml
~/paparazzi3/conf/flight_plans/xsens_cachejunction.xml
~/paparazzi3/conf/airframes/osam_xsens_twog.xml
+~/paparazzi3/conf/settings/tuning_ins.xml
Modified: paparazzi3/trunk/sw/airborne/infrared.c
===================================================================
--- paparazzi3/trunk/sw/airborne/infrared.c 2010-10-21 21:18:45 UTC (rev
6202)
+++ paparazzi3/trunk/sw/airborne/infrared.c 2010-10-22 21:51:01 UTC (rev
6203)
@@ -39,10 +39,6 @@
#include "sys_time.h"
#include "airframe.h"
-#ifdef UGEAR
-#include "osam_imu_ugear.h"
-#endif
-
#if defined IR_ESTIMATED_PHI_PI_4 || defined IR_ESTIMATED_PHI_MINUS_PI_4 ||
defined IR_ESTIMATED_THETA_PI_4
#error "IR_ESTIMATED_PHI_PI_4 correction has been deprecated. Please remove
the definition from your airframe config file"
#endif
@@ -229,12 +225,4 @@
else
estimator_theta *= ir_correction_down;
-#if defined UGEAR
- #if !(defined IMUIR)
- ugear_debug3 = 333;
- estimator_phi = (float)ugear_phi/10000 - ir_roll_neutral;
- estimator_theta = (float)ugear_theta/10000 - ir_pitch_neutral;
- #endif
-#endif
-
}
Modified: paparazzi3/trunk/sw/airborne/main_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/main_ap.c 2010-10-21 21:18:45 UTC (rev
6202)
+++ paparazzi3/trunk/sw/airborne/main_ap.c 2010-10-22 21:51:01 UTC (rev
6203)
@@ -99,15 +99,8 @@
#include "baro_ets.h"
#endif // USE_BARO_ETS
-/*code added by Haiyang Chao for using Xsens IMU for fixed wing UAV 20080804*/
-#ifdef UGEAR
-#include "osam_imu_ugear.h"
-#endif
-
-/*code added by Haiyang Chao ends*/
-
#if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY
-#warning "LOW_BATTERY deprecated. Renammed into CATASTROPHIC_BAT_LEVEL (in
aiframe file)"
+#warning "LOW_BATTERY deprecated. Renamed into CATASTROPHIC_BAT_LEVEL (in
airframe file)"
#define CATASTROPHIC_BAT_LEVEL LOW_BATTERY
#endif
@@ -614,10 +607,6 @@
gps_configure_uart();
#endif
-#ifdef UGEAR
- ugear_init();
-#endif /*added by haiyang for ugear communication*/
-
#if defined DATALINK
#if DATALINK == XBEE
@@ -647,34 +636,6 @@
/*********** EVENT ***********************************************************/
void event_task_ap( void ) {
-#ifdef UGEAR
- if (UgearBuffer()){
- ReadUgearBuffer();
- }
- if (ugear_msg_received){
- parse_ugear_msg();
- ugear_msg_received = FALSE;
- if (gps_pos_available){
- //gps_downlink();
- gps_verbose_downlink = !launch;
- UseGpsPosNoSend(estimator_update_state_gps);
- gps_msg_received_counter = gps_msg_received_counter+1;
- #ifdef GX2
- if (gps_msg_received_counter == 1){
- gps_send();
- gps_msg_received_counter = 0;
- }
- #endif
- #ifdef XSENSDL
- if (gps_msg_received_counter == 25){
- gps_send();
- gps_msg_received_counter = 0;
- }
- #endif
- gps_pos_available = FALSE;
- }
- }
-#endif /* UGEAR*/
#ifdef GPS
#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the
datalink */
Copied: paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.c (from rev
6202, paparazzi3/trunk/sw/airborne/osam_imu_ugear.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.c 2010-10-22
21:51:01 UTC (rev 6203)
@@ -0,0 +1,329 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2003-2006 Haiyang Chao
+ *
+ * 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.
+ *
+ */
+
+/** \file osam_imu_ugear.c
+ * \brief Communication with any IMU through serial communication (originally
gps uart)
+ * This file is first generated by Haiyang Chao on 20080507 for communication.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "ins_osam_ugear.h"
+#include "gps.h"
+#include "gps_ubx.h"
+#include "latlong.h"
+#include "sys_time.h"
+#include "airframe.h"
+#include "nav.h"
+#include "estimator.h"
+#include "xsens_protocol.h"
+
+#define UNINIT 0
+#define GOT_SYNC1 1
+#define GOT_SYNC2 2
+#define GOT_ID 3
+#define GOT_LEN 4
+#define GOT_PAYLOAD 5
+#define GOT_CHECKSUM1 6
+#define GOT_CHECKSUM2 7
+
+#define UGEAR_SYNC1 0x93
+#define UGEAR_SYNC2 0xE0
+/*#define UGEAR_MAX_PAYLOAD 24*/
+#define UGEAR_MAX_PAYLOAD 40
+#define IMU_PACKET_SIZE 12
+#define RAD2DEG 57.3
+#define WrapUp(x) (x < 0 ? x+3600 : x)
+
+/* from main_ap.c */
+extern bool_t launch;
+
+/* variable defined for dlsetting generated by Haiyang 20080717*/
+bool_t imu1_ir0 = 0;
+float fd_alpha = 0;
+float fi_alpha = 0;
+float ins_roll_neutral;
+float ins_pitch_neutral;
+
+/* variable definition copied from gps_ubx.c 20080508*/
+uint16_t gps_week;
+uint32_t gps_itow;
+int32_t gps_alt;
+uint16_t gps_gspeed;
+int16_t gps_climb;
+int16_t gps_course;
+int32_t gps_utm_east, gps_utm_north;
+uint8_t gps_utm_zone;
+uint8_t gps_mode;
+volatile bool_t gps_msg_received;
+bool_t gps_pos_available;
+uint8_t ugear_id, ugear_class;
+int32_t gps_lat, gps_lon;
+uint16_t gps_reset;
+
+uint16_t gps_PDOP;
+uint32_t gps_Pacc, gps_Sacc;
+uint8_t gps_numSV;
+/* variable definition copied from gps_ubx.c20080508*/
+
+/* variable definition copied from gps.c20080515*/
+uint8_t gps_nb_ovrn;
+struct svinfo gps_svinfos[GPS_NB_CHANNELS];
+uint8_t gps_nb_channels;
+
+/* variable definition copied from gps.c20080515*/
+
+//uint16_t last_gps_msg_t; /** cputime of the last gps message */
+
+volatile bool_t ugear_msg_received;
+
+static uint8_t ugear_status;
+static uint8_t ugear_type;
+static uint8_t ugear_len;
+static uint8_t ugear_msg_idx;
+static uint8_t ck_a, ck_b;
+
+int16_t ugear_phi;
+int16_t ugear_psi;
+int16_t ugear_theta;
+
+int16_t gps_ve;
+int16_t gps_vn;
+int16_t gps_vd;
+/* added 20080522 for debugging*/
+int16_t ugear_debug1;
+int16_t ugear_debug2;
+int16_t ugear_debug3;
+int32_t ugear_debug4;
+int32_t ugear_debug5;
+int32_t ugear_debug6;
+
+//bool_t gps_verbose_downlink;
+
+uint8_t ugear_msg_buf[UGEAR_MAX_PAYLOAD] __attribute__ ((aligned));
+
+/*The following definition is for ugear data 20080509, modified on 0512
Haiyang*/
+struct imu {
+ int32_t phi,the,psi; /* attitudes */
+};
+
+struct gps {
+ int32_t lon,lat,alt;
+ int16_t ve,vn,vd;
+};
+
+struct imu imupacket;
+struct gps gpspacket;
+/*The above definition is for ugear data 20080509 Haiyang*/
+
+void ugear_init( void ) {
+ ugear_status = UNINIT;
+ ugear_phi = 0;
+ ugear_psi = 0;
+ ugear_theta = 0;
+ ugear_debug2 = 0;
+ ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+ ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+}
+
+void parse_ugear( uint8_t c ) {
+ /*checksum go first*/
+ if (ugear_status < GOT_PAYLOAD) {
+ ck_a += c;
+ ck_b += ck_a;
+ }
+ switch (ugear_status) {
+ case UNINIT:
+ if (c == UGEAR_SYNC1)
+ ugear_status++;
+ break;
+ case GOT_SYNC1:
+ if (c != UGEAR_SYNC2)
+ goto error;
+ ck_a = 0;
+ ck_b = 0;
+ ugear_status++;
+ break;
+ case GOT_SYNC2:
+ if (ugear_msg_received) {
+ /* Previous message has not yet been parsed: discard this one */
+ goto error;
+ }
+ ugear_type = c;
+ ugear_status++;
+ //ugear_theta = 30; // for debug
+ if (ugear_type > 2)
+ goto restart;
+ break;
+ case GOT_ID:
+ ugear_len = c;
+ ugear_msg_idx = 0;
+ ugear_status++;
+ break;
+ case GOT_LEN:
+ ugear_msg_buf[ugear_msg_idx] = c;
+ ugear_msg_idx++;
+ if (ugear_msg_idx >= ugear_len) {
+ ugear_status++;
+ }
+ break;
+ case GOT_PAYLOAD:
+ if (c != ck_a)
+ goto error;
+ ugear_status++;
+ break;
+ case GOT_CHECKSUM1:
+ if (c != ck_b)
+ goto error;
+ ugear_msg_received = TRUE;
+ UgearToggleLed();
+ goto restart;
+ break;
+ }
+ return;
+error:
+restart:
+ ugear_status = UNINIT;
+ return;
+}
+/*
+void decode_imupacket( struct imu *data, uint8_t* buffer){
+
+ data->phi = (double)((((signed char)buffer[25])<<8)|buffer[26]);
+ data->theta = (double)((((signed char)buffer[27])<<8)|buffer[28]);
+ data->psi = (double)((((signed char)buffer[29])<<8)|buffer[30]);
+
+}
+*/
+void parse_ugear_msg( void ){
+
+ float ins_phi, ins_psi, ins_theta;
+
+ switch (ugear_type){
+ case 0: /*gps*/
+ ugear_debug1 = ugear_debug1+1;
+ gps_lat = UGEAR_NAV_POSLLH_LAT(ugear_msg_buf);
+ gps_lon = UGEAR_NAV_POSLLH_LON(ugear_msg_buf);
+
+ nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
+ latlong_utm_of(RadOfDeg(gps_lat/1e7),
RadOfDeg(gps_lon/1e7), nav_utm_zone0);
+ gps_utm_east = latlong_utm_x * 100;
+ gps_utm_north = latlong_utm_y * 100;
+
+ gps_alt = UGEAR_NAV_POSLLH_HEIGHT(ugear_msg_buf);
+ gps_utm_zone = nav_utm_zone0;
+
+ gps_gspeed = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
+ gps_climb = - UGEAR_NAV_POSLLH_VD(ugear_msg_buf);
+ gps_course =
UGEAR_NAV_VELNED_Heading(ugear_msg_buf)/10000; /*in decdegree */
+ gps_PDOP = UGEAR_NAV_SOL_PDOP(ugear_msg_buf);
+ gps_Pacc = UGEAR_NAV_SOL_Pacc(ugear_msg_buf);
+ gps_Sacc = UGEAR_NAV_SOL_Sacc(ugear_msg_buf);
+ gps_numSV = UGEAR_NAV_SOL_numSV(ugear_msg_buf);
+ gps_week = 0; // FIXME
+ gps_itow = UGEAR_NAV_VELNED_ITOW(ugear_msg_buf);
+
+ //ugear_debug2 = gps_climb;
+ //ugear_debug4 =
(int32_t)(UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf));
+ //ugear_debug5 = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
+ //ugear_debug6 = (int16_t)estimator_phi*100;
+
+ gps_mode = 3; /*force GPSfix to be valided*/
+ gps_pos_available = TRUE; /* The 3 UBX messages are
sent in one rafale */
+ break;
+ case 1: /*IMU*/
+ ugear_debug2 = ugear_debug2+1;
+ ugear_phi = UGEAR_IMU_PHI(ugear_msg_buf);
+ ugear_psi = UGEAR_IMU_PSI(ugear_msg_buf);
+ ugear_theta = UGEAR_IMU_THE(ugear_msg_buf);
+ ugear_debug4 = (int32_t)ugear_phi;
+ ugear_debug5 = (int32_t)ugear_theta;
+ ugear_debug6 = (int32_t)ugear_psi;
+ ugear_debug3 = 333;
+ ins_phi = (float)ugear_phi/10000 - ins_roll_neutral;
+ ins_psi = 0;
+ ins_theta = (float)ugear_theta/10000 -
ins_pitch_neutral;
+#ifndef INFRARED
+ EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
+#endif
+ break;
+ case 2: /*GPS status*/
+// ugear_debug1 = 2;
+ gps_nb_channels = XSENS_GPSStatus_nch(ugear_msg_buf);
+ uint8_t is;
+ for(is = 0; is < Min(gps_nb_channels, 16); is++) {
+ uint8_t ch =
XSENS_GPSStatus_chn(ugear_msg_buf,is);
+ if (ch > 16) continue;
+ gps_svinfos[ch].svid =
XSENS_GPSStatus_svid(ugear_msg_buf, is);
+ gps_svinfos[ch].flags =
XSENS_GPSStatus_bitmask(ugear_msg_buf, is);
+ gps_svinfos[ch].qi =
XSENS_GPSStatus_qi(ugear_msg_buf, is);
+ gps_svinfos[ch].cno =
XSENS_GPSStatus_cnr(ugear_msg_buf, is);
+ gps_svinfos[ch].elev = 0;
+ gps_svinfos[ch].azim = 0;
+ }
+ break;
+ case 3: /*servo*/
+ break;
+ case 4: /*health*/
+ break;
+
+ }
+
+}
+
+/* add the following function only to get rid of compilation error in
datalink.c 20080608 Haiyang*/
+void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) {
+}
+
+void ugear_event( void ) {
+ if (UgearBuffer()){
+ ReadUgearBuffer();
+ }
+ if (ugear_msg_received){
+ parse_ugear_msg();
+ ugear_msg_received = FALSE;
+ if (gps_pos_available){
+ //gps_downlink();
+ gps_verbose_downlink = !launch;
+ UseGpsPosNoSend(estimator_update_state_gps);
+ gps_msg_received_counter = gps_msg_received_counter+1;
+ #ifdef GX2
+ if (gps_msg_received_counter == 1){
+ gps_send();
+ gps_msg_received_counter = 0;
+ }
+ #endif
+ #ifdef XSENSDL
+ if (gps_msg_received_counter == 25){
+ gps_send();
+ gps_msg_received_counter = 0;
+ }
+ #endif
+ gps_pos_available = FALSE;
+ }
+ }
+}
+
+
Copied: paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.h (from rev
6202, paparazzi3/trunk/sw/airborne/osam_imu_ugear.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.h 2010-10-22
21:51:01 UTC (rev 6203)
@@ -0,0 +1,95 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2003-2006 Haiyang Chao
+ *
+ * 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.
+ *
+ */
+
+/** \file osam_imu_ugear.h
+ * \brief Communication with any IMU through serial communication (originally
gps uart)
+ * This file is first generated by Haiyang Chao on 20080507 for communication.
+ */
+
+#include "std.h"
+
+void ugear_init( void );
+void parse_ugear(uint8_t c);
+void parse_ugear_msg( void );
+void ugear_event( void );
+
+#define __UgearLink(dev, _x) dev##_x
+#define _UgearLink(dev, _x) __UgearLink(dev, _x)
+#define UgearLink(_x) _UgearLink(UGEAR_LINK, _x)
+
+#define UgearBuffer() UgearLink(ChAvailable())
+#define ReadUgearBuffer() { while
(UgearLink(ChAvailable())&&!ugear_msg_received)
parse_ugear(UgearLink(Getch())); }
+
+#ifdef UGEAR_LED
+#define UgearToggleLed() LED_TOGGLE(UGEAR_LED)
+#else
+#define UgearToggleLed() {}
+#endif
+
+#define UGEAR_MAX_PAYLOAD 40
+/*#define UGEAR_MAX_PAYLOAD 24*/ /*Bugs!!! why work for the former version
??20080708*/
+/*#define GPS_NB_CHANNELS 16*/
+
+#define UGEAR_NAV_POSLLH_LAT(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload+4)|*((uint8_t*)_ubx_payload+1+4)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+4))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+4))<<24)
+#define UGEAR_NAV_POSLLH_LON(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload)|*((uint8_t*)_ubx_payload+1)<<8|((int32_t)*((uint8_t*)_ubx_payload+2))<<16|((int32_t)*((uint8_t*)_ubx_payload+3))<<24)
+#define UGEAR_NAV_POSLLH_HEIGHT(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload+8)|*((uint8_t*)_ubx_payload+1+8)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+8))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+8))<<24)
+#define UGEAR_NAV_POSLLH_VD(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload+12)|*((uint8_t*)_ubx_payload+1+12)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+12))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+12))<<24)
+#define UGEAR_NAV_VELNED_Heading(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload+16)|*((uint8_t*)_ubx_payload+1+16)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+16))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+16))<<24)
+#define UGEAR_NAV_SOL_Pacc(_ubx_payload)
(uint32_t)(*((uint8_t*)_ubx_payload+20)|*((uint8_t*)_ubx_payload+1+20)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+20))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+20))<<24)
+#define UGEAR_NAV_SOL_Sacc(_ubx_payload)
(uint32_t)(*((uint8_t*)_ubx_payload+24)|*((uint8_t*)_ubx_payload+1+24)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+24))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+24))<<24)
+#define UGEAR_NAV_VELNED_GSpeed(_ubx_payload)
(uint32_t)(*((uint8_t*)_ubx_payload+28)|*((uint8_t*)_ubx_payload+1+28)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+28))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+28))<<24)
+#define UGEAR_NAV_VELNED_ITOW(_ubx_payload)
(uint32_t)(*((uint8_t*)_ubx_payload+32)|*((uint8_t*)_ubx_payload+1+32)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+32))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+32))<<24)
+#define UGEAR_NAV_SOL_PDOP(_ubx_payload)
(uint16_t)(*((uint8_t*)_ubx_payload+36)|*((uint8_t*)_ubx_payload+1+36)<<8)
+#define UGEAR_NAV_SOL_numSV(_ubx_payload)
(uint8_t)(*((uint8_t*)_ubx_payload+38))
+#define UGEAR_IMU_PHI(_ubx_payload)
(int16_t)(*((uint8_t*)_ubx_payload)|*((uint8_t*)_ubx_payload+1)<<8)
+#define UGEAR_IMU_THE(_ubx_payload)
(int16_t)(*((uint8_t*)_ubx_payload+2)|*((uint8_t*)_ubx_payload+1+2)<<8)
+#define UGEAR_IMU_PSI(_ubx_payload)
(int16_t)(*((uint8_t*)_ubx_payload+4)|*((uint8_t*)_ubx_payload+1+4)<<8)
+
+extern volatile uint8_t ugear_msg_received;
+
+/* variable defined for dlsetting generated by Haiyang 20080717*/
+extern bool_t imu1_ir0;
+extern float fd_alpha;
+extern float fi_alpha;
+
+/*define the following varables for communication with ugear by haiyang
20080508*/
+extern int16_t ugear_phi;
+extern int16_t ugear_psi;
+extern int16_t ugear_theta;
+
+/* added 20080522 for debugging*/
+extern int16_t ugear_debug1;
+extern int16_t ugear_debug2;
+extern int16_t ugear_debug3;
+extern int32_t ugear_debug4;
+extern int32_t ugear_debug5;
+extern int32_t ugear_debug6;
+
+extern struct imu imupacket;
+extern struct gps gpspacket;
+extern uint8_t ugear_msg_buf[UGEAR_MAX_PAYLOAD];
+
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+
Deleted: paparazzi3/trunk/sw/airborne/osam_imu_ugear.c
===================================================================
--- paparazzi3/trunk/sw/airborne/osam_imu_ugear.c 2010-10-21 21:18:45 UTC
(rev 6202)
+++ paparazzi3/trunk/sw/airborne/osam_imu_ugear.c 2010-10-22 21:51:01 UTC
(rev 6203)
@@ -1,282 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2003-2006 Haiyang Chao
- *
- * 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.
- *
- */
-
-/** \file osam_imu_ugear.c
- * \brief Communication with any IMU through serial communication (originally
gps uart)
- * This file is first generated by Haiyang Chao on 20080507 for communication.
- */
-
-#include <stdlib.h>
-#include <string.h>
-
-#include "osam_imu_ugear.h"
-#include "gps.h"
-#include "latlong.h"
-#include "sys_time.h"
-#include "airframe.h"
-#include "nav.h"
-#include "estimator.h"
-#include "xsens_protocol.h"
-
-#define UNINIT 0
-#define GOT_SYNC1 1
-#define GOT_SYNC2 2
-#define GOT_ID 3
-#define GOT_LEN 4
-#define GOT_PAYLOAD 5
-#define GOT_CHECKSUM1 6
-#define GOT_CHECKSUM2 7
-
-#define UGEAR_SYNC1 0x93
-#define UGEAR_SYNC2 0xE0
-/*#define UGEAR_MAX_PAYLOAD 24*/
-#define UGEAR_MAX_PAYLOAD 40
-#define IMU_PACKET_SIZE 12
-#define RAD2DEG 57.3
-#define WrapUp(x) (x < 0 ? x+3600 : x)
-
-/* variable defined for dlsetting generated by Haiyang 20080717*/
-bool_t imu1_ir0 = 0;
-float fd_alpha = 0;
-float fi_alpha = 0;
-
-/* variable definition copied from gps_ubx.c 20080508*/
-uint16_t gps_week;
-uint32_t gps_itow;
-int32_t gps_alt;
-uint16_t gps_gspeed;
-int16_t gps_climb;
-int16_t gps_course;
-int32_t gps_utm_east, gps_utm_north;
-uint8_t gps_utm_zone;
-uint8_t gps_mode;
-volatile bool_t gps_msg_received;
-bool_t gps_pos_available;
-uint8_t ugear_id, ugear_class;
-int32_t gps_lat, gps_lon;
-uint16_t gps_reset;
-
-uint16_t gps_PDOP;
-uint32_t gps_Pacc, gps_Sacc;
-uint8_t gps_numSV;
-/* variable definition copied from gps_ubx.c20080508*/
-
-/* variable definition copied from gps.c20080515*/
-uint8_t gps_nb_ovrn;
-struct svinfo gps_svinfos[GPS_NB_CHANNELS];
-uint8_t gps_nb_channels;
-
-/* variable definition copied from gps.c20080515*/
-
-uint16_t last_gps_msg_t; /** cputime of the last gps message */
-
-volatile bool_t ugear_msg_received;
-
-static uint8_t ugear_status;
-static uint8_t ugear_type;
-static uint8_t ugear_len;
-static uint8_t ugear_msg_idx;
-static uint8_t ck_a, ck_b;
-
-int16_t ugear_phi;
-int16_t ugear_psi;
-int16_t ugear_theta;
-
-int16_t gps_ve;
-int16_t gps_vn;
-int16_t gps_vd;
-/* added 20080522 for debugging*/
-int16_t ugear_debug1;
-int16_t ugear_debug2;
-int16_t ugear_debug3;
-int32_t ugear_debug4;
-int32_t ugear_debug5;
-int32_t ugear_debug6;
-
-bool_t gps_verbose_downlink;
-
-uint8_t ugear_msg_buf[UGEAR_MAX_PAYLOAD] __attribute__ ((aligned));
-
-/*The following definition is for ugear data 20080509, modified on 0512
Haiyang*/
-struct imu {
- int32_t phi,the,psi; /* attitudes */
-};
-
-struct gps {
- int32_t lon,lat,alt;
- int16_t ve,vn,vd;
-};
-
-struct imu imupacket;
-struct gps gpspacket;
-/*The above definition is for ugear data 20080509 Haiyang*/
-
-void ugear_init( void ) {
- ugear_status = UNINIT;
- ugear_phi = 0;
- ugear_psi = 0;
- ugear_theta = 0;
- ugear_debug2 = 0;
-}
-
-void parse_ugear( uint8_t c ) {
- /*checksum go first*/
- if (ugear_status < GOT_PAYLOAD) {
- ck_a += c;
- ck_b += ck_a;
- }
- switch (ugear_status) {
- case UNINIT:
- if (c == UGEAR_SYNC1)
- ugear_status++;
- break;
- case GOT_SYNC1:
- if (c != UGEAR_SYNC2)
- goto error;
- ck_a = 0;
- ck_b = 0;
- ugear_status++;
- break;
- case GOT_SYNC2:
- if (ugear_msg_received) {
- /* Previous message has not yet been parsed: discard this one */
- goto error;
- }
- ugear_type = c;
- ugear_status++;
- //ugear_theta = 30; // for debug
- if (ugear_type > 2)
- goto restart;
- break;
- case GOT_ID:
- ugear_len = c;
- ugear_msg_idx = 0;
- ugear_status++;
- break;
- case GOT_LEN:
- ugear_msg_buf[ugear_msg_idx] = c;
- ugear_msg_idx++;
- if (ugear_msg_idx >= ugear_len) {
- ugear_status++;
- }
- break;
- case GOT_PAYLOAD:
- if (c != ck_a)
- goto error;
- ugear_status++;
- break;
- case GOT_CHECKSUM1:
- if (c != ck_b)
- goto error;
- ugear_msg_received = TRUE;
- UgearToggleLed();
- goto restart;
- break;
- }
- return;
-error:
-restart:
- ugear_status = UNINIT;
- return;
-}
-/*
-void decode_imupacket( struct imu *data, uint8_t* buffer){
-
- data->phi = (double)((((signed char)buffer[25])<<8)|buffer[26]);
- data->theta = (double)((((signed char)buffer[27])<<8)|buffer[28]);
- data->psi = (double)((((signed char)buffer[29])<<8)|buffer[30]);
-
-}
-*/
-void parse_ugear_msg( void ){
-
- switch (ugear_type){
- case 0: /*gps*/
- ugear_debug1 = ugear_debug1+1;
- gps_lat = UGEAR_NAV_POSLLH_LAT(ugear_msg_buf);
- gps_lon = UGEAR_NAV_POSLLH_LON(ugear_msg_buf);
-
- nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
- latlong_utm_of(RadOfDeg(gps_lat/1e7),
RadOfDeg(gps_lon/1e7), nav_utm_zone0);
- gps_utm_east = latlong_utm_x * 100;
- gps_utm_north = latlong_utm_y * 100;
-
- gps_alt = UGEAR_NAV_POSLLH_HEIGHT(ugear_msg_buf);
- gps_utm_zone = nav_utm_zone0;
-
- gps_gspeed = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
- gps_climb = - UGEAR_NAV_POSLLH_VD(ugear_msg_buf);
- gps_course =
UGEAR_NAV_VELNED_Heading(ugear_msg_buf)/10000; /*in decdegree */
- gps_PDOP = UGEAR_NAV_SOL_PDOP(ugear_msg_buf);
- gps_Pacc = UGEAR_NAV_SOL_Pacc(ugear_msg_buf);
- gps_Sacc = UGEAR_NAV_SOL_Sacc(ugear_msg_buf);
- gps_numSV = UGEAR_NAV_SOL_numSV(ugear_msg_buf);
- gps_week = 0; // FIXME
- gps_itow = UGEAR_NAV_VELNED_ITOW(ugear_msg_buf);
-
- //ugear_debug2 = gps_climb;
- //ugear_debug4 =
(int32_t)(UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf));
- //ugear_debug5 = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
- //ugear_debug6 = (int16_t)estimator_phi*100;
-
- gps_mode = 3; /*force GPSfix to be valided*/
- gps_pos_available = TRUE; /* The 3 UBX messages are
sent in one rafale */
- break;
- case 1: /*IMU*/
- ugear_debug2 = ugear_debug2+1;
- ugear_phi = UGEAR_IMU_PHI(ugear_msg_buf);
- ugear_psi = UGEAR_IMU_PSI(ugear_msg_buf);
- ugear_theta = UGEAR_IMU_THE(ugear_msg_buf);
- ugear_debug4 = (int32_t)ugear_phi;
- ugear_debug5 = (int32_t)ugear_theta;
- ugear_debug6 = (int32_t)ugear_psi;
- break;
- case 2: /*GPS status*/
-// ugear_debug1 = 2;
- gps_nb_channels = XSENS_GPSStatus_nch(ugear_msg_buf);
- uint8_t is;
- for(is = 0; is < Min(gps_nb_channels, 16); is++) {
- uint8_t ch =
XSENS_GPSStatus_chn(ugear_msg_buf,is);
- if (ch > 16) continue;
- gps_svinfos[ch].svid =
XSENS_GPSStatus_svid(ugear_msg_buf, is);
- gps_svinfos[ch].flags =
XSENS_GPSStatus_bitmask(ugear_msg_buf, is);
- gps_svinfos[ch].qi =
XSENS_GPSStatus_qi(ugear_msg_buf, is);
- gps_svinfos[ch].cno =
XSENS_GPSStatus_cnr(ugear_msg_buf, is);
- gps_svinfos[ch].elev = 0;
- gps_svinfos[ch].azim = 0;
- }
- break;
- case 3: /*servo*/
- break;
- case 4: /*health*/
- break;
-
- }
-
-}
-
-/* add the following function only to get rid of compilation error in
datalink.c 20080608 Haiyang*/
-void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) {
-}
-
Deleted: paparazzi3/trunk/sw/airborne/osam_imu_ugear.h
===================================================================
--- paparazzi3/trunk/sw/airborne/osam_imu_ugear.h 2010-10-21 21:18:45 UTC
(rev 6202)
+++ paparazzi3/trunk/sw/airborne/osam_imu_ugear.h 2010-10-22 21:51:01 UTC
(rev 6203)
@@ -1,93 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2003-2006 Haiyang Chao
- *
- * 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.
- *
- */
-
-/** \file osam_imu_ugear.h
- * \brief Communication with any IMU through serial communication (originally
gps uart)
- * This file is first generated by Haiyang Chao on 20080507 for communication.
- */
-
-#include "std.h"
-
-void ugear_init( void );
-void parse_ugear(uint8_t c);
-void parse_ugear_msg( void );
-
-#define __UgearLink(dev, _x) dev##_x
-#define _UgearLink(dev, _x) __UgearLink(dev, _x)
-#define UgearLink(_x) _UgearLink(UGEAR_LINK, _x)
-
-#define UgearBuffer() UgearLink(ChAvailable())
-#define ReadUgearBuffer() { while
(UgearLink(ChAvailable())&&!ugear_msg_received)
parse_ugear(UgearLink(Getch())); }
-
-#ifdef UGEAR_LED
-#define UgearToggleLed() LED_TOGGLE(UGEAR_LED)
-#else
-#define UgearToggleLed() {}
-#endif
-
-#define UGEAR_MAX_PAYLOAD 40
-/*#define UGEAR_MAX_PAYLOAD 24*/ /*Bugs!!! why work for the former version
??20080708*/
-/*#define GPS_NB_CHANNELS 16*/
-
-#define UGEAR_NAV_POSLLH_LAT(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload+4)|*((uint8_t*)_ubx_payload+1+4)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+4))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+4))<<24)
-#define UGEAR_NAV_POSLLH_LON(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload)|*((uint8_t*)_ubx_payload+1)<<8|((int32_t)*((uint8_t*)_ubx_payload+2))<<16|((int32_t)*((uint8_t*)_ubx_payload+3))<<24)
-#define UGEAR_NAV_POSLLH_HEIGHT(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload+8)|*((uint8_t*)_ubx_payload+1+8)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+8))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+8))<<24)
-#define UGEAR_NAV_POSLLH_VD(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload+12)|*((uint8_t*)_ubx_payload+1+12)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+12))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+12))<<24)
-#define UGEAR_NAV_VELNED_Heading(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload+16)|*((uint8_t*)_ubx_payload+1+16)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+16))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+16))<<24)
-#define UGEAR_NAV_SOL_Pacc(_ubx_payload)
(uint32_t)(*((uint8_t*)_ubx_payload+20)|*((uint8_t*)_ubx_payload+1+20)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+20))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+20))<<24)
-#define UGEAR_NAV_SOL_Sacc(_ubx_payload)
(uint32_t)(*((uint8_t*)_ubx_payload+24)|*((uint8_t*)_ubx_payload+1+24)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+24))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+24))<<24)
-#define UGEAR_NAV_VELNED_GSpeed(_ubx_payload)
(uint32_t)(*((uint8_t*)_ubx_payload+28)|*((uint8_t*)_ubx_payload+1+28)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+28))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+28))<<24)
-#define UGEAR_NAV_VELNED_ITOW(_ubx_payload)
(uint32_t)(*((uint8_t*)_ubx_payload+32)|*((uint8_t*)_ubx_payload+1+32)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+32))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+32))<<24)
-#define UGEAR_NAV_SOL_PDOP(_ubx_payload)
(uint16_t)(*((uint8_t*)_ubx_payload+36)|*((uint8_t*)_ubx_payload+1+36)<<8)
-#define UGEAR_NAV_SOL_numSV(_ubx_payload)
(uint8_t)(*((uint8_t*)_ubx_payload+38))
-#define UGEAR_IMU_PHI(_ubx_payload)
(int16_t)(*((uint8_t*)_ubx_payload)|*((uint8_t*)_ubx_payload+1)<<8)
-#define UGEAR_IMU_THE(_ubx_payload)
(int16_t)(*((uint8_t*)_ubx_payload+2)|*((uint8_t*)_ubx_payload+1+2)<<8)
-#define UGEAR_IMU_PSI(_ubx_payload)
(int16_t)(*((uint8_t*)_ubx_payload+4)|*((uint8_t*)_ubx_payload+1+4)<<8)
-
-extern volatile uint8_t ugear_msg_received;
-
-/* variable defined for dlsetting generated by Haiyang 20080717*/
-extern bool_t imu1_ir0;
-extern float fd_alpha;
-extern float fi_alpha;
-
-/*define the following varables for communication with ugear by haiyang
20080508*/
-extern int16_t ugear_phi;
-extern int16_t ugear_psi;
-extern int16_t ugear_theta;
-
-/* added 20080522 for debugging*/
-extern int16_t ugear_debug1;
-extern int16_t ugear_debug2;
-extern int16_t ugear_debug3;
-extern int32_t ugear_debug4;
-extern int32_t ugear_debug5;
-extern int32_t ugear_debug6;
-
-extern struct imu imupacket;
-extern struct gps gpspacket;
-extern uint8_t ugear_msg_buf[UGEAR_MAX_PAYLOAD];
-
-/* add the following function only to get rid of compilation error in
datalink.c 20080608 Haiyang*/
-void ubxsend_cfg_rst(uint16_t, uint8_t);
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6203] Make osam ugear a module,
Martin Mueller <=