Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ins_vectornav.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015 Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
3 * Utah State University, http://aggieair.usu.edu/
4 *
5 * This file is part of paparazzi.
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, write to
19 * the Free Software Foundation, 59 Temple Place - Suite 330,
20 * Boston, MA 02111-1307, USA.
21 */
31
33
34#if PERIODIC_TELEMETRY
36
37static void send_ins(struct transport_tx *trans, struct link_device *dev)
38{
39 pprz_msg_send_INS(trans, dev, AC_ID,
40 &ins_vn.ltp_pos_i.x, &ins_vn.ltp_pos_i.y, &ins_vn.ltp_pos_i.z,
41 &ins_vn.ltp_speed_i.x, &ins_vn.ltp_speed_i.y, &ins_vn.ltp_speed_i.z,
42 &ins_vn.ltp_accel_i.x, &ins_vn.ltp_accel_i.y, &ins_vn.ltp_accel_i.z);
43}
44
45static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
46{
47 pprz_msg_send_INS_Z(trans, dev, AC_ID,
48 &ins_vn.baro_z, &ins_vn.ltp_pos_i.z, &ins_vn.ltp_speed_i.z, &ins_vn.ltp_accel_i.z);
49}
50
51static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
52{
53 if (ins_vn.ltp_initialized) {
54 pprz_msg_send_INS_REF(trans, dev, AC_ID,
55 &ins_vn.ltp_def.ecef.x, &ins_vn.ltp_def.ecef.y, &ins_vn.ltp_def.ecef.z,
56 &ins_vn.ltp_def.lla.lat, &ins_vn.ltp_def.lla.lon, &ins_vn.ltp_def.lla.alt,
57 &ins_vn.ltp_def.hmsl, &ins_vn.qfe);
58 }
59}
60
61static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
62{
63 pprz_msg_send_VECTORNAV_INFO(trans, dev, AC_ID,
64 &ins_vn.vn_data.timestamp,
65 &ins_vn.vn_packet.chksm_error,
66 &ins_vn.vn_packet.hdr_error,
67 &ins_vn.vn_rate,
68 &ins_vn.vn_data.mode,
69 &ins_vn.vn_data.err,
70 &ins_vn.vn_data.ypr_u.phi,
71 &ins_vn.vn_data.ypr_u.theta,
72 &ins_vn.vn_data.ypr_u.psi);
73}
74
75static void send_accel(struct transport_tx *trans, struct link_device *dev)
76{
78 pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &id,
79 &ins_vn.vn_data.accel.x, &ins_vn.vn_data.accel.y, &ins_vn.vn_data.accel.z);
80}
81
82static void send_gyro(struct transport_tx *trans, struct link_device *dev)
83{
85 pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &id,
86 &ins_vn.vn_data.gyro.p, &ins_vn.vn_data.gyro.q, &ins_vn.vn_data.gyro.r);
87}
88
89static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
90{
92 pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &id,
93 &ins_vn.accel_i.x, &ins_vn.accel_i.y, &ins_vn.accel_i.z);
94}
95
96static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
97{
99 pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &id,
100 &ins_vn.gyro_i.p, &ins_vn.gyro_i.q, &ins_vn.gyro_i.r);
101}
102#endif
103
104#ifndef USE_INS_NAV_INIT
105#define USE_INS_NAV_INIT TRUE
106PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE");
107#endif
108
109
117{
118 static uint16_t last_cnt = 0;
119 static uint16_t sec_cnt = 0;
120
121 sec_cnt = ins_vn.vn_packet.counter - last_cnt;
122 ins_vn.vn_rate = sec_cnt; // update frequency counter
123
124 // we want at least 75% of periodic frequency to be able to control the airfcraft
125 if (ins_vn.vn_rate < (PERIODIC_FREQUENCY*0.75)) {
126 gps.fix = GPS_FIX_NONE;
127 }
128
129 // Make gps pacc available in GPS page on GCS
130 static uint32_t last_pacc = 0;
131 // update only if pacc changes
132 if (last_pacc != gps.pacc) {
133 last_pacc = gps.pacc;
134 // we don't know the value of CNO, hence oscillate
135 // between 0 and 1 to not confuse the user
136 gps.svinfos[0].cno = (gps.svinfos[0].cno + 1) % 2;
137 }
138
139 // update counter
140 last_cnt = ins_vn.vn_packet.counter;
141
142 // reset mode
143 ins_vn.vn_data.mode = 0;
144}
145
146
151{
152 // receive data
153 vn200_event(&(ins_vn.vn_packet));
154
155 // read message
156 if (ins_vn.vn_packet.msg_available) {
157 vn200_read_message(&(ins_vn.vn_packet),&(ins_vn.vn_data));
158 ins_vn.vn_packet.msg_available = false;
160 }
161}
162
163
168{
169 // Initialize variables
170 ins_vn.vn_status = VNNotTracking;
171 ins_vn.vn_rate = 0;
172
173 // Initialize packet
174 ins_vn.vn_packet.status = VNMsgSync;
175 ins_vn.vn_packet.msg_idx = 0;
176 ins_vn.vn_packet.msg_available = false;
177 ins_vn.vn_packet.chksm_error = 0;
178 ins_vn.vn_packet.hdr_error = 0;
179 ins_vn.vn_packet.overrun_error = 0;
180 ins_vn.vn_packet.noise_error = 0;
181 ins_vn.vn_packet.framing_error = 0;
182
183 INT32_VECT3_ZERO(ins_vn.ltp_pos_i);
184 INT32_VECT3_ZERO(ins_vn.ltp_speed_i);
185 INT32_VECT3_ZERO(ins_vn.ltp_accel_i);
186 INT32_VECT3_ZERO(ins_vn.accel_i);
187
188 // initialize data struct
189 memset(&(ins_vn.vn_data), 0, sizeof(struct VNData));
190
191#if USE_INS_NAV_INIT
192 ins_init_origin_i_from_flightplan(MODULE_INS_VECTORNAV_ID, &ins_vn.ltp_def);
193 ins_vn.ltp_initialized = true;
194#else
195 ins_vn.ltp_initialized = false;
196#endif
197
198 struct FloatEulers body_to_imu_eulers =
200 orientationSetEulers_f(&ins_vn.body_to_imu, &body_to_imu_eulers);
201
202#if PERIODIC_TELEMETRY
206 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VECTORNAV_INFO, send_vn_info);
209 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
210 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_SCALED, send_gyro_scaled);
211#endif
212}
213
214
215
216
222{
223 gps.sacc = (uint32_t)(ins_vn.vn_data.vel_u * 100);
224}
225
231{
232 float pacc = ins_vn.vn_data.pos_u[0]; // in meters
233 if (ins_vn.vn_data.pos_u[1] > pacc) {
234 pacc = ins_vn.vn_data.pos_u[1];
235 }
236 if (ins_vn.vn_data.pos_u[2] > pacc) {
237 pacc = ins_vn.vn_data.pos_u[2];
238 }
239 gps.pacc = (uint32_t)(pacc * 100);
240}
241
242
243
249{
250 // Acceleration [m/s^2]
251 // in fixed point for sending as ABI and telemetry msgs
252 ACCELS_BFP_OF_REAL(ins_vn.accel_i, ins_vn.vn_data.accel);
253
254 // Rates [rad/s]
255 static struct FloatRates body_rate;
256 // in fixed point for sending as ABI and telemetry msgs
257 RATES_BFP_OF_REAL(ins_vn.gyro_i, ins_vn.vn_data.gyro);
258 float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ins_vn.body_to_imu), &ins_vn.vn_data.gyro); // compute body rates
259 stateSetBodyRates_f(MODULE_INS_VECTORNAV_ID, &body_rate); // Set state [rad/s]
260
261 // Attitude [deg]
262 static struct FloatQuat imu_quat; // convert from euler to quat
263 float_quat_of_eulers(&imu_quat, &ins_vn.vn_data.attitude);
264 static struct FloatRMat imu_rmat; // convert from quat to rmat
265 float_rmat_of_quat(&imu_rmat, &imu_quat);
266 static struct FloatRMat ltp_to_body_rmat; // rotate to body frame
267 float_rmat_comp(&ltp_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu));
268 stateSetNedToBodyRMat_f(MODULE_INS_VECTORNAV_ID, &ltp_to_body_rmat); // set body states [rad]
269
270 // NED (LTP) velocity [m/s]
271 // North east down (NED), also known as local tangent plane (LTP),
272 // is a geographical coordinate system for representing state vectors that is commonly used in aviation.
273 // It consists of three numbers: one represents the position along the northern axis,
274 // one along the eastern axis, and one represents vertical position. Down is chosen as opposed to
275 // up in order to comply with the right-hand rule.
276 // The origin of this coordinate system is usually chosen to be the aircraft's center of gravity.
277 // x = North
278 // y = East
279 // z = Down
280 stateSetSpeedNed_f(MODULE_INS_VECTORNAV_ID, &ins_vn.vn_data.vel_ned); // set state
281
282 // NED (LTP) acceleration [m/s^2]
283 static struct FloatVect3 accel_meas_ltp;// first we need to rotate linear acceleration from imu-frame to body-frame
284 float_rmat_transp_vmult(&accel_meas_ltp, orientationGetRMat_f(&ins_vn.body_to_imu), &(ins_vn.vn_data.lin_accel));
285 static struct NedCoor_f ltp_accel; // assign to NedCoord_f struct
286 VECT3_ASSIGN(ltp_accel, accel_meas_ltp.x, accel_meas_ltp.y, accel_meas_ltp.z);
287 stateSetAccelNed_f(MODULE_INS_VECTORNAV_ID, &ltp_accel); // then set the states
288 ins_vn.ltp_accel_f = ltp_accel;
289
290 // LLA position [rad, rad, m]
291 //static struct LlaCoor_f lla_pos; // convert from deg to rad, and from double to float
292 ins_vn.lla_pos.lat = RadOfDeg((float)ins_vn.vn_data.pos_lla[0]); // ins_impl.pos_lla[0] = lat
293 ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.vn_data.pos_lla[1]); // ins_impl.pos_lla[1] = lon
294 ins_vn.lla_pos.alt = ((float)ins_vn.vn_data.pos_lla[2]); // ins_impl.pos_lla[2] = alt
295 LLA_BFP_OF_REAL(gps.lla_pos, ins_vn.lla_pos);
296 SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
297 stateSetPositionLla_i(MODULE_INS_VECTORNAV_ID, &gps.lla_pos);
298
299 // ECEF velocity
300 // TODO: properly implement
301
302 // ECEF position
303 gps.ecef_pos.x = stateGetPositionEcef_i()->x;
304 gps.ecef_pos.y = stateGetPositionEcef_i()->y;
305 gps.ecef_pos.z = stateGetPositionEcef_i()->z;
306 SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
307
308 // GPS Ground speed
309 float speed = sqrt(ins_vn.vn_data.vel_ned.x * ins_vn.vn_data.vel_ned.x + ins_vn.vn_data.vel_ned.y * ins_vn.vn_data.vel_ned.y);
310 gps.gspeed = ((uint16_t)(speed * 100));
311
312 // GPS course
313 gps.course = (int32_t)(1e7 * (atan2(ins_vn.vn_data.vel_ned.y, ins_vn.vn_data.vel_ned.x)));
314 SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
315
316 // Because we have not HMSL data from Vectornav, we are using LLA-Altitude
317 // as a workaround
318 float geoid_h = wgs84_ellipsoid_to_geoid_f(ins_vn.lla_pos.lat, ins_vn.lla_pos.lon);
319 gps.hmsl = (int32_t)((ins_vn.lla_pos.alt - geoid_h)* 1000.0f);
320 SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
321
322 // Set GPS fix
323 gps.fix = ins_vn.vn_data.gps_fix;
324
325 // Set GPS num_sv
326 gps.num_sv = ins_vn.vn_data.num_sv;
327
328 // set position uncertainty
330
331 // set velocity uncertainty
333
334 // check GPS status
335 gps.last_msg_time = sys_time.nb_sec;
336 gps.last_msg_ticks = sys_time.nb_sec_rem;
337 if (gps.fix == GPS_FIX_3D) {
338 gps.last_3dfix_time = sys_time.nb_sec;
339 gps.last_3dfix_ticks = sys_time.nb_sec_rem;
340 }
341
342 // update internal states for telemetry purposes
343 // TODO: directly convert vectornav output instead of using state interface
344 // to support multiple INS running at the same time
345 ins_vn.ltp_pos_i = *stateGetPositionNed_i();
346 ins_vn.ltp_speed_i = *stateGetSpeedNed_i();
347 ins_vn.ltp_accel_i = *stateGetAccelNed_i();
348}
#define IMU_VECTORNAV_ID
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
static struct uart_periph * dev
struct GpsState gps
global GPS state
Definition gps.c:74
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
#define GPS_FIX_NONE
No GPS fix.
Definition gps.h:42
#define GPS_VALID_POS_ECEF_BIT
Definition gps.h:48
#define GPS_VALID_HMSL_BIT
Definition gps.h:53
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
#define GPS_VALID_COURSE_BIT
Definition gps.h:54
void float_rmat_ratemult(struct FloatRates *rb, const struct FloatRMat *m_a2b, const struct FloatRates *ra)
rotate anglular rates by rotation matrix.
void float_quat_of_eulers(struct FloatQuat *q, const struct FloatEulers *e)
quat of euler roation 'ZYX'
void float_rmat_of_quat(struct FloatRMat *rm, const struct FloatQuat *q)
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_rmat_comp(struct FloatRMat *m_a2c, const struct FloatRMat *m_a2b, const struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
euler angles
Roation quaternion.
rotation matrix
angular rates
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define RATES_BFP_OF_REAL(_ri, _rf)
#define ACCELS_BFP_OF_REAL(_ef, _ei)
#define INT32_VECT3_ZERO(_v)
int32_t z
in centimeters
int32_t x
in centimeters
int32_t y
in centimeters
#define LLA_BFP_OF_REAL(_o, _i)
static float wgs84_ellipsoid_to_geoid_f(float lat, float lon)
Get WGS84 ellipsoid/geoid separation.
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
static void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition state.h:1166
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition state.h:1196
static void stateSetNedToBodyRMat_f(uint16_t id, struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition state.h:1279
static struct EcefCoor_i * stateGetPositionEcef_i(void)
Get position in ECEF coordinates (int).
Definition state.h:796
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition state.h:805
static void stateSetPositionLla_i(uint16_t id, struct LlaCoor_i *lla_pos)
Set position from LLA coordinates (int).
Definition state.h:669
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 struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition state.h:1015
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:272
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:286
static void send_accel(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:242
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
Definition imu.c:228
void ins_init_origin_i_from_flightplan(uint16_t id, struct LtpDef_i *ltp_def)
initialize the local origin (ltp_def in fixed point) from flight plan position
Definition ins.c:33
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:563
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:608
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:589
void ins_vectornav_monitor(void)
Monitors vectornav data rate and changes GPS lock if the data rate is too low.
static void send_ins(struct transport_tx *trans, struct link_device *dev)
void ins_vectornav_set_pacc(void)
Find maximum uncertainty (NED) position accuracy in cm.
struct InsVectornav ins_vn
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
void ins_vectornav_set_sacc(void)
Set speed (velocity) uncertainty (NED) speed accuracy in cm/s.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
void ins_vectornav_init(void)
Initialize Vectornav struct.
void ins_vectornav_event(void)
Event handling for Vectornav.
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
static void send_accel(struct transport_tx *trans, struct link_device *dev)
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
void ins_vectornav_propagate()
Propagate the received states into the vehicle state machine.
Vectornav VN-200 INS module.
#define INS_VN_BODY_TO_IMU_PHI
#define INS_VN_BODY_TO_IMU_PSI
#define INS_VN_BODY_TO_IMU_THETA
vector in North East Down coordinates Units: meters
WGS-84 Geoid Heights.
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
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
void vn200_read_message(struct VNPacket *vn_packet, struct VNData *vn_data)
Read received message and populate data struct with new measurements.
void vn200_event(struct VNPacket *vnp)
@ VNNotTracking
@ VNMsgSync