Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_vectornav.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
3  * Utah State University, http://aggieair.usu.edu/
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
31 
33 
34 #if PERIODIC_TELEMETRY
36 
37 static void send_ins(struct transport_tx *trans, struct link_device *dev)
38 {
39  pprz_msg_send_INS(trans, dev, AC_ID,
43 }
44 
45 static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
46 {
47  pprz_msg_send_INS_Z(trans, dev, AC_ID,
49 }
50 
51 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
52 {
53  if (ins_vn.ltp_initialized) {
54  pprz_msg_send_INS_REF(trans, dev, AC_ID,
58  }
59 }
60 
61 static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
62 {
63  pprz_msg_send_VECTORNAV_INFO(trans, dev, AC_ID,
67  &ins_vn.vn_rate,
73 }
74 
75 static void send_accel(struct transport_tx *trans, struct link_device *dev)
76 {
77  pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID,
79 }
80 
81 static void send_gyro(struct transport_tx *trans, struct link_device *dev)
82 {
83  pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
85 }
86 
87 static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
88 {
89  pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID,
91 }
92 
93 static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
94 {
95  pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID,
97 }
98 #endif
99 
100 #ifndef USE_INS_NAV_INIT
101 #define USE_INS_NAV_INIT TRUE
102 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE");
103 #endif
104 
105 
113 {
114  static uint16_t last_cnt = 0;
115  static uint16_t sec_cnt = 0;
116 
117  sec_cnt = ins_vn.vn_packet.counter - last_cnt;
118  ins_vn.vn_rate = sec_cnt; // update frequency counter
119 
120  // we want at least 75% of periodic frequency to be able to control the airfcraft
121  if (ins_vn.vn_rate < (PERIODIC_FREQUENCY*0.75)) {
122  gps.fix = GPS_FIX_NONE;
123  }
124 
125  // Make gps pacc available in GPS page on GCS
126  static uint32_t last_pacc = 0;
127  // update only if pacc changes
128  if (last_pacc != gps.pacc) {
129  last_pacc = gps.pacc;
130  // we don't know the value of CNO, hence oscillate
131  // between 0 and 1 to not confuse the user
132  gps.svinfos[0].cno = (gps.svinfos[0].cno + 1) % 2;
133  }
134 
135  // update counter
136  last_cnt = ins_vn.vn_packet.counter;
137 
138  // reset mode
139  ins_vn.vn_data.mode = 0;
140 }
141 
142 
147 {
148  // receive data
150 
151  // read message
156  }
157 }
158 
159 
164 {
165  // Initialize variables
167  ins_vn.vn_rate = 0;
168 
169  // Initialize packet
178 
183 
184  // initialize data struct
185  memset(&(ins_vn.vn_data), 0, sizeof(struct VNData));
186 
187 #if USE_INS_NAV_INIT
189  ins_vn.ltp_initialized = true;
190 #else
191  ins_vn.ltp_initialized = false;
192 #endif
193 
194  struct FloatEulers body_to_imu_eulers =
196  orientationSetEulers_f(&ins_vn.body_to_imu, &body_to_imu_eulers);
197 
198 #if PERIODIC_TELEMETRY
202  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VECTORNAV_INFO, send_vn_info);
203  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL, send_accel);
204  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO, send_gyro);
205  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
206  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_SCALED, send_gyro_scaled);
207 #endif
208 }
209 
210 
211 
212 
218 {
219  gps.sacc = (uint32_t)(ins_vn.vn_data.vel_u * 100);
220 }
221 
227 {
228  float pacc = ins_vn.vn_data.pos_u[0]; // in meters
229  if (ins_vn.vn_data.pos_u[1] > pacc) {
230  pacc = ins_vn.vn_data.pos_u[1];
231  }
232  if (ins_vn.vn_data.pos_u[2] > pacc) {
233  pacc = ins_vn.vn_data.pos_u[2];
234  }
235  gps.pacc = (uint32_t)(pacc * 100);
236 }
237 
238 
239 
245 {
246  // Acceleration [m/s^2]
247  // in fixed point for sending as ABI and telemetry msgs
249 
250  // Rates [rad/s]
251  static struct FloatRates body_rate;
252  // in fixed point for sending as ABI and telemetry msgs
254  float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ins_vn.body_to_imu), &ins_vn.vn_data.gyro); // compute body rates
255  stateSetBodyRates_f(&body_rate); // Set state [rad/s]
256 
257  // Attitude [deg]
258  static struct FloatQuat imu_quat; // convert from euler to quat
260  static struct FloatRMat imu_rmat; // convert from quat to rmat
261  float_rmat_of_quat(&imu_rmat, &imu_quat);
262  static struct FloatRMat ltp_to_body_rmat; // rotate to body frame
263  float_rmat_comp(&ltp_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu));
264  stateSetNedToBodyRMat_f(&ltp_to_body_rmat); // set body states [rad]
265 
266  // NED (LTP) velocity [m/s]
267  // North east down (NED), also known as local tangent plane (LTP),
268  // is a geographical coordinate system for representing state vectors that is commonly used in aviation.
269  // It consists of three numbers: one represents the position along the northern axis,
270  // one along the eastern axis, and one represents vertical position. Down is chosen as opposed to
271  // up in order to comply with the right-hand rule.
272  // The origin of this coordinate system is usually chosen to be the aircraft's center of gravity.
273  // x = North
274  // y = East
275  // z = Down
276  stateSetSpeedNed_f(&ins_vn.vn_data.vel_ned); // set state
277 
278  // NED (LTP) acceleration [m/s^2]
279  static struct FloatVect3 accel_meas_ltp;// first we need to rotate linear acceleration from imu-frame to body-frame
281  static struct NedCoor_f ltp_accel; // assign to NedCoord_f struct
282  VECT3_ASSIGN(ltp_accel, accel_meas_ltp.x, accel_meas_ltp.y, accel_meas_ltp.z);
283  stateSetAccelNed_f(&ltp_accel); // then set the states
284  ins_vn.ltp_accel_f = ltp_accel;
285 
286  // LLA position [rad, rad, m]
287  //static struct LlaCoor_f lla_pos; // convert from deg to rad, and from double to float
288  ins_vn.lla_pos.lat = RadOfDeg((float)ins_vn.vn_data.pos_lla[0]); // ins_impl.pos_lla[0] = lat
289  ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.vn_data.pos_lla[1]); // ins_impl.pos_lla[1] = lon
290  ins_vn.lla_pos.alt = ((float)ins_vn.vn_data.pos_lla[2]); // ins_impl.pos_lla[2] = alt
294 
295  // ECEF velocity
296  // TODO: properly implement
297 
298  // ECEF position
303 
304  // GPS Ground speed
306  gps.gspeed = ((uint16_t)(speed * 100));
307 
308  // GPS course
309  gps.course = (int32_t)(1e7 * (atan2(ins_vn.vn_data.vel_ned.y, ins_vn.vn_data.vel_ned.x)));
311 
312  // Because we have not HMSL data from Vectornav, we are using LLA-Altitude
313  // as a workaround
315  gps.hmsl = (int32_t)((ins_vn.lla_pos.alt - geoid_h)* 1000.0f);
317 
318  // Set GPS fix
320 
321  // Set GPS num_sv
323 
324  // set position uncertainty
326 
327  // set velocity uncertainty
329 
330  // check GPS status
333  if (gps.fix == GPS_FIX_3D) {
336  }
337 
338  // update internal states for telemetry purposes
339  // TODO: directly convert vectornav output instead of using state interface
340  // to support multiple INS running at the same time
344 }
ins_vectornav_set_pacc
void ins_vectornav_set_pacc(void)
Find maximum uncertainty (NED) position accuracy in cm.
Definition: ins_vectornav.c:226
send_vn_info
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:61
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
InsVectornav::gyro_i
struct Int32Rates gyro_i
Definition: ins_vectornav.h:87
GpsState::valid_fields
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
send_accel
static void send_accel(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:75
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
LLA_BFP_OF_REAL
#define LLA_BFP_OF_REAL(_o, _i)
Definition: pprz_geodetic_int.h:171
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
GPS_VALID_COURSE_BIT
#define GPS_VALID_COURSE_BIT
Definition: gps.h:54
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
send_gyro
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:81
VNPacket::counter
uint16_t counter
Definition: vn200_serial.h:75
stateSetSpeedNed_f
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
send_gyro_scaled
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:93
INS_VN_BODY_TO_IMU_PSI
#define INS_VN_BODY_TO_IMU_PSI
Definition: ins_vectornav.h:59
InsVectornav::ltp_pos_i
struct NedCoor_i ltp_pos_i
Definition: ins_vectornav.h:69
ins_vectornav.h
VNPacket::msg_idx
uint8_t msg_idx
Definition: vn200_serial.h:68
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
VNData::vel_ned
struct NedCoor_f vel_ned
The estimated velocity in the North East Down (NED) frame, given in m/s.
Definition: vn200_serial.h:103
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
VNPacket::msg_available
bool msg_available
Definition: vn200_serial.h:63
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
VNPacket::hdr_error
uint32_t hdr_error
Definition: vn200_serial.h:65
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition: pprz_geodetic_int.h:99
VNMsgSync
@ VNMsgSync
Definition: vn200_serial.h:55
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
GpsState::pacc
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
LlaCoor_f::lon
float lon
in radians
Definition: pprz_geodetic_float.h:56
INS_VN_BODY_TO_IMU_PHI
#define INS_VN_BODY_TO_IMU_PHI
Definition: ins_vectornav.h:57
GpsState::sacc
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
ins_vectornav_set_sacc
void ins_vectornav_set_sacc(void)
Set speed (velocity) uncertainty (NED) speed accuracy in cm/s.
Definition: ins_vectornav.c:217
VNData::gps_fix
uint8_t gps_fix
None|2D|3D.
Definition: vn200_serial.h:101
stateGetPositionNed_i
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
VNPacket::noise_error
uint16_t noise_error
Definition: vn200_serial.h:71
InsVectornav::vn_rate
uint16_t vn_rate
data frequency
Definition: ins_vectornav.h:83
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
InsVectornav::lla_pos
struct LlaCoor_f lla_pos
Definition: ins_vectornav.h:72
stateSetNedToBodyRMat_f
static void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition: state.h:1099
ins_vectornav_monitor
void ins_vectornav_monitor(void)
Monitors vectornav data rate and changes GPS lock if the data rate is too low.
Definition: ins_vectornav.c:112
orientationGetRMat_f
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Definition: pprz_orientation_conversion.h:234
VNData::lin_accel
struct FloatVect3 lin_accel
Linear acceleration in imu frame [m/s^2].
Definition: vn200_serial.h:93
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
GPS_VALID_POS_LLA_BIT
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:49
VNData::num_sv
uint8_t num_sv
number of visible satellites
Definition: vn200_serial.h:100
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
VNData::ypr_u
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
Definition: vn200_serial.h:94
FloatVect3
Definition: pprz_algebra_float.h:54
float_rmat_transp_vmult
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_float.c:120
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
INT32_VECT3_ZERO
#define INT32_VECT3_ZERO(_v)
Definition: pprz_algebra_int.h:289
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
GpsState::last_msg_ticks
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:116
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
SVinfo::cno
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:81
InsVectornav::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ins_vectornav.h:90
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
VNData::pos_u
float pos_u[3]
The current GPS position uncertainty in the North East Down (NED) coordinate frame,...
Definition: vn200_serial.h:91
ins_vectornav_propagate
void ins_vectornav_propagate()
Propagate the received states into the vehicle state machine.
Definition: ins_vectornav.c:244
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
stateGetSpeedNed_i
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
InsVectornav::vn_packet
struct VNPacket vn_packet
Packet struct.
Definition: ins_vectornav.h:80
send_accel_scaled
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:87
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
float_rmat_comp
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Definition: pprz_algebra_float.c:78
VNData::mode
uint8_t mode
0-not tracking, 1 - poor performance, 2- OK
Definition: vn200_serial.h:96
float_rmat_of_quat
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
Definition: pprz_algebra_float.c:236
InsVectornav::ltp_def
struct LtpDef_i ltp_def
Definition: ins_vectornav.h:65
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
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
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
InsVectornav::accel_i
struct Int32Vect3 accel_i
Definition: ins_vectornav.h:86
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
GpsState::last_3dfix_time
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
InsVectornav::ltp_speed_i
struct NedCoor_i ltp_speed_i
Definition: ins_vectornav.h:70
orientationSetEulers_f
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
Definition: pprz_orientation_conversion.h:189
GpsState::last_3dfix_ticks
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:114
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
ins_init_origin_i_from_flightplan
void ins_init_origin_i_from_flightplan(struct LtpDef_i *ltp_def)
initialize the local origin (ltp_def in fixed point) from flight plan position
Definition: ins.c:39
LlaCoor_f::alt
float alt
in meters (normally above WGS84 reference ellipsoid)
Definition: pprz_geodetic_float.h:57
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
send_ins
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:37
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
InsVectornav::vn_status
enum VNStatus vn_status
VN status.
Definition: ins_vectornav.h:82
stateSetAccelNed_f
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
VNData::gyro
struct FloatRates gyro
Rates in the imu frame m/s.
Definition: vn200_serial.h:90
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
GpsState::last_msg_time
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
VNData::vel_u
float vel_u
NED velocity uncertainty [m/s].
Definition: vn200_serial.h:92
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
InsVectornav::ltp_initialized
bool ltp_initialized
Definition: ins_vectornav.h:66
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
InsVectornav::vn_data
struct VNData vn_data
Data struct.
Definition: ins_vectornav.h:81
VNPacket::framing_error
uint16_t framing_error
Definition: vn200_serial.h:72
stateGetPositionEcef_i
static struct EcefCoor_i * stateGetPositionEcef_i(void)
Get position in ECEF coordinates (int).
Definition: state.h:656
sys_time
Definition: sys_time.h:71
InsVectornav::qfe
float qfe
Definition: ins_vectornav.h:77
GpsState::num_sv
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
InsVectornav::baro_z
float baro_z
z-position calculated from baro in meters (z-down)
Definition: ins_vectornav.h:76
sys_time::nb_sec_rem
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
wgs84_ellipsoid_to_geoid_f
static float wgs84_ellipsoid_to_geoid_f(float lat, float lon)
Get WGS84 ellipsoid/geoid separation.
Definition: pprz_geodetic_wgs84.h:106
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
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
InsVectornav::ltp_accel_f
struct NedCoor_f ltp_accel_f
Definition: ins_vectornav.h:73
stateGetAccelNed_i
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
LlaCoor_f::lat
float lat
in radians
Definition: pprz_geodetic_float.h:55
VNData::err
uint8_t err
see page 122 of VN-200 datasheet
Definition: vn200_serial.h:97
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
stateSetPositionLla_i
static void stateSetPositionLla_i(struct LlaCoor_i *lla_pos)
Set position from LLA coordinates (int).
Definition: state.h:547
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
VNData::timestamp
float timestamp
Time since VN startup [s].
Definition: vn200_serial.h:86
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
GPS_VALID_POS_ECEF_BIT
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
VNData::pos_lla
double pos_lla[3]
Definition: vn200_serial.h:102
send_ins_z
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:45
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
sys_time::nb_sec
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
ins_vectornav_init
void ins_vectornav_init(void)
Initialize Vectornav struct.
Definition: ins_vectornav.c:163
GpsState::svinfos
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:112
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
ins_vn
struct InsVectornav ins_vn
Definition: ins_vectornav.c:32
InsVectornav
Definition: ins_vectornav.h:64
GPS_FIX_NONE
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
VNData
Definition: vn200_serial.h:84
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:182
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
GPS_VALID_HMSL_BIT
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
INS_VN_BODY_TO_IMU_THETA
#define INS_VN_BODY_TO_IMU_THETA
Definition: ins_vectornav.h:58
InsVectornav::ltp_accel_i
struct NedCoor_i ltp_accel_i
Definition: ins_vectornav.h:71
pprz_geodetic_wgs84.h
WGS-84 Geoid Heights.
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
VNPacket::status
enum VNMsgStatus status
Definition: vn200_serial.h:67
send_ins_ref
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:51
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
ins_vectornav_event
void ins_vectornav_event(void)
Event handling for Vectornav.
Definition: ins_vectornav.c:146