Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_xsens700.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Christophe De Wagter
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 
28 #include "ins_xsens700.h"
29 #include "modules/ins/ins.h"
30 
31 #include "generated/airframe.h"
32 
33 #include "mcu_periph/sys_time.h"
35 #include "pprzlink/messages.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 "modules/gps/gps.h"
42 #include "modules/core/abi.h"
45 #include "modules/nav/common_nav.h" /* needed for nav_utm_zone0 */
46 #endif
47 
51 #ifndef INS_XSENS700_GPS_ID
52 #define INS_XSENS700_GPS_ID GPS_MULTI_ID
53 #endif
54 PRINT_CONFIG_VAR(INS_XSENS700_GPS_ID)
56 
59 
60 static void handle_ins_msg(void);
61 static void update_state_interface(void);
62 static void gps_cb(uint8_t sender_id __attribute__((unused)),
63  uint32_t stamp __attribute__((unused)),
64  struct GpsState *gps_s);
65 
67 {
68  xsens700_init();
69 
72 
73  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
75  stateSetPositionUtm_f(&utm0);
76 
77  AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb);
78 }
79 
81 {
87  }
88 }
89 
90 static void gps_cb(uint8_t sender_id __attribute__((unused)),
91  uint32_t stamp __attribute__((unused)),
92  struct GpsState *gps_s)
93 {
94  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
95 
96  // set position
98 
99  struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
100  // set velocity
101  stateSetSpeedNed_f(&ned_vel);
102 }
103 
104 
105 #if USE_GPS_XSENS
106 void gps_xsens700_init(void)
107 {
108  xsens700.gps.nb_channels = 0;
109 }
110 
111 static void gps_xsens700_publish(void)
112 {
113  // publish gps data
114  uint32_t now_ts = get_sys_time_usec();
115  xsens700.gps.last_msg_ticks = sys_time.nb_sec_rem;
116  xsens700.gps.last_msg_time = sys_time.nb_sec;
117  if (xsens700.gps.fix == GPS_FIX_3D) {
118  xsens700.gps.last_3dfix_ticks = sys_time.nb_sec_rem;
119  xsens700.gps.last_3dfix_time = sys_time.nb_sec;
120  }
121  AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &xsens700.gps);
122 }
123 #endif
124 
125 
126 static void update_state_interface(void)
127 {
128  // Send to Estimator (Control)
129 #ifdef XSENS_BACKWARDS
130  struct FloatEulers att = {
133  -xsens700.euler.psi + RadOfDeg(180)
134  };
135  struct FloatEulerstRates rates = {
136  xsens700.gyro.p,
137  -xsens700.gyro.q,
138  -xsens700.gyro.r
139  };
140 #else
141  struct FloatEulers att = {
145  };
146  struct FloatRates rates = {
147  -xsens700.gyro.p,
148  xsens700.gyro.q,
149  -xsens700.gyro.r
150  };
151 #endif
153  stateSetBodyRates_f(&rates);
154 }
155 
156 
157 void handle_ins_msg(void)
158 {
159 
161 
162  if (xsens700.new_attitude) {
163 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
164  new_ins_attitude = true;
165 #endif
166  xsens700.new_attitude = false;
167  }
168 
169 #if USE_GPS_XSENS
170  if (xsens700.gps_available) {
171  // Horizontal speed
172  float fspeed = FLOAT_VECT2_NORM(xsens700.vel);
173  if (xsens700.gps.fix != GPS_FIX_3D) {
174  fspeed = 0;
175  }
176  xsens700.gps.gspeed = fspeed * 100.;
177  xsens700.gps.speed_3d = float_vect3_norm(&xsens700.vel) * 100;
178 
179  float fcourse = atan2f(xsens700.vel.y, xsens700.vel.x);
180  xsens700.gps.course = fcourse * 1e7;
181  SetBit(xsens700.gps.valid_fields, GPS_VALID_COURSE_BIT);
182 
183  gps_xsens700_publish();
184  xsens700.gps_available = false;
185  }
186 #endif // USE_GPS_XSENS
187 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
#define GPS_XSENS_ID
#define INS_PITCH_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:46
#define INS_ROLL_NEUTRAL_DEFAULT
Definition: ahrs_sim.c:43
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
int32_t nav_utm_east0
Definition: common_nav.c:43
uint8_t nav_utm_zone0
Definition: common_nav.c:45
int32_t nav_utm_north0
Definition: common_nav.c:44
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:569
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:536
Device independent GPS code (interface)
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
#define GPS_VALID_COURSE_BIT
Definition: gps.h:52
data structure for GPS information
Definition: gps.h:85
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
static float float_vect3_norm(struct FloatVect3 *v)
#define FLOAT_VECT2_NORM(_v)
euler angles
angular rates
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
Integrated Navigation System interface.
volatile uint8_t new_ins_attitude
static void handle_ins_msg(void)
Definition: ins_xsens700.c:157
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_xsens700.c:90
void ins_xsens700_init(void)
Definition: ins_xsens700.c:66
float ins_roll_neutral
Definition: ins_xsens700.c:58
static void update_state_interface(void)
Definition: ins_xsens700.c:126
#define INS_XSENS700_GPS_ID
ABI binding for gps data.
Definition: ins_xsens700.c:52
float ins_pitch_neutral
Definition: ins_xsens700.c:57
void ins_xsens700_event(void)
Definition: ins_xsens700.c:80
static abi_event gps_ev
Definition: ins_xsens700.c:55
Xsens700 as a full INS solution.
Paparazzi floating point math for geodetic calculations.
vector in North East Down coordinates Units: meters
position in UTM coordinates Units: meters
WGS-84 Geoid Heights.
Architecture independent timing functions.
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
void xsens700_init(void)
Definition: xsens700.c:53
void parse_xsens700_msg(void)
Definition: xsens700.c:141
struct Xsens xsens700
Definition: xsens700.c:49
struct XsensParser parser
Definition: xsens.h:66
struct FloatEulers euler
Definition: xsens.h:64
struct FloatVect3 vel
NED velocity in m/s.
Definition: xsens.h:61
struct FloatRates gyro
Definition: xsens.h:56
volatile bool new_attitude
Definition: xsens.h:67
void xsens_parser_event(struct XsensParser *xsensparser)
Definition: xsens_parser.c:35
volatile uint8_t msg_received
Definition: xsens_parser.h:47