Paparazzi UAS  v5.15_devel-111-g8fb4629
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nps_ins_vectornav.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2012 The Paparazzi Team
4  * Copyright (C) 2016 Michal Podhradsky <http://github.com/podhrmic>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 #include "nps_ins.h"
25 #include <sys/time.h>
26 #include "nps_fdm.h"
27 #include <time.h>
28 #include <stdio.h>
29 #include "nps_sensors.h"
30 #include <stdlib.h> /* srand, rand */
31 
32 /*
33  * Vectornav info
34  */
35 #define VN_DATA_START 10
36 #define VN_BUFFER_SIZE 512
37 #define GPS_SEC_IN_DAY 86400
38 
39 static uint8_t VN_SYNC = 0xFA;
40 static uint8_t VN_OUTPUT_GROUP = 0x39;
41 static uint16_t VN_GROUP_FIELD_1 = 0x01E9;
42 static uint16_t VN_GROUP_FIELD_2 = 0x061A;
43 static uint16_t VN_GROUP_FIELD_3 = 0x0140;
44 static uint16_t VN_GROUP_FIELD_4 = 0x0009;
45 
47 
49 
50 struct VectornavData {
52  float YawPitchRoll[3];
53  float AngularRate[3];
54  double Position[3];
55  float Velocity[3];
56  float Accel[3];
60  float PosU[3];
61  float VelU;
62  float LinearAccelBody[3];
63  float YprU[3];
65  float VelBody[3];
66 };
67 
69 
70 void ins_vectornav_init(void);
71 void ins_vectornav_init(void) {}
72 void ins_vectornav_event(void);
73 void ins_vectornav_event(void) {}
74 
80 static short vn_calculate_crc(unsigned char data[], unsigned int length)
81 {
82  unsigned int i;
83  unsigned short crc = 0;
84  for (i = 0; i < length; i++) {
85  crc = (unsigned char)(crc >> 8) | (crc << 8);
86  crc ^= data[i];
87  crc ^= (unsigned char)(crc & 0xff) >> 4;
88  crc ^= crc << 12;
89  crc ^= (crc & 0x00ff) << 5;
90  }
91  return crc;
92 }
93 
94 void nps_ins_init(void)
95 {
96  ins_buffer = &vn_buffer[0];
97 }
98 
99 
104 {
105  struct timeval curTime;
106  gettimeofday(&curTime, NULL);
107  int milli = curTime.tv_usec / 1000;
108  struct tm t_res;
109  localtime_r(&curTime.tv_sec, &t_res);
110  struct tm *tt = &t_res;
111 
112  uint64_t tow = (uint64_t)GPS_SEC_IN_DAY * tt->tm_wday + (uint64_t)3600 * tt->tm_hour + (uint64_t)60 * tt->tm_min + tt->tm_sec; // sec
113  tow = tow * 1000; // tow to ms
114  tow = tow + milli; // tow with added ms
115  tow = tow * 1e6; // tow in nanoseconds
116 
117  return tow;
118 }
119 
125 void nps_ins_fetch_data(struct NpsFdm *fdm_ins)
126 {
127  struct NpsFdm fdm_data;
128  memcpy(&fdm_data, fdm_ins, sizeof(struct NpsFdm));
129 
130  // Timestamp
131  vn_data.TimeStartup = (uint64_t)(fdm_data.time * 1000000000.0);
132 
133  //Attitude, float, [degrees], yaw, pitch, roll, NED frame
134  vn_data.YawPitchRoll[0] = DegOfRad((float)fdm_data.ltp_to_body_eulers.psi); // yaw
135  vn_data.YawPitchRoll[1] = DegOfRad((float)fdm_data.ltp_to_body_eulers.theta); // pitch
136  vn_data.YawPitchRoll[2] = DegOfRad((float)fdm_data.ltp_to_body_eulers.phi); // roll
137 
138  // Rates (imu frame), float, [rad/s]
139  vn_data.AngularRate[0] = (float)fdm_data.body_ecef_rotvel.p;
140  vn_data.AngularRate[1] = (float)fdm_data.body_ecef_rotvel.q;
141  vn_data.AngularRate[2] = (float)fdm_data.body_ecef_rotvel.r;
142 
143  //Pos LLA, double,[beg, deg, m]
144  //The estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectfully.
145  vn_data.Position[0] = DegOfRad(sensors.gps.lla_pos.lat);
146  vn_data.Position[1] = DegOfRad(sensors.gps.lla_pos.lon);
148 
149  //VelNed, float [m/s]
150  //The estimated velocity in the North East Down (NED) frame, given in m/s.
151  vn_data.Velocity[0] = (float)fdm_data.ltp_ecef_vel.x;
152  vn_data.Velocity[1] = (float)fdm_data.ltp_ecef_vel.y;
153  vn_data.Velocity[2] = (float)fdm_data.ltp_ecef_vel.z;
154 
155  // Accel (imu-frame), float, [m/s^-2]
156  vn_data.Accel[0] = (float)fdm_data.body_ecef_accel.x;
157  vn_data.Accel[1] = (float)fdm_data.body_ecef_accel.y;
158  vn_data.Accel[2] = (float)fdm_data.body_ecef_accel.z;
159 
160  // tow (in nanoseconds), uint64
162 
163  //num sats, uint8
164  vn_data.NumSats = 8; // random number
165 
166  //gps fix, uint8
167  // TODO: add warm-up time
168  vn_data.Fix = 3; // 3D fix
169 
170  //posU, float[3]
171  // TODO: use proper sensor simulation
172  vn_data.PosU[0] = 2.5+(((float)rand())/RAND_MAX)*0.1;
173  vn_data.PosU[1] = 2.5+(((float)rand())/RAND_MAX)*0.1;
174  vn_data.PosU[2] = 2.5+(((float)rand())/RAND_MAX)*0.1;
175 
176  //velU, float
177  // TODO: use proper sensor simulation
178  vn_data.VelU = 5.0+(((float)rand())/RAND_MAX)*0.1;
179 
180  //linear acceleration imu-body frame, float [m/s^2]
181  vn_data.LinearAccelBody[0] = (float)fdm_data.ltp_ecef_vel.x;
182  vn_data.LinearAccelBody[1] = (float)fdm_data.ltp_ecef_vel.y;
183  vn_data.LinearAccelBody[2] = (float)fdm_data.ltp_ecef_vel.z;
184 
185  //YprU, float[3]
186  // TODO: use proper sensor simulation
187  vn_data.YprU[0] = 2.5+(((float)rand())/RAND_MAX)*0.1;
188  vn_data.YprU[1] = 0.5+(((float)rand())/RAND_MAX)*0.1;
189  vn_data.YprU[2] = 0.5+(((float)rand())/RAND_MAX)*0.1;
190 
191  //instatus, uint16
192  vn_data.InsStatus = 0x02;
193 
194  //Vel body, float [m/s]
195  // The estimated velocity in the body (i.e. imu) frame, given in m/s.
196  vn_data.VelBody[0] = (float)fdm_data.body_accel.x;
197  vn_data.VelBody[1] = (float)fdm_data.body_accel.y;
198  vn_data.VelBody[2] = (float)fdm_data.body_accel.z;
199 }
200 
201 
203 {
204  static uint16_t idx;
205 
206  vn_buffer[0] = VN_SYNC;
208  vn_buffer[2] = (uint8_t)(VN_GROUP_FIELD_1 >> 8);
210  vn_buffer[4] = (uint8_t)(VN_GROUP_FIELD_2 >> 8);
212  vn_buffer[6] = (uint8_t)(VN_GROUP_FIELD_3 >> 8);
214  vn_buffer[8] = (uint8_t)(VN_GROUP_FIELD_4 >> 8);
216 
217  idx = VN_DATA_START;
218 
219  // Timestamp
220  memcpy(&vn_buffer[idx], &vn_data.TimeStartup, sizeof(uint64_t));
221  idx += sizeof(uint64_t);
222 
223  //Attitude, float, [degrees], yaw, pitch, roll, NED frame
224  memcpy(&vn_buffer[idx], &vn_data.YawPitchRoll, 3 * sizeof(float));
225  idx += 3 * sizeof(float);
226 
227  // Rates (imu frame), float, [rad/s]
228  memcpy(&vn_buffer[idx], &vn_data.AngularRate, 3 * sizeof(float));
229  idx += 3 * sizeof(float);
230 
231  //Pos LLA, double,[beg, deg, m]
232  //The estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectfully.
233  memcpy(&vn_buffer[idx], &vn_data.Position, 3 * sizeof(double));
234  idx += 3 * sizeof(double);
235 
236  //VelNed, float [m/s]
237  //The estimated velocity in the North East Down (NED) frame, given in m/s.
238  memcpy(&vn_buffer[idx], &vn_data.Velocity, 3 * sizeof(float));
239  idx += 3 * sizeof(float);
240 
241  // Accel (imu-frame), float, [m/s^-2]
242  memcpy(&vn_buffer[idx], &vn_data.Accel, 3 * sizeof(float));
243  idx += 3 * sizeof(float);
244 
245  // tow (in nanoseconds), uint64
246  memcpy(&vn_buffer[idx], &vn_data.Tow, sizeof(uint64_t));
247  idx += sizeof(uint64_t);
248 
249  //num sats, uint8
251  idx++;
252 
253  //gps fix, uint8
255  idx++;
256 
257  //posU, float[3]
258  memcpy(&vn_buffer[idx], &vn_data.PosU, 3 * sizeof(float));
259  idx += 3 * sizeof(float);
260 
261  //velU, float
262  memcpy(&vn_buffer[idx], &vn_data.VelU, sizeof(float));
263  idx += sizeof(float);
264 
265  //linear acceleration imu-body frame, float [m/s^2]
266  memcpy(&vn_buffer[idx], &vn_data.LinearAccelBody, 3 * sizeof(float));
267  idx += 3 * sizeof(float);
268 
269  //YprU, float[3]
270  memcpy(&vn_buffer[idx], &vn_data.YprU, 3 * sizeof(float));
271  idx += 3 * sizeof(float);
272 
273  //instatus, uint16
274  memcpy(&vn_buffer[idx], &vn_data.InsStatus, sizeof(uint16_t));
275  idx += sizeof(uint16_t);
276 
277  //Vel body, float [m/s]
278  // The estimated velocity in the body (i.e. imu) frame, given in m/s.
279  memcpy(&vn_buffer[idx], &vn_data.VelBody, 3 * sizeof(float));
280  idx += 3 * sizeof(float);
281 
282  // calculate checksum & send
283  uint16_t chk = vn_calculate_crc(&vn_buffer[1], idx - 1);
284  vn_buffer[idx] = (uint8_t)(chk >> 8);
285  idx++;
286  vn_buffer[idx] = (uint8_t)(chk & 0xFF);
287  idx++;
288 
289  return idx;
290 }
unsigned short uint16_t
Definition: types.h:16
double time
Definition: nps_fdm.h:46
static uint32_t idx
static uint16_t VN_GROUP_FIELD_4
uint8_t vn_buffer[VN_BUFFER_SIZE]
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
double phi
in radians
uint64_t TimeStartup
static uint16_t VN_GROUP_FIELD_1
#define VN_BUFFER_SIZE
static uint16_t VN_GROUP_FIELD_3
double psi
in radians
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
double q
in rad/s^2
double theta
in radians
static uint8_t VN_OUTPUT_GROUP
double r
in rad/s^2
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
struct NedCoor_d ltp_ecef_vel
velocity in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:74
static uint8_t VN_SYNC
Definition: nps_fdm.h:44
double x
in meters
#define VN_DATA_START
unsigned long long uint64_t
Definition: types.h:20
static uint64_t vn_get_time_of_week(void)
struct VectornavData vn_data
struct LlaCoor_d lla_pos
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
Definition: nps_fdm.h:87
static uint16_t VN_GROUP_FIELD_2
void ins_vectornav_event(void)
Event handling for Vectornav.
double Position[3]
void nps_ins_init(void)
struct NpsSensors sensors
Definition: nps_sensors.c:6
float AngularRate[3]
float LinearAccelBody[3]
static short vn_calculate_crc(unsigned char data[], unsigned int length)
Calculates the 16-bit CRC for the given ASCII or binary message.
unsigned char uint8_t
Definition: types.h:14
double lon
in radians
void ins_vectornav_init(void)
Initialize Vectornav struct.
struct DoubleVect3 body_ecef_accel
acceleration in body frame, wrt ECEF frame
Definition: nps_fdm.h:71
double z
in meters
uint8_t * ins_buffer
uint16_t nps_ins_fill_buffer(void)
void nps_ins_fetch_data(struct NpsFdm *fdm_ins)
Fetch data from FDM and store them into vectornav packet NOTE: some noise is being added...
double p
in rad/s^2
float YawPitchRoll[3]
double y
in meters
struct NpsSensorGps gps
Definition: nps_sensors.h:22
#define GPS_SEC_IN_DAY