Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
27
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
56
59
60static void handle_ins_msg(void);
61static void update_state_interface(void);
62static void gps_cb(uint8_t sender_id __attribute__((unused)),
63 uint32_t stamp __attribute__((unused)),
64 struct GpsState *gps_s);
65
67{
69
72
74 stateSetLocalUtmOrigin_f(MODULE_INS_XSENS700_ID, &utm0);
75 stateSetPositionUtm_f(MODULE_INS_XSENS700_ID, &utm0);
76
77 AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb);
78}
79
81{
83 if (xsens700.parser.msg_received) {
86 xsens700.parser.msg_received = false;
87 }
88}
89
90static 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
97 stateSetPositionUtm_f(MODULE_INS_XSENS700_ID, &utm);
98
99 struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
100 // set velocity
101 stateSetSpeedNed_f(MODULE_INS_XSENS700_ID, &ned_vel);
102}
103
104
105#if USE_GPS_XSENS
106void gps_xsens700_init(void)
107{
108 xsens700.gps.nb_channels = 0;
109}
110
111static 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
126static void update_state_interface(void)
127{
128 // Send to Estimator (Control)
129#ifdef XSENS_BACKWARDS
130 struct FloatEulers att = {
131 xsens700.euler.phi + ins_roll_neutral,
132 -xsens700.euler.theta + ins_pitch_neutral,
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 = {
142 -xsens700.euler.phi + ins_roll_neutral,
143 xsens700.euler.theta + ins_pitch_neutral,
144 -xsens700.euler.psi
145 };
146 struct FloatRates rates = {
147 -xsens700.gyro.p,
148 xsens700.gyro.q,
149 -xsens700.gyro.r
150 };
151#endif
152 stateSetNedToBodyEulers_f(MODULE_INS_XSENS700_ID, &att);
153 stateSetBodyRates_f(MODULE_INS_XSENS700_ID, &rates);
154}
155
156
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).
struct abi_struct abi_event
Definition abi_common.h:101
#define GPS_XSENS_ID
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event gps_ev
#define INS_PITCH_NEUTRAL_DEFAULT
Definition ahrs_sim.c:46
#define INS_ROLL_NEUTRAL_DEFAULT
Definition ahrs_sim.c:43
float ins_roll_neutral
Definition ins_arduimu.c:14
float ins_pitch_neutral
Definition ins_arduimu.c:15
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
int32_t nav_utm_east0
Definition common_nav.c:48
uint8_t nav_utm_zone0
Definition common_nav.c:50
int32_t nav_utm_north0
Definition common_nav.c:49
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:591
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:558
Device independent GPS code (interface).
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
#define GPS_VALID_COURSE_BIT
Definition gps.h:54
data structure for GPS information
Definition gps.h:90
static float float_vect3_norm(const struct FloatVect3 *v)
#define FLOAT_VECT2_NORM(_v)
euler angles
angular rates
static void stateSetNedToBodyEulers_f(uint16_t id, struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition state.h:1286
static void stateSetPositionUtm_f(uint16_t id, struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition state.h:709
static void stateSetLocalUtmOrigin_f(uint16_t id, struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition state.h:552
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1371
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition state.h:958
static void handle_ins_msg(void)
Definition imu_xsens.c:53
Integrated Navigation System interface.
volatile uint8_t new_ins_attitude
static void handle_ins_msg(void)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
void ins_xsens700_init(void)
static void update_state_interface(void)
#define INS_XSENS700_GPS_ID
ABI binding for gps data.
void ins_xsens700_event(void)
Xsens700 as a full INS solution.
static void update_state_interface(void)
Definition ins_xsens.c:124
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
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:80
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition sys_time.h:81
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
void xsens700_init(void)
Definition xsens700.c:53
void parse_xsens700_msg(void)
Definition xsens700.c:141
struct Xsens xsens700
Definition xsens700.c:49
void xsens_parser_event(struct XsensParser *xsensparser)