Paparazzi UAS  v5.12_stable-4-g9b43e9b
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_event(void) {}
72 
78 unsigned short vn_calculate_crc(unsigned char data[], unsigned int length)
79 {
80  unsigned int i;
81  unsigned short crc = 0;
82  for (i = 0; i < length; i++) {
83  crc = (unsigned char)(crc >> 8) | (crc << 8);
84  crc ^= data[i];
85  crc ^= (unsigned char)(crc & 0xff) >> 4;
86  crc ^= crc << 12;
87  crc ^= (crc & 0x00ff) << 5;
88  }
89  return crc;
90 }
91 
92 void nps_ins_init(void)
93 {
94  ins_buffer = &vn_buffer[0];
95 }
96 
97 
102 {
103  struct timeval curTime;
104  gettimeofday(&curTime, NULL);
105  int milli = curTime.tv_usec / 1000;
106  struct tm t_res;
107  localtime_r(&curTime.tv_sec, &t_res);
108  struct tm *tt = &t_res;
109 
110  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
111  tow = tow * 1000; // tow to ms
112  tow = tow + milli; // tow with added ms
113  tow = tow * 1e6; // tow in nanoseconds
114 
115  return tow;
116 }
117 
123 void nps_ins_fetch_data(struct NpsFdm *fdm_ins)
124 {
125  struct NpsFdm fdm_data;
126  memcpy(&fdm_data, fdm_ins, sizeof(struct NpsFdm));
127 
128  // Timestamp
129  vn_data.TimeStartup = (uint64_t)(fdm_data.time * 1000000000.0);
130 
131  //Attitude, float, [degrees], yaw, pitch, roll, NED frame
132  vn_data.YawPitchRoll[0] = DegOfRad((float)fdm_data.ltp_to_body_eulers.psi); // yaw
133  vn_data.YawPitchRoll[1] = DegOfRad((float)fdm_data.ltp_to_body_eulers.theta); // pitch
134  vn_data.YawPitchRoll[2] = DegOfRad((float)fdm_data.ltp_to_body_eulers.phi); // roll
135 
136  // Rates (imu frame), float, [rad/s]
137  vn_data.AngularRate[0] = (float)fdm_data.body_ecef_rotvel.p;
138  vn_data.AngularRate[1] = (float)fdm_data.body_ecef_rotvel.q;
139  vn_data.AngularRate[2] = (float)fdm_data.body_ecef_rotvel.r;
140 
141  //Pos LLA, double,[beg, deg, m]
142  //The estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectfully.
143  vn_data.Position[0] = DegOfRad(sensors.gps.lla_pos.lat);
144  vn_data.Position[1] = DegOfRad(sensors.gps.lla_pos.lon);
146 
147  //VelNed, float [m/s]
148  //The estimated velocity in the North East Down (NED) frame, given in m/s.
149  vn_data.Velocity[0] = (float)fdm_data.ltp_ecef_vel.x;
150  vn_data.Velocity[1] = (float)fdm_data.ltp_ecef_vel.y;
151  vn_data.Velocity[2] = (float)fdm_data.ltp_ecef_vel.z;
152 
153  // Accel (imu-frame), float, [m/s^-2]
154  vn_data.Accel[0] = (float)fdm_data.body_ecef_accel.x;
155  vn_data.Accel[1] = (float)fdm_data.body_ecef_accel.y;
156  vn_data.Accel[2] = (float)fdm_data.body_ecef_accel.z;
157 
158  // tow (in nanoseconds), uint64
160 
161  //num sats, uint8
162  vn_data.NumSats = 8; // random number
163 
164  //gps fix, uint8
165  // TODO: add warm-up time
166  vn_data.Fix = 3; // 3D fix
167 
168  //posU, float[3]
169  // TODO: use proper sensor simulation
170  vn_data.PosU[0] = 2.5+(((float)rand())/RAND_MAX)*0.1;
171  vn_data.PosU[1] = 2.5+(((float)rand())/RAND_MAX)*0.1;
172  vn_data.PosU[2] = 2.5+(((float)rand())/RAND_MAX)*0.1;
173 
174  //velU, float
175  // TODO: use proper sensor simulation
176  vn_data.VelU = 5.0+(((float)rand())/RAND_MAX)*0.1;
177 
178  //linear acceleration imu-body frame, float [m/s^2]
179  vn_data.LinearAccelBody[0] = (float)fdm_data.ltp_ecef_vel.x;
180  vn_data.LinearAccelBody[1] = (float)fdm_data.ltp_ecef_vel.y;
181  vn_data.LinearAccelBody[2] = (float)fdm_data.ltp_ecef_vel.z;
182 
183  //YprU, float[3]
184  // TODO: use proper sensor simulation
185  vn_data.YprU[0] = 2.5+(((float)rand())/RAND_MAX)*0.1;
186  vn_data.YprU[1] = 0.5+(((float)rand())/RAND_MAX)*0.1;
187  vn_data.YprU[2] = 0.5+(((float)rand())/RAND_MAX)*0.1;
188 
189  //instatus, uint16
190  vn_data.InsStatus = 0x02;
191 
192  //Vel body, float [m/s]
193  // The estimated velocity in the body (i.e. imu) frame, given in m/s.
194  vn_data.VelBody[0] = (float)fdm_data.body_accel.x;
195  vn_data.VelBody[1] = (float)fdm_data.body_accel.y;
196  vn_data.VelBody[2] = (float)fdm_data.body_accel.z;
197 }
198 
199 
201 {
202  static uint16_t idx;
203 
204  vn_buffer[0] = VN_SYNC;
206  vn_buffer[2] = (uint8_t)(VN_GROUP_FIELD_1 >> 8);
208  vn_buffer[4] = (uint8_t)(VN_GROUP_FIELD_2 >> 8);
210  vn_buffer[6] = (uint8_t)(VN_GROUP_FIELD_3 >> 8);
212  vn_buffer[8] = (uint8_t)(VN_GROUP_FIELD_4 >> 8);
214 
215  idx = VN_DATA_START;
216 
217  // Timestamp
218  memcpy(&vn_buffer[idx], &vn_data.TimeStartup, sizeof(uint64_t));
219  idx += sizeof(uint64_t);
220 
221  //Attitude, float, [degrees], yaw, pitch, roll, NED frame
222  memcpy(&vn_buffer[idx], &vn_data.YawPitchRoll, 3 * sizeof(float));
223  idx += 3 * sizeof(float);
224 
225  // Rates (imu frame), float, [rad/s]
226  memcpy(&vn_buffer[idx], &vn_data.AngularRate, 3 * sizeof(float));
227  idx += 3 * sizeof(float);
228 
229  //Pos LLA, double,[beg, deg, m]
230  //The estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectfully.
231  memcpy(&vn_buffer[idx], &vn_data.Position, 3 * sizeof(double));
232  idx += 3 * sizeof(double);
233 
234  //VelNed, float [m/s]
235  //The estimated velocity in the North East Down (NED) frame, given in m/s.
236  memcpy(&vn_buffer[idx], &vn_data.Velocity, 3 * sizeof(float));
237  idx += 3 * sizeof(float);
238 
239  // Accel (imu-frame), float, [m/s^-2]
240  memcpy(&vn_buffer[idx], &vn_data.Accel, 3 * sizeof(float));
241  idx += 3 * sizeof(float);
242 
243  // tow (in nanoseconds), uint64
244  memcpy(&vn_buffer[idx], &vn_data.Tow, sizeof(uint64_t));
245  idx += sizeof(uint64_t);
246 
247  //num sats, uint8
249  idx++;
250 
251  //gps fix, uint8
253  idx++;
254 
255  //posU, float[3]
256  memcpy(&vn_buffer[idx], &vn_data.PosU, 3 * sizeof(float));
257  idx += 3 * sizeof(float);
258 
259  //velU, float
260  memcpy(&vn_buffer[idx], &vn_data.VelU, sizeof(float));
261  idx += sizeof(float);
262 
263  //linear acceleration imu-body frame, float [m/s^2]
264  memcpy(&vn_buffer[idx], &vn_data.LinearAccelBody, 3 * sizeof(float));
265  idx += 3 * sizeof(float);
266 
267  //YprU, float[3]
268  memcpy(&vn_buffer[idx], &vn_data.YprU, 3 * sizeof(float));
269  idx += 3 * sizeof(float);
270 
271  //instatus, uint16
272  memcpy(&vn_buffer[idx], &vn_data.InsStatus, sizeof(uint16_t));
273  idx += sizeof(uint16_t);
274 
275  //Vel body, float [m/s]
276  // The estimated velocity in the body (i.e. imu) frame, given in m/s.
277  memcpy(&vn_buffer[idx], &vn_data.VelBody, 3 * sizeof(float));
278  idx += 3 * sizeof(float);
279 
280  // calculate checksum & send
281  uint16_t chk = vn_calculate_crc(&vn_buffer[1], idx - 1);
282  vn_buffer[idx] = (uint8_t)(chk >> 8);
283  idx++;
284  vn_buffer[idx] = (uint8_t)(chk & 0xFF);
285  idx++;
286 
287  return idx;
288 }
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
unsigned short vn_calculate_crc(unsigned char data[], unsigned int length)
Calculates the 16-bit CRC for the given ASCII or binary message.
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]
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