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
ahrs_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  */
29 #include "subsystems/abi.h"
30 
32 
33 #if PERIODIC_TELEMETRY
35 
36 
37 static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
38 {
39  static uint16_t last_cnt = 0;
40  static uint16_t sec_cnt = 0;
41 
42  sec_cnt = ahrs_vn.vn_packet.counter - last_cnt;
43  ahrs_vn.vn_freq = sec_cnt; // update frequency counter
44 
45  pprz_msg_send_VECTORNAV_INFO(trans, dev, AC_ID,
49  &sec_cnt,
55  // update counter
56  last_cnt = ahrs_vn.vn_packet.counter;
57 
58  // reset mode
59  ahrs_vn.vn_data.mode = 0;
60 }
61 #endif
62 
67 {
68  // receive data
70 
71  // read message
75 
78  }
79 
80  // send ABI messages
81  uint32_t now_ts = get_sys_time_usec();
82  // in fixed point for sending as ABI and telemetry msgs
84  AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i);
86  AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i);
87  }
88 }
89 
90 
95 {
96  // Initialize variables
98  ahrs_vn.vn_freq = 0;
99 
100  // Initialize packet
109 
111 
112  // initialize data struct
113  memset(&(ahrs_vn.vn_data), 0, sizeof(struct VNData));
114 
115 #if PERIODIC_TELEMETRY
116  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VECTORNAV_INFO, send_vn_info);
117 #endif
118 }
119 
124 {
125  // Rates [rad/s]
126  static struct FloatRates body_rate;
127  float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ahrs_vn.body_to_imu), &ahrs_vn.vn_data.gyro); // compute body rates
128  stateSetBodyRates_f(&body_rate); // Set state [rad/s]
129 
130  // Attitude [deg]
131  static struct FloatQuat imu_quat; // convert from euler to quat
133  stateSetNedToBodyQuat_f(&imu_quat);
134 
135 }
136 
137 
#define IMU_VECTORNAV_ID
unsigned short uint16_t
Definition: types.h:16
float vn_freq
data frequency
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
uint16_t overrun_error
Definition: vn200_serial.h:70
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
uint8_t msg_idx
Definition: vn200_serial.h:68
float phi
in radians
void ahrs_vectornav_init(void)
Initialize Vectornav struct.
struct VNData vn_data
Data struct.
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
Periodic telemetry system header (includes downlink utility and generated code).
uint32_t chksm_error
Definition: vn200_serial.h:64
void ahrs_vectornav_event(void)
Event handling for Vectornav.
uint16_t counter
Definition: vn200_serial.h:75
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
#define INT32_VECT3_ZERO(_v)
Main include for ABI (AirBorneInterface).
uint32_t hdr_error
Definition: vn200_serial.h:65
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
float psi
in radians
uint16_t noise_error
Definition: vn200_serial.h:71
bool msg_available
Definition: vn200_serial.h:63
void vn200_event(struct VNPacket *vnp)
Definition: vn200_serial.c:110
enum VNStatus vn_status
VN status.
struct FloatEulers attitude
Attitude, float, [rad], yaw, pitch, roll.
Definition: vn200_serial.h:88
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
Roation quaternion.
struct Int32Vect3 accel_i
float theta
in radians
struct OrientationReps body_to_imu
body_to_imu rotation
void ahrs_vectornav_propagate(void)
Read received data.
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
Definition: vn200_serial.h:94
uint8_t mode
0-not tracking, 1 - poor performance, 2- OK
Definition: vn200_serial.h:96
struct FloatVect3 accel
Acceleration in the imu frame, m/s.
Definition: vn200_serial.h:89
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 FloatRates gyro
Rates in the imu frame m/s.
Definition: vn200_serial.h:90
struct Int32Rates gyro_i
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
uint16_t framing_error
Definition: vn200_serial.h:72
bool ahrs_vectornav_is_enabled(void)
struct AhrsVectornav ahrs_vn
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
Vectornav VN-200 as AHRS.
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
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
enum VNMsgStatus status
Definition: vn200_serial.h:67
angular rates
struct VNPacket vn_packet
Packet struct.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46