Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
imu_vectornav.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
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 
30 
31 // Systime
32 #include "mcu_periph/sys_time.h"
33 
34 // Abi
35 #include "subsystems/abi.h"
36 
37 // Generated
38 #include "generated/airframe.h"
39 
41 
42 /* no scaling */
43 void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
44 void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
45 
46 #if PERIODIC_TELEMETRY
48 
49 
50 static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
51 {
52  static uint16_t last_cnt = 0;
53  static uint16_t sec_cnt = 0;
54 
55  sec_cnt = imu_vn.vn_packet.counter - last_cnt;
56  imu_vn.vn_freq = sec_cnt; // update frequency counter
57 
58  pprz_msg_send_VECTORNAV_INFO(trans, dev, AC_ID,
62  &sec_cnt,
68  // update counter
69  last_cnt = imu_vn.vn_packet.counter;
70 
71  // reset mode
72  imu_vn.vn_data.mode = 0;
73 }
74 #endif
75 
76 
81 {
82  // Initialize variables
84  imu_vn.vn_freq = 0;
85 
86  // Initialize packet
95 
96  // initialize data structs
97  memset(&(imu_vn.vn_data), 0, sizeof(struct VNData));
98 
99 #if PERIODIC_TELEMETRY
100  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VECTORNAV_INFO, send_vn_info);
101 #endif
102 }
103 
104 
105 
110 {
111  // receive data
113 
114  // read message
119  }
120 }
121 
122 
129 {
130  static uint16_t last_cnt = 0;
131  static uint16_t sec_cnt = 0;
132 
133  sec_cnt = imu_vn.vn_packet.counter - last_cnt;
134  imu_vn.vn_freq = sec_cnt; // update frequency counter
135 
136  // we want at least 75% of periodic frequency to be able to control the airfcraft
137  if (imu_vn.vn_freq >= (PERIODIC_FREQUENCY*0.75)) {
139  }
140  else {
142  }
143 }
144 
145 
150 {
151  uint64_t tmp = imu_vn.vn_data.nanostamp / 1000;
152  uint32_t now_ts = (uint32_t) tmp;
153 
154  // Rates and accel
157 
158  // Send accel and gyro data separate for other AHRS algorithms
159  AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &imu.gyro);
160  AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &imu.accel);
161 }
162 
#define IMU_VECTORNAV_ID
unsigned short uint16_t
Definition: types.h:16
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
uint16_t overrun_error
Definition: vn200_serial.h:70
uint8_t msg_idx
Definition: vn200_serial.h:68
float phi
in radians
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
Definition: imu_vectornav.c:50
Periodic telemetry system header (includes downlink utility and generated code).
float vn_freq
data frequency
Definition: imu_vectornav.h:44
uint32_t chksm_error
Definition: vn200_serial.h:64
uint16_t counter
Definition: vn200_serial.h:75
Main include for ABI (AirBorneInterface).
Vectornav VN-200 IMU subsystems, to be used with other AHRS/INS algorithms.
uint32_t hdr_error
Definition: vn200_serial.h:65
float psi
in radians
uint16_t noise_error
Definition: vn200_serial.h:71
bool msg_available
Definition: vn200_serial.h:63
struct Imu imu
global IMU state
Definition: imu.c:108
void vn200_event(struct VNPacket *vnp)
Definition: vn200_serial.c:110
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
float theta
in radians
uint64_t nanostamp
Definition: vn200_serial.h:85
enum VNStatus vn_status
VN status.
Definition: imu_vectornav.h:43
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
Definition: vn200_serial.h:94
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
unsigned long long uint64_t
Definition: types.h:20
uint8_t mode
0-not tracking, 1 - poor performance, 2- OK
Definition: vn200_serial.h:96
Architecture independent timing functions.
struct FloatVect3 accel
Acceleration in the imu frame, m/s.
Definition: vn200_serial.h:89
struct VNPacket vn_packet
Packet struct.
Definition: imu_vectornav.h:41
unsigned long uint32_t
Definition: types.h:18
void vn200_read_message(struct VNPacket *vn_packet, struct VNData *vn_data)
Read received message and populate data struct with new measurements.
Definition: vn200_serial.c:180
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
struct ImuVectornav imu_vn
Definition: imu_vectornav.c:40
struct FloatRates gyro
Rates in the imu frame m/s.
Definition: vn200_serial.h:90
void imu_scale_gyro(struct Imu *_imu)
Definition: imu_vectornav.c:43
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
void imu_vectornav_init(void)
Init IMU struct and set up ABI messages.
Definition: imu_vectornav.c:80
uint16_t framing_error
Definition: vn200_serial.h:72
void imu_vectornav_event(void)
Event function to read and parse data from the serial port.
void imu_vectornav_propagate(void)
Send ABI messages.
void imu_scale_accel(struct Imu *_imu)
Definition: imu_vectornav.c:44
struct VNData vn_data
Data struct.
Definition: imu_vectornav.h:42
abstract IMU interface providing fixed point interface
Definition: imu.h:37
float timestamp
Time since VN startup [s].
Definition: vn200_serial.h:86
uint8_t err
see page 122 of VN-200 datasheet
Definition: vn200_serial.h:97
enum VNMsgStatus status
Definition: vn200_serial.h:67
void imu_vectornav_periodic(void)
Periodic function checks for the frequency of packets, triggers warning in case the IMU stops sending...
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38