Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_xsens.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
27 #include "ins_xsens.h"
28 #include "xsens_common.h"
29 #include "subsystems/ins.h"
30 
31 #include "generated/airframe.h"
32 
33 #include "mcu_periph/sys_time.h"
34 #include "subsystems/abi.h"
35 #include "state.h"
36 
37 #if USE_GPS_XSENS
38 #if !USE_GPS
39 #error "USE_GPS needs to be 1 to use the Xsens GPS!"
40 #endif
41 #include "subsystems/gps.h"
43 #include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
44 #endif
45 
49 #ifndef INS_XSENS_GPS_ID
50 #define INS_XSENS_GPS_ID GPS_MULTI_ID
51 #endif
52 PRINT_CONFIG_VAR(INS_XSENS_GPS_ID)
54 
57 
58 static void handle_ins_msg(void);
59 static void update_state_interface(void);
60 static void gps_cb(uint8_t sender_id __attribute__((unused)),
61  uint32_t stamp __attribute__((unused)),
62  struct GpsState *gps_s);
63 
64 void ins_xsens_init(void)
65 {
66  xsens_init();
67 
68  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
69  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
70 
71  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
73  stateSetPositionUtm_f(&utm0);
74 
75  AbiBindMsgGPS(INS_XSENS_GPS_ID, &gps_ev, gps_cb);
76 }
77 
78 void ins_xsens_event(void)
79 {
80  xsens_event();
81  if (xsens.msg_received) {
84  xsens.msg_received = false;
85  }
86 }
87 
88 static void gps_cb(uint8_t sender_id __attribute__((unused)),
89  uint32_t stamp __attribute__((unused)),
90  struct GpsState *gps_s)
91 {
92  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
93  utm.alt = gps_s->hmsl / 1000.;
94 
95  // set position
97 
98  struct NedCoor_f ned_vel = {
99  gps_s->ned_vel.x / 100.,
100  gps_s->ned_vel.y / 100.,
101  gps_s->ned_vel.z / 100.
102  };
103  // set velocity
104  stateSetSpeedNed_f(&ned_vel);
105 }
106 
107 #if USE_GPS_XSENS
108 void gps_xsens_init(void)
109 {
110  xsens.gps.nb_channels = 0;
111 }
112 
113 static void gps_xsens_publish(void)
114 {
115  // publish gps data
116  uint32_t now_ts = get_sys_time_usec();
117  xsens.gps.last_msg_ticks = sys_time.nb_sec_rem;
118  xsens.gps.last_msg_time = sys_time.nb_sec;
119  if (xsens.gps.fix == GPS_FIX_3D) {
120  xsens.gps.last_3dfix_ticks = sys_time.nb_sec_rem;
121  xsens.gps.last_3dfix_time = sys_time.nb_sec;
122  }
123  AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &xsens.gps);
124 }
125 #endif
126 
127 
128 static void update_state_interface(void)
129 {
130  // Send to Estimator (Control)
131 #ifdef XSENS_BACKWARDS
132  struct FloatEulers att = {
135  xsens.euler.psi + RadOfDeg(180)
136  };
137  struct FloatEulerstRates rates = {
138  -xsens.gyro.p,
139  -xsens.gyro.q,
140  xsens.gyro.r
141  };
142 #else
143  struct FloatEulers att = {
146  xsens.euler.psi
147  };
148  struct FloatRates rates = xsens.gyro;
149 #endif
151  stateSetBodyRates_f(&rates);
152 }
153 
154 
155 static void handle_ins_msg(void)
156 {
157 
159 
160  if (xsens.new_attitude) {
161  new_ins_attitude = true;
162  xsens.new_attitude = false;
163  }
164 
165 #if USE_GPS_XSENS
166  if (xsens.gps_available) {
167  // Horizontal speed
168  float fspeed = FLOAT_VECT2_NORM(xsens.vel);
169  if (xsens.gps.fix != GPS_FIX_3D) {
170  fspeed = 0;
171  }
172  xsens.gps.gspeed = fspeed * 100.;
173  xsens.gps.speed_3d = float_vect3_norm(&xsens.vel) * 100;
174 
175  float fcourse = atan2f(xsens.vel.y, xsens.vel.x);
176  xsens.gps.course = fcourse * 1e7;
177  SetBit(xsens.gps.valid_fields, GPS_VALID_COURSE_BIT);
178 
179  gps_xsens_publish();
180  xsens.gps_available = false;
181  }
182 #endif // USE_GPS_XSENS
183 }
184 
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
void xsens_init(void)
Definition: xsens.c:114
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
float ins_roll_neutral
Definition: ins_xsens.c:56
Xsens as a full INS solution.
float phi
in radians
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
float ins_pitch_neutral
Definition: ins_xsens.c:55
float r
in rad/s
void parse_xsens_msg(void)
Definition: xsens.c:185
Main include for ABI (AirBorneInterface).
uint8_t nav_utm_zone0
Definition: common_nav.c:44
float psi
in radians
Integrated Navigation System interface.
position in UTM coordinates Units: meters
void xsens_event(void)
Definition: xsens_common.c:44
float q
in rad/s
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
float p
in rad/s
#define INS_ROLL_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:43
int32_t z
Down.
struct FloatVect3 vel
NED velocity in m/s.
Definition: xsens.h:58
#define INS_XSENS_GPS_ID
ABI binding for gps data.
Definition: ins_xsens.c:50
static abi_event gps_ev
Definition: ins_xsens.c:53
euler angles
void ins_xsens_event(void)
Definition: ins_xsens.c:78
int32_t y
East.
static float float_vect3_norm(struct FloatVect3 *v)
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
Paparazzi floating point math for geodetic calculations.
#define GPS_VALID_COURSE_BIT
Definition: gps.h:54
float theta
in radians
struct Xsens xsens
Definition: xsens.c:110
#define GPS_XSENS_ID
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
#define FLOAT_VECT2_NORM(_v)
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:88
int32_t nav_utm_north0
Definition: common_nav.c:43
vector in North East Down coordinates Units: meters
Architecture independent timing functions.
data structure for GPS information
Definition: gps.h:81
Device independent GPS code (interface)
int32_t x
North.
unsigned long uint32_t
Definition: types.h:18
#define INS_PITCH_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:46
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
static void update_state_interface(void)
Definition: ins_xsens.c:128
static void handle_ins_msg(void)
Definition: ins_xsens.c:155
unsigned char uint8_t
Definition: types.h:14
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
Convenience functions to get utm position from GPS state.
Definition: gps.c:393
API to get/set the generic vehicle states.
volatile uint8_t new_ins_attitude
volatile bool msg_received
Definition: xsens.h:63
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
int32_t nav_utm_east0
Definition: common_nav.c:42
struct FloatEulers euler
Definition: xsens.h:61
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_xsens.c:88
Parser for the Xsens protocol.
struct FloatRates gyro
Definition: xsens.h:53
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:90
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
angular rates
void ins_xsens_init(void)
Definition: ins_xsens.c:64
volatile bool new_attitude
Definition: xsens.h:64