Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Michal Podhradsky,
3  * Utah State University,
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
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  */
32 struct InsVectornav ins_vn;
37 static void send_ins(struct transport_tx *trans, struct link_device *dev)
38 {
39  pprz_msg_send_INS(trans, dev, AC_ID,
43 }
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 }
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 }
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 }
75 static void send_accel(struct transport_tx *trans, struct link_device *dev)
76 {
78  pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &id,
80 }
82 static void send_gyro(struct transport_tx *trans, struct link_device *dev)
83 {
85  pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &id,
87 }
89 static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
90 {
92  pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &id,
94 }
96 static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
97 {
99  pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &id,
101 }
102 #endif
104 #ifndef USE_INS_NAV_INIT
107 #endif
117 {
118  static uint16_t last_cnt = 0;
119  static uint16_t sec_cnt = 0;
121  sec_cnt = ins_vn.vn_packet.counter - last_cnt;
122  ins_vn.vn_rate = sec_cnt; // update frequency counter
124  // we want at least 75% of periodic frequency to be able to control the airfcraft
125  if (ins_vn.vn_rate < (PERIODIC_FREQUENCY*0.75)) {
126  gps.fix = GPS_FIX_NONE;
127  }
129  // Make gps pacc available in GPS page on GCS
130  static uint32_t last_pacc = 0;
131  // update only if pacc changes
132  if (last_pacc != gps.pacc) {
133  last_pacc = gps.pacc;
134  // we don't know the value of CNO, hence oscillate
135  // between 0 and 1 to not confuse the user
136  gps.svinfos[0].cno = (gps.svinfos[0].cno + 1) % 2;
137  }
139  // update counter
140  last_cnt = ins_vn.vn_packet.counter;
142  // reset mode
143  ins_vn.vn_data.mode = 0;
144 }
151 {
152  // receive data
155  // read message
160  }
161 }
168 {
169  // Initialize variables
171  ins_vn.vn_rate = 0;
173  // Initialize packet
188  // initialize data struct
189  memset(&(ins_vn.vn_data), 0, sizeof(struct VNData));
193  ins_vn.ltp_initialized = true;
194 #else
195  ins_vn.ltp_initialized = false;
196 #endif
198  struct FloatEulers body_to_imu_eulers =
200  orientationSetEulers_f(&ins_vn.body_to_imu, &body_to_imu_eulers);
206  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VECTORNAV_INFO, send_vn_info);
207  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL, send_accel);
208  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO, send_gyro);
209  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
210  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_GYRO_SCALED, send_gyro_scaled);
211 #endif
212 }
222 {
223  gps.sacc = (uint32_t)(ins_vn.vn_data.vel_u * 100);
224 }
231 {
232  float pacc = ins_vn.vn_data.pos_u[0]; // in meters
233  if (ins_vn.vn_data.pos_u[1] > pacc) {
234  pacc = ins_vn.vn_data.pos_u[1];
235  }
236  if (ins_vn.vn_data.pos_u[2] > pacc) {
237  pacc = ins_vn.vn_data.pos_u[2];
238  }
239  gps.pacc = (uint32_t)(pacc * 100);
240 }
249 {
250  // Acceleration [m/s^2]
251  // in fixed point for sending as ABI and telemetry msgs
254  // Rates [rad/s]
255  static struct FloatRates body_rate;
256  // in fixed point for sending as ABI and telemetry msgs
258  float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ins_vn.body_to_imu), &ins_vn.vn_data.gyro); // compute body rates
259  stateSetBodyRates_f(&body_rate); // Set state [rad/s]
261  // Attitude [deg]
262  static struct FloatQuat imu_quat; // convert from euler to quat
264  static struct FloatRMat imu_rmat; // convert from quat to rmat
265  float_rmat_of_quat(&imu_rmat, &imu_quat);
266  static struct FloatRMat ltp_to_body_rmat; // rotate to body frame
267  float_rmat_comp(&ltp_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu));
268  stateSetNedToBodyRMat_f(&ltp_to_body_rmat); // set body states [rad]
270  // NED (LTP) velocity [m/s]
271  // North east down (NED), also known as local tangent plane (LTP),
272  // is a geographical coordinate system for representing state vectors that is commonly used in aviation.
273  // It consists of three numbers: one represents the position along the northern axis,
274  // one along the eastern axis, and one represents vertical position. Down is chosen as opposed to
275  // up in order to comply with the right-hand rule.
276  // The origin of this coordinate system is usually chosen to be the aircraft's center of gravity.
277  // x = North
278  // y = East
279  // z = Down
280  stateSetSpeedNed_f(&ins_vn.vn_data.vel_ned); // set state
282  // NED (LTP) acceleration [m/s^2]
283  static struct FloatVect3 accel_meas_ltp;// first we need to rotate linear acceleration from imu-frame to body-frame
285  static struct NedCoor_f ltp_accel; // assign to NedCoord_f struct
286  VECT3_ASSIGN(ltp_accel, accel_meas_ltp.x, accel_meas_ltp.y, accel_meas_ltp.z);
287  stateSetAccelNed_f(&ltp_accel); // then set the states
288  ins_vn.ltp_accel_f = ltp_accel;
290  // LLA position [rad, rad, m]
291  //static struct LlaCoor_f lla_pos; // convert from deg to rad, and from double to float
292 = RadOfDeg((float)ins_vn.vn_data.pos_lla[0]); // ins_impl.pos_lla[0] = lat
293  ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.vn_data.pos_lla[1]); // ins_impl.pos_lla[1] = lon
294  ins_vn.lla_pos.alt = ((float)ins_vn.vn_data.pos_lla[2]); // ins_impl.pos_lla[2] = alt
299  // ECEF velocity
300  // TODO: properly implement
302  // ECEF position
308  // GPS Ground speed
310  gps.gspeed = ((uint16_t)(speed * 100));
312  // GPS course
313  gps.course = (int32_t)(1e7 * (atan2(ins_vn.vn_data.vel_ned.y, ins_vn.vn_data.vel_ned.x)));
316  // Because we have not HMSL data from Vectornav, we are using LLA-Altitude
317  // as a workaround
319  gps.hmsl = (int32_t)((ins_vn.lla_pos.alt - geoid_h)* 1000.0f);
322  // Set GPS fix
325  // Set GPS num_sv
328  // set position uncertainty
331  // set velocity uncertainty
334  // check GPS status
337  if (gps.fix == GPS_FIX_3D) {
340  }
342  // update internal states for telemetry purposes
343  // TODO: directly convert vectornav output instead of using state interface
344  // to support multiple INS running at the same time
348 }
struct GpsState gps
global GPS state
Definition: gps.c:69
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:92
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:90
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:101
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:97
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:79
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:89
Definition: gps.h:47
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:112
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:40
Definition: gps.h:46
Definition: gps.h:51
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:115
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:113
uint32_t pacc
position accuracy in cm
Definition: gps.h:98
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:95
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:86
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:110
Definition: gps.h:52
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:114
uint8_t num_sv
number of sat in fix
Definition: gps.h:104
uint8_t fix
status of fix
Definition: gps.h:105
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
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_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
euler angles
Roation quaternion.
rotation matrix
angular rates
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
#define INT32_VECT3_ZERO(_v)
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
int32_t z
in centimeters
struct LlaCoor_i lla
Reference point in lla.
int32_t x
in centimeters
int32_t y
struct EcefCoor_i ecef
Reference point in ecef.
int32_t y
in centimeters
int32_t lon
in degrees*1e7
int32_t x
#define LLA_BFP_OF_REAL(_o, _i)
static float wgs84_ellipsoid_to_geoid_f(float lat, float lon)
Get WGS84 ellipsoid/geoid separation.
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
static void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition: state.h:1099
static struct EcefCoor_i * stateGetPositionEcef_i(void)
Get position in ECEF coordinates (int).
Definition: state.h:656
static void stateSetPositionLla_i(struct LlaCoor_i *lla_pos)
Set position from LLA coordinates (int).
Definition: state.h:547
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
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
void ins_vectornav_monitor(void)
Monitors vectornav data rate and changes GPS lock if the data rate is too low.
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:37
void ins_vectornav_set_pacc(void)
Find maximum uncertainty (NED) position accuracy in cm.
struct InsVectornav ins_vn
Definition: ins_vectornav.c:32
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:51
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:45
void ins_vectornav_set_sacc(void)
Set speed (velocity) uncertainty (NED) speed accuracy in cm/s.
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:96
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:82
void ins_vectornav_init(void)
Initialize Vectornav struct.
void ins_vectornav_event(void)
Event handling for Vectornav.
static void send_vn_info(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:61
static void send_accel(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:75
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
Definition: ins_vectornav.c:89
void ins_vectornav_propagate()
Propagate the received states into the vehicle state machine.
Vectornav VN-200 INS module.
Definition: ins_vectornav.h:57
struct NedCoor_f ltp_accel_f
Definition: ins_vectornav.h:73
struct NedCoor_i ltp_accel_i
Definition: ins_vectornav.h:71
struct LlaCoor_f lla_pos
Definition: ins_vectornav.h:72
struct Int32Vect3 accel_i
Definition: ins_vectornav.h:86
struct VNData vn_data
Data struct.
Definition: ins_vectornav.h:81
struct VNPacket vn_packet
Packet struct.
Definition: ins_vectornav.h:80
float baro_z
z-position calculated from baro in meters (z-down)
Definition: ins_vectornav.h:76
bool ltp_initialized
Definition: ins_vectornav.h:66
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ins_vectornav.h:90
enum VNStatus vn_status
VN status.
Definition: ins_vectornav.h:82
struct NedCoor_i ltp_pos_i
Definition: ins_vectornav.h:69
Definition: ins_vectornav.h:59
struct Int32Rates gyro_i
Definition: ins_vectornav.h:87
struct NedCoor_i ltp_speed_i
Definition: ins_vectornav.h:70
Definition: ins_vectornav.h:58
uint16_t vn_rate
data frequency
Definition: ins_vectornav.h:83
struct LtpDef_i ltp_def
Definition: ins_vectornav.h:65
float alt
in meters (normally above WGS84 reference ellipsoid)
float lon
in radians
float x
in meters
float lat
in radians
float y
in meters
vector in North East Down coordinates Units: meters
WGS-84 Geoid Heights.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
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
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
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
void vn200_event(struct VNPacket *vnp)
Definition: vn200_serial.c:110
uint16_t framing_error
Definition: vn200_serial.h:72
uint32_t hdr_error
Definition: vn200_serial.h:65
uint8_t err
see page 122 of VN-200 datasheet
Definition: vn200_serial.h:97
@ VNNotTracking
Definition: vn200_serial.h:79
uint16_t overrun_error
Definition: vn200_serial.h:70
struct FloatRates gyro
Rates in the imu frame m/s.
Definition: vn200_serial.h:90
struct FloatEulers attitude
Attitude, float, [rad], yaw, pitch, roll.
Definition: vn200_serial.h:88
struct FloatEulers ypr_u
Attitude uncertainty, 1sigma, float, [degrees], yaw, pitch, roll.
Definition: vn200_serial.h:94
struct FloatVect3 accel
Acceleration in the imu frame, m/s.
Definition: vn200_serial.h:89
uint8_t mode
0-not tracking, 1 - poor performance, 2- OK
Definition: vn200_serial.h:96
float timestamp
Time since VN startup [s].
Definition: vn200_serial.h:86
float pos_u[3]
The current GPS position uncertainty in the North East Down (NED) coordinate frame,...
Definition: vn200_serial.h:91
@ VNMsgSync
Definition: vn200_serial.h:55
double pos_lla[3]
Definition: vn200_serial.h:102
float vel_u
NED velocity uncertainty [m/s].
Definition: vn200_serial.h:92
uint8_t gps_fix
Definition: vn200_serial.h:101
uint8_t num_sv
number of visible satellites
Definition: vn200_serial.h:100
struct FloatVect3 lin_accel
Linear acceleration in imu frame [m/s^2].
Definition: vn200_serial.h:93
uint16_t counter
Definition: vn200_serial.h:75
bool msg_available
Definition: vn200_serial.h:63
struct NedCoor_f vel_ned
The estimated velocity in the North East Down (NED) frame, given in m/s.
Definition: vn200_serial.h:103
uint16_t noise_error
Definition: vn200_serial.h:71
uint32_t chksm_error
Definition: vn200_serial.h:64
uint8_t msg_idx
Definition: vn200_serial.h:68
enum VNMsgStatus status
Definition: vn200_serial.h:67