Paparazzi UAS  v6.2_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 
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_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i);
88  AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i);
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 
AhrsVectornav::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ahrs_vectornav.h:60
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
VNPacket::chksm_error
uint32_t chksm_error
Definition: vn200_serial.h:64
VNNotTracking
@ VNNotTracking
Definition: vn200_serial.h:79
ahrs_vectornav_is_enabled
bool ahrs_vectornav_is_enabled(void)
Definition: ahrs_vectornav_wrapper.c:62
VNPacket::overrun_error
uint16_t overrun_error
Definition: vn200_serial.h:70
VNPacket::counter
uint16_t counter
Definition: vn200_serial.h:75
ahrs_vectornav_wrapper.h
abi.h
ahrs_vectornav.h
VNPacket::msg_idx
uint8_t msg_idx
Definition: vn200_serial.h:68
VNPacket::msg_available
bool msg_available
Definition: vn200_serial.h:63
AhrsVectornav
Definition: ahrs_vectornav.h:47
VNPacket::hdr_error
uint32_t hdr_error
Definition: vn200_serial.h:65
VNMsgSync
@ VNMsgSync
Definition: vn200_serial.h:55
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
VNData::attitude
struct FloatEulers attitude
Attitude, float, [rad], yaw, pitch, roll.
Definition: vn200_serial.h:88
orientationGetRMat_f
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Definition: pprz_orientation_conversion.h:234
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
ahrs_vn
struct AhrsVectornav ahrs_vn
Definition: ahrs_vectornav.c:33
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
VNData::ypr_u
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
Definition: vn200_serial.h:94
INT32_VECT3_ZERO
#define INT32_VECT3_ZERO(_v)
Definition: pprz_algebra_int.h:289
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
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
AhrsVectornav::vn_freq
float vn_freq
data frequency
Definition: ahrs_vectornav.h:52
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
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stateSetBodyRates_f
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
VNData::mode
uint8_t mode
0-not tracking, 1 - poor performance, 2- OK
Definition: vn200_serial.h:96
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:51
IMU_VECTORNAV_ID
#define IMU_VECTORNAV_ID
Definition: abi_sender_ids.h:359
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
send_vn_info
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_vectornav.c:39
VNData::gyro
struct FloatRates gyro
Rates in the imu frame m/s.
Definition: vn200_serial.h:90
AhrsVectornav::vn_status
enum VNStatus vn_status
VN status.
Definition: ahrs_vectornav.h:51
AhrsVectornav::vn_data
struct VNData vn_data
Data struct.
Definition: ahrs_vectornav.h:50
AhrsVectornav::vn_packet
struct VNPacket vn_packet
Packet struct.
Definition: ahrs_vectornav.h:49
VNPacket::framing_error
uint16_t framing_error
Definition: vn200_serial.h:72
ahrs_vectornav_init
void ahrs_vectornav_init(void)
Initialize Vectornav struct.
Definition: ahrs_vectornav.c:96
AhrsVectornav::accel_i
struct Int32Vect3 accel_i
Definition: ahrs_vectornav.h:56
float_rmat_ratemult
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
Definition: pprz_algebra_float.c:150
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
ahrs_vectornav_event
void ahrs_vectornav_event(void)
Event handling for Vectornav.
Definition: ahrs_vectornav.c:68
state.h
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
ahrs_vectornav_propagate
void ahrs_vectornav_propagate(void)
Read received data.
Definition: ahrs_vectornav.c:125
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
AhrsVectornav::gyro_i
struct Int32Rates gyro_i
Definition: ahrs_vectornav.h:57
VNData
Definition: vn200_serial.h:84
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
stateSetNedToBodyQuat_f
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
VNPacket::status
enum VNMsgStatus status
Definition: vn200_serial.h:67
float_quat_of_eulers
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
Definition: pprz_algebra_float.c:477