Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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  */
30 #include "modules/core/abi.h"
31 #include "state.h"
32 
33 struct AhrsVectornav ahrs_vn;
34 
35 #if PERIODIC_TELEMETRY
37 
38 
39 static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
40 {
41  static uint16_t last_cnt = 0;
42  static uint16_t sec_cnt = 0;
43 
44  sec_cnt = ahrs_vn.vn_packet.counter - last_cnt;
45  ahrs_vn.vn_freq = sec_cnt; // update frequency counter
46 
47  pprz_msg_send_VECTORNAV_INFO(trans, dev, AC_ID,
51  &sec_cnt,
57  // update counter
58  last_cnt = ahrs_vn.vn_packet.counter;
59 
60  // reset mode
61  ahrs_vn.vn_data.mode = 0;
62 }
63 #endif
64 
69 {
70  // receive data
72 
73  // read message
77 
80  }
81 
82  // send ABI messages
83  uint32_t now_ts = get_sys_time_usec();
84  // in fixed point for sending as ABI and telemetry msgs
86  AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i, 1, ahrs_vn.vn_freq, NAN);
88  AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i, 1, ahrs_vn.vn_freq, NAN);
89  }
90 }
91 
92 
97 {
98  // Initialize variables
100  ahrs_vn.vn_freq = 0;
101 
102  // Initialize packet
111 
113 
114  // initialize data struct
115  memset(&(ahrs_vn.vn_data), 0, sizeof(struct VNData));
116 
117 #if PERIODIC_TELEMETRY
118  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VECTORNAV_INFO, send_vn_info);
119 #endif
120 }
121 
126 {
127  // Rates [rad/s]
128  static struct FloatRates body_rate;
129  float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ahrs_vn.body_to_imu), &ahrs_vn.vn_data.gyro); // compute body rates
130  stateSetBodyRates_f(&body_rate); // Set state [rad/s]
131 
132  // Attitude [deg]
133  static struct FloatQuat imu_quat; // convert from euler to quat
135  stateSetNedToBodyQuat_f(&imu_quat);
136 
137 }
138 
139 
Main include for ABI (AirBorneInterface).
#define IMU_VECTORNAV_ID
void ahrs_vectornav_event(void)
Event handling for Vectornav.
void ahrs_vectornav_propagate(void)
Read received data.
struct AhrsVectornav ahrs_vn
void ahrs_vectornav_init(void)
Initialize Vectornav struct.
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
Vectornav VN-200 as AHRS.
enum VNStatus vn_status
VN status.
struct Int32Vect3 accel_i
float vn_freq
data frequency
struct OrientationReps body_to_imu
body_to_imu rotation
struct VNData vn_data
Data struct.
struct Int32Rates gyro_i
struct VNPacket vn_packet
Packet struct.
bool ahrs_vectornav_is_enabled(void)
Vectornav VN-200 as AHRS.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
float phi
in radians
float theta
in radians
float psi
in radians
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
Roation quaternion.
angular rates
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
#define INT32_VECT3_ZERO(_v)
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
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:184
void vn200_event(struct VNPacket *vnp)
Definition: vn200_serial.c:113
uint16_t framing_error
Definition: vn200_serial.h:71
uint32_t hdr_error
Definition: vn200_serial.h:64
uint8_t err
see page 122 of VN-200 datasheet
Definition: vn200_serial.h:96
@ VNNotTracking
Definition: vn200_serial.h:78
uint16_t overrun_error
Definition: vn200_serial.h:69
struct FloatRates gyro
Rates in the imu frame m/s.
Definition: vn200_serial.h:89
struct FloatEulers attitude
Attitude, float, [rad], yaw, pitch, roll.
Definition: vn200_serial.h:87
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
Definition: vn200_serial.h:93
struct FloatVect3 accel
Acceleration in the imu frame, m/s.
Definition: vn200_serial.h:88
uint8_t mode
0-not tracking, 1 - poor performance, 2- OK
Definition: vn200_serial.h:95
float timestamp
Time since VN startup [s].
Definition: vn200_serial.h:85
@ VNMsgSync
Definition: vn200_serial.h:54
uint16_t counter
Definition: vn200_serial.h:74
bool msg_available
Definition: vn200_serial.h:62
uint16_t noise_error
Definition: vn200_serial.h:70
uint32_t chksm_error
Definition: vn200_serial.h:63
uint8_t msg_idx
Definition: vn200_serial.h:67
enum VNMsgStatus status
Definition: vn200_serial.h:66