Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
39static 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,
48 &ahrs_vn.vn_data.timestamp,
49 &ahrs_vn.vn_packet.chksm_error,
50 &ahrs_vn.vn_packet.hdr_error,
51 &sec_cnt,
52 &ahrs_vn.vn_data.mode,
53 &ahrs_vn.vn_data.err,
54 &ahrs_vn.vn_data.ypr_u.phi,
55 &ahrs_vn.vn_data.ypr_u.theta,
56 &ahrs_vn.vn_data.ypr_u.psi);
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
71 vn200_event(&(ahrs_vn.vn_packet));
72
73 // read message
74 if (ahrs_vn.vn_packet.msg_available) {
75 vn200_read_message(&(ahrs_vn.vn_packet),&(ahrs_vn.vn_data));
76 ahrs_vn.vn_packet.msg_available = false;
77
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
83 RATES_BFP_OF_REAL(ahrs_vn.gyro_i, ahrs_vn.vn_data.gyro);
84 AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i, 1, ahrs_vn.vn_freq, NAN);
85 ACCELS_BFP_OF_REAL(ahrs_vn.accel_i, ahrs_vn.vn_data.accel);
86 AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i, 1, ahrs_vn.vn_freq, NAN);
87 }
88}
89
90
95{
96 // Initialize variables
97 ahrs_vn.vn_status = VNNotTracking;
98 ahrs_vn.vn_freq = 0;
99
100 // Initialize packet
101 ahrs_vn.vn_packet.status = VNMsgSync;
102 ahrs_vn.vn_packet.msg_idx = 0;
103 ahrs_vn.vn_packet.msg_available = false;
104 ahrs_vn.vn_packet.chksm_error = 0;
105 ahrs_vn.vn_packet.hdr_error = 0;
106 ahrs_vn.vn_packet.overrun_error = 0;
107 ahrs_vn.vn_packet.noise_error = 0;
108 ahrs_vn.vn_packet.framing_error = 0;
109
110 INT32_VECT3_ZERO(ahrs_vn.accel_i);
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(MODULE_AHRS_VECTORNAV_ID, &body_rate); // Set state [rad/s]
129
130 // Attitude [deg]
131 static struct FloatQuat imu_quat; // convert from euler to quat
132 float_quat_of_eulers(&imu_quat, &ahrs_vn.vn_data.attitude);
133 stateSetNedToBodyQuat_f(MODULE_AHRS_VECTORNAV_ID, &imu_quat);
134
135}
136
137
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.
Vectornav VN-200 as AHRS.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static struct uart_periph * dev
void float_rmat_ratemult(struct FloatRates *rb, const struct FloatRMat *m_a2b, const struct FloatRates *ra)
rotate anglular rates by rotation matrix.
void float_quat_of_eulers(struct FloatQuat *q, const struct FloatEulers *e)
quat of euler roation 'ZYX'
Roation quaternion.
angular rates
#define RATES_BFP_OF_REAL(_ri, _rf)
#define ACCELS_BFP_OF_REAL(_ef, _ei)
#define INT32_VECT3_ZERO(_v)
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
static void stateSetNedToBodyQuat_f(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition state.h:1272
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1371
API to get/set the generic vehicle states.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
void vn200_read_message(struct VNPacket *vn_packet, struct VNData *vn_data)
Read received message and populate data struct with new measurements.
void vn200_event(struct VNPacket *vnp)
@ VNNotTracking
@ VNMsgSync