Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/ins/ins.h"
29 
30 #include "generated/airframe.h"
31 
32 #include "mcu_periph/sys_time.h"
33 #include "modules/core/abi.h"
34 #include "state.h"
35 
36 #if USE_GPS_XSENS
37 #if !USE_GPS
38 #error "USE_GPS needs to be 1 to use the Xsens GPS!"
39 #endif
40 #include "modules/gps/gps.h"
42 #include "modules/nav/common_nav.h" /* needed for nav_utm_zone0 */
43 #endif
44 
48 #ifndef INS_XSENS_GPS_ID
49 #define INS_XSENS_GPS_ID GPS_MULTI_ID
50 #endif
51 PRINT_CONFIG_VAR(INS_XSENS_GPS_ID)
53 
56 
57 static void handle_ins_msg(void);
58 static void update_state_interface(void);
59 static void gps_cb(uint8_t sender_id __attribute__((unused)),
60  uint32_t stamp __attribute__((unused)),
61  struct GpsState *gps_s);
62 
63 void ins_xsens_init(void)
64 {
65  xsens_init();
66 
69 
70  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
72  stateSetPositionUtm_f(&utm0);
73 
74  AbiBindMsgGPS(INS_XSENS_GPS_ID, &gps_ev, gps_cb);
75 }
76 
77 void ins_xsens_event(void)
78 {
84  }
85 }
86 
87 static void gps_cb(uint8_t sender_id __attribute__((unused)),
88  uint32_t stamp __attribute__((unused)),
89  struct GpsState *gps_s)
90 {
91  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
92  utm.alt = gps_s->hmsl / 1000.;
93 
94  // set position
96 
97  struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
98  // set velocity
99  stateSetSpeedNed_f(&ned_vel);
100 }
101 
102 #if USE_GPS_XSENS
103 void gps_xsens_init(void)
104 {
105  xsens.gps.nb_channels = 0;
106 }
107 
108 static void gps_xsens_publish(void)
109 {
110  // publish gps data
111  uint32_t now_ts = get_sys_time_usec();
112  xsens.gps.last_msg_ticks = sys_time.nb_sec_rem;
113  xsens.gps.last_msg_time = sys_time.nb_sec;
114  if (xsens.gps.fix == GPS_FIX_3D) {
115  xsens.gps.last_3dfix_ticks = sys_time.nb_sec_rem;
116  xsens.gps.last_3dfix_time = sys_time.nb_sec;
117  }
118  AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &xsens.gps);
119 }
120 #endif
121 
122 
123 static void update_state_interface(void)
124 {
125  // Send to Estimator (Control)
126 #ifdef XSENS_BACKWARDS
127  struct FloatEulers att = {
130  xsens.euler.psi + RadOfDeg(180)
131  };
132  struct FloatEulerstRates rates = {
133  -xsens.gyro.p,
134  -xsens.gyro.q,
135  xsens.gyro.r
136  };
137 #else
138  struct FloatEulers att = {
141  xsens.euler.psi
142  };
143  struct FloatRates rates = xsens.gyro;
144 #endif
146  stateSetBodyRates_f(&rates);
147 }
148 
149 
150 static void handle_ins_msg(void)
151 {
152 
154 
155  if (xsens.new_attitude) {
156 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
157  new_ins_attitude = true;
158 #endif
159  xsens.new_attitude = false;
160  }
161 
162 #if USE_GPS_XSENS
163  if (xsens.gps_available) {
164  // Horizontal speed
165  float fspeed = FLOAT_VECT2_NORM(xsens.vel);
166  if (xsens.gps.fix != GPS_FIX_3D) {
167  fspeed = 0;
168  }
169  xsens.gps.gspeed = fspeed * 100.;
170  xsens.gps.speed_3d = float_vect3_norm(&xsens.vel) * 100;
171 
172  float fcourse = atan2f(xsens.vel.y, xsens.vel.x);
173  xsens.gps.course = fcourse * 1e7;
174  SetBit(xsens.gps.valid_fields, GPS_VALID_COURSE_BIT);
175 
176  gps_xsens_publish();
177  xsens.gps_available = false;
178  }
179 #endif // USE_GPS_XSENS
180 }
181 
update_state_interface
static void update_state_interface(void)
Definition: ins_xsens.c:123
ins.h
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
Xsens::vel
struct FloatVect3 vel
NED velocity in m/s.
Definition: xsens.h:61
GPS_VALID_COURSE_BIT
#define GPS_VALID_COURSE_BIT
Definition: gps.h:57
xsens_parser_event
void xsens_parser_event(struct XsensParser *xsensparser)
Definition: xsens_parser.c:35
stateSetSpeedNed_f
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_xsens.c:87
parse_xsens_msg
void parse_xsens_msg(void)
Definition: xsens.c:184
gps_ev
static abi_event gps_ev
Definition: ins_xsens.c:52
ins_xsens_init
void ins_xsens_init(void)
Definition: ins_xsens.c:63
INS_PITCH_NEUTRAL_DEFAULT
#define INS_PITCH_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:46
GPS_XSENS_ID
#define GPS_XSENS_ID
Definition: abi_sender_ids.h:252
ins_xsens_event
void ins_xsens_event(void)
Definition: ins_xsens.c:77
xsens
struct Xsens xsens
Definition: xsens.c:109
abi.h
nav_utm_east0
int32_t nav_utm_east0
Definition: common_nav.c:43
Xsens::new_attitude
volatile bool new_attitude
Definition: xsens.h:67
Xsens::euler
struct FloatEulers euler
Definition: xsens.h:64
INS_ROLL_NEUTRAL_DEFAULT
#define INS_ROLL_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:43
GpsState
data structure for GPS information
Definition: gps.h:90
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
ins_roll_neutral
float ins_roll_neutral
Definition: ins_xsens.c:55
common_nav.h
ned_vel_float_from_gps
struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ned velocity (float) Converted on the fly if not available.
Definition: gps.c:506
stateSetPositionUtm_f
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
nav_utm_north0
int32_t nav_utm_north0
Definition: common_nav.c:44
xsens_init
void xsens_init(void)
Definition: xsens.c:113
XsensParser::msg_received
volatile uint8_t msg_received
Definition: xsens_parser.h:47
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
stateSetLocalUtmOrigin_f
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
pprz_geodetic_float.h
Paparazzi floating point math for geodetic calculations.
float_vect3_norm
static float float_vect3_norm(struct FloatVect3 *v)
Definition: pprz_algebra_float.h:171
gps.h
Device independent GPS code (interface)
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
handle_ins_msg
static void handle_ins_msg(void)
Definition: ins_xsens.c:150
ins_pitch_neutral
float ins_pitch_neutral
Definition: ins_xsens.c:54
UtmCoor_f::alt
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_float.h:84
FLOAT_VECT2_NORM
#define FLOAT_VECT2_NORM(_v)
Definition: pprz_algebra_float.h:132
stateSetBodyRates_f
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
sys_time.h
Architecture independent timing functions.
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
Xsens::parser
struct XsensParser parser
Definition: xsens.h:66
INS_XSENS_GPS_ID
#define INS_XSENS_GPS_ID
ABI binding for gps data.
Definition: ins_xsens.c:49
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
utm_float_from_gps
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:539
ins_xsens.h
Xsens::gyro
struct FloatRates gyro
Definition: xsens.h:56
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
sys_time
Definition: sys_time.h:71
UtmCoor_f
position in UTM coordinates Units: meters
Definition: pprz_geodetic_float.h:81
sys_time::nb_sec_rem
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
nav_utm_zone0
uint8_t nav_utm_zone0
Definition: common_nav.c:45
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:97
stateSetNedToBodyEulers_f
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
state.h
FALSE
#define FALSE
Definition: std.h:5
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
sys_time::nb_sec
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
new_ins_attitude
volatile uint8_t new_ins_attitude
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
FloatRates
angular rates
Definition: pprz_algebra_float.h:93