Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
uint16_t
unsigned short uint16_t
Definition: types.h:16
imu_vectornav.h
imu_vectornav_periodic
void imu_vectornav_periodic(void)
Periodic function checks for the frequency of packets, triggers warning in case the IMU stops sending...
Definition: imu_vectornav.c:128
VNPacket::chksm_error
uint32_t chksm_error
Definition: vn200_serial.h:64
VNNotTracking
@ VNNotTracking
Definition: vn200_serial.h:79
VNPacket::overrun_error
uint16_t overrun_error
Definition: vn200_serial.h:70
VNPacket::counter
uint16_t counter
Definition: vn200_serial.h:75
imu_vectornav_propagate
void imu_vectornav_propagate(void)
Send ABI messages.
Definition: imu_vectornav.c:149
abi.h
VNPacket::msg_idx
uint8_t msg_idx
Definition: vn200_serial.h:68
Imu::accel
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
ImuVectornav::vn_packet
struct VNPacket vn_packet
Packet struct.
Definition: imu_vectornav.h:41
VNPacket::msg_available
bool msg_available
Definition: vn200_serial.h:63
VNPacket::hdr_error
uint32_t hdr_error
Definition: vn200_serial.h:65
VNMsgSync
@ VNMsgSync
Definition: vn200_serial.h:55
send_vn_info
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
Definition: imu_vectornav.c:50
uint32_t
unsigned long uint32_t
Definition: types.h:18
imu_vectornav_init
void imu_vectornav_init(void)
Init IMU struct and set up ABI messages.
Definition: imu_vectornav.c:80
VNPacket::noise_error
uint16_t noise_error
Definition: vn200_serial.h:71
vn200_event
void vn200_event(struct VNPacket *vnp)
Definition: vn200_serial.c:110
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
VNData::ypr_u
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
Definition: vn200_serial.h:94
telemetry.h
Imu
abstract IMU interface providing fixed point interface
Definition: imu.h:37
ACCELS_BFP_OF_REAL
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
VNData::accel
struct FloatVect3 accel
Acceleration in the imu frame, m/s.
Definition: vn200_serial.h:89
imu_vn
struct ImuVectornav imu_vn
Definition: imu_vectornav.c:40
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
RATES_BFP_OF_REAL
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
VNData::nanostamp
uint64_t nanostamp
Definition: vn200_serial.h:85
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
VNData::mode
uint8_t mode
0-not tracking, 1 - poor performance, 2- OK
Definition: vn200_serial.h:96
ImuVectornav::vn_status
enum VNStatus vn_status
VN status.
Definition: imu_vectornav.h:43
sys_time.h
Architecture independent timing functions.
imu_scale_gyro
void imu_scale_gyro(struct Imu *_imu)
Definition: imu_vectornav.c:43
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
IMU_VECTORNAV_ID
#define IMU_VECTORNAV_ID
Definition: abi_sender_ids.h:343
vn200_read_message
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
imu
struct Imu imu
global IMU state
Definition: imu.c:108
VNData::gyro
struct FloatRates gyro
Rates in the imu frame m/s.
Definition: vn200_serial.h:90
ImuVectornav::vn_data
struct VNData vn_data
Data struct.
Definition: imu_vectornav.h:42
VNPacket::framing_error
uint16_t framing_error
Definition: vn200_serial.h:72
ImuVectornav
Definition: imu_vectornav.h:39
VNOK
@ VNOK
Definition: vn200_serial.h:81
imu_vectornav_event
void imu_vectornav_event(void)
Event function to read and parse data from the serial port.
Definition: imu_vectornav.c:109
VNData::err
uint8_t err
see page 122 of VN-200 datasheet
Definition: vn200_serial.h:97
VNData::timestamp
float timestamp
Time since VN startup [s].
Definition: vn200_serial.h:86
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
ImuVectornav::vn_freq
float vn_freq
data frequency
Definition: imu_vectornav.h:44
VNData
Definition: vn200_serial.h:84
imu_scale_accel
void imu_scale_accel(struct Imu *_imu)
Definition: imu_vectornav.c:44
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
uint64_t
unsigned long long uint64_t
Definition: types.h:20
Imu::gyro
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38
VNPacket::status
enum VNMsgStatus status
Definition: vn200_serial.h:67