Paparazzi UAS  v5.15_devel-61-g9c6bcf9
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
dw1000_arduino.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
27 
28 #include "std.h"
29 #include "mcu_periph/uart.h"
30 #include "pprzlink/messages.h"
32 #include "subsystems/abi.h"
34 #include "subsystems/gps.h"
35 #include "state.h"
36 #include "generated/flight_plan.h"
37 #include "generated/airframe.h"
38 #include <stdio.h>
39 #include <stdlib.h>
40 #include <string.h>
41 
48 #ifndef DW1000_NB_ANCHORS
49 #define DW1000_NB_ANCHORS 3
50 #endif
51 
53 #ifndef DW1000_OFFSET
54 #define DW1000_OFFSET { 0.f, 0.f, 0.f }
55 #endif
56 
58 #ifndef DW1000_SCALE
59 #define DW1000_SCALE { 1.f, 1.f, 1.f }
60 #endif
61 
63 #ifndef DW1000_INITIAL_HEADING
64 #define DW1000_INITIAL_HEADING 0.f
65 #endif
66 
68 #ifndef DW1000_TIMEOUT
69 #define DW1000_TIMEOUT 500
70 #endif
71 
73 #define DW_STX 0xFE
74 
76 #define DW_WAIT_STX 0
77 #define DW_GET_DATA 1
78 #define DW_GET_CK 2
79 #define DW_NB_DATA 6
80 
82 struct DW1000 {
89  struct EnuCoor_f pos;
90  struct EnuCoor_f raw_pos;
92  struct LtpDef_i ltp_def;
93  bool updated;
94 };
95 
96 static struct DW1000 dw1000;
97 
98 
100 static inline float float_from_buf(uint8_t* b) {
101  float f;
102  memcpy((uint8_t*)(&f), b, sizeof(float));
103  return f;
104 }
105 
107 static inline uint16_t uint16_from_buf(uint8_t* b) {
108  uint16_t u16;
109  memcpy ((uint8_t*)(&u16), b, sizeof(uint16_t));
110  return u16;
111 }
112 
114 static void fill_anchor(struct DW1000 *dw) {
115  for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
116  uint16_t id = uint16_from_buf(dw->buf);
117  if (dw->anchors[i].id == id) {
118  dw->anchors[i].distance = float_from_buf(dw->buf + 2);
119  dw->anchors[i].time = get_sys_time_float();
120  dw->updated = true;
121  break;
122  }
123  }
124 }
125 
127 static void dw1000_arduino_parse(struct DW1000 *dw, uint8_t c)
128 {
129  switch (dw->state) {
130 
131  case DW_WAIT_STX:
132  /* Waiting Synchro */
133  if (c == DW_STX) {
134  dw->idx = 0;
135  dw->ck = 0;
136  dw->state = DW_GET_DATA;
137  }
138  break;
139 
140  case DW_GET_DATA:
141  /* Read Bytes */
142  dw->buf[dw->idx++] = c;
143  dw->ck += c;
144  if (dw->idx == DW_NB_DATA) {
145  dw->state = DW_GET_CK;
146  }
147  break;
148 
149  case DW_GET_CK:
150  /* Checksum */
151  if (dw->ck == c) {
152  fill_anchor(dw);
153  }
154  dw->state = DW_WAIT_STX;
155  break;
156  }
157 }
158 
159 static void send_gps_dw1000_small(struct DW1000 *dw)
160 {
161  // rotate and convert to cm integer
162  float x = dw->pos.x * cosf(dw->initial_heading) - dw->pos.y * sinf(dw->initial_heading);
163  float y = dw->pos.x * sinf(dw->initial_heading) + dw->pos.y * cosf(dw->initial_heading);
164  struct EnuCoor_i enu_pos;
165  enu_pos.x = (int32_t) (x * 100);
166  enu_pos.y = (int32_t) (y * 100);
167  enu_pos.z = (int32_t) (dw->pos.z * 100);
168 
169  // Convert the ENU coordinates to ECEF
170  ecef_of_enu_point_i(&(dw->gps_dw1000.ecef_pos), &(dw->ltp_def), &enu_pos);
172 
175 
176  dw->gps_dw1000.hmsl = dw->ltp_def.hmsl + enu_pos.z * 10;
178 
179  dw->gps_dw1000.num_sv = 7;
181  dw->gps_dw1000.fix = GPS_FIX_3D; // set 3D fix to true
182 
183  // set gps msg time
188 
189  // publish new GPS data
190  uint32_t now_ts = get_sys_time_usec();
191  AbiSendMsgGPS(GPS_DW1000_ID, now_ts, &(dw->gps_dw1000));
192 }
193 
195 {
196  // store current heading as ref and stop periodic call
198  dw1000_arduino_dw1000_reset_heading_ref_status = MODULES_STOP;
199 }
200 
202 static const uint16_t ids[] = DW1000_ANCHORS_IDS;
203 static const float pos_x[] = DW1000_ANCHORS_POS_X;
204 static const float pos_y[] = DW1000_ANCHORS_POS_Y;
205 static const float pos_z[] = DW1000_ANCHORS_POS_Z;
206 static const float offset[] = DW1000_OFFSET;
207 static const float scale[] = DW1000_SCALE;
208 
209 static void scale_position(struct DW1000 *dw)
210 {
211  dw->pos.x = scale[0] * (dw->raw_pos.x - offset[0]);
212  dw->pos.y = scale[1] * (dw->raw_pos.y - offset[1]);
213  dw->pos.z = scale[2] * (dw->raw_pos.z - offset[2]);
214 }
215 
217 {
218  // init DW1000 structure
219  dw1000.idx = 0;
220  dw1000.ck = 0;
223  dw1000.pos.x = 0.f;
224  dw1000.pos.y = 0.f;
225  dw1000.pos.z = 0.f;
226  dw1000.updated = false;
227  for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
228  dw1000.anchors[i].distance = 0.f;
229  dw1000.anchors[i].time = 0.f;
230  dw1000.anchors[i].id = ids[i];
231  dw1000.anchors[i].pos.x = pos_x[i];
232  dw1000.anchors[i].pos.y = pos_y[i];
233  dw1000.anchors[i].pos.z = pos_z[i];
234  }
235 
236  // gps structure init
238  dw1000.gps_dw1000.pdop = 0;
239  dw1000.gps_dw1000.sacc = 0;
240  dw1000.gps_dw1000.pacc = 0;
241  dw1000.gps_dw1000.cacc = 0;
243 
244  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
245  llh_nav0.lat = NAV_LAT0;
246  llh_nav0.lon = NAV_LON0;
247  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
248  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
249  ltp_def_from_lla_i(&dw1000.ltp_def, &llh_nav0);
250 
251  // init trilateration algorithm
253 
254 }
255 
257 {
258  // Check for timeout
260 }
261 
263 {
264  float buf[9];
265  buf[0] = dw1000.anchors[0].distance;
266  buf[1] = dw1000.anchors[1].distance;
267  buf[2] = dw1000.anchors[2].distance;
268  buf[3] = dw1000.raw_pos.x;
269  buf[4] = dw1000.raw_pos.y;
270  buf[5] = dw1000.raw_pos.z;
271  buf[6] = dw1000.pos.x;
272  buf[7] = dw1000.pos.y;
273  buf[8] = dw1000.pos.z;
274  DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 9, buf);
275 }
276 
280 static bool check_anchor_timeout(struct DW1000 *dw)
281 {
282  const float now = get_sys_time_float();
283  const float timeout = (float)DW1000_TIMEOUT / 1000.;
284  for (int i = 0; i < DW1000_NB_ANCHORS; i++) {
285  if (now - dw->anchors[i].time > timeout) {
286  return true;
287  }
288  }
289  return false;
290 }
291 
293 {
294  // Look for data on serial link and send to parser
295  while (uart_char_available(&DW1000_ARDUINO_DEV)) {
296  uint8_t ch = uart_getch(&DW1000_ARDUINO_DEV);
298  // process if new data
299  if (dw1000.updated) {
300  // if no timeout on anchors, run trilateration algorithm
301  if (check_anchor_timeout(&dw1000) == false &&
303  // apply scale and neutral corrections
305  // send fake GPS message for INS filters
307  }
308  dw1000.updated = false;
309  }
310  }
311 }
312 
313 
uint8_t idx
buffer index
#define DW1000_OFFSET
default offset, applied to final result not to individual distances
unsigned short uint16_t
Definition: types.h:16
static float timeout
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int trilateration_compute(struct Anchor *anchors, struct EnuCoor_f *pos)
Compute trilateration based on the latest measurments.
Definition: trilateration.c:87
uint32_t pacc
position accuracy in cm
Definition: gps.h:94
definition of the local (flat earth) coordinate system
int32_t y
North.
int32_t x
East.
uint16_t id
anchor ID
Definition: trilateration.h:38
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
uint8_t state
parser state
void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Convert a point in local ENU to ECEF.
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:845
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:82
struct EnuCoor_f raw_pos
unscaled local pos in anchors frame
vector in East North Up coordinates Units: meters
uint16_t uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
static const float pos_z[]
uint8_t buf[DW_NB_DATA]
incoming data buffer
Main include for ABI (AirBorneInterface).
float psi
in radians
void dw1000_arduino_report()
static uint16_t uint16_from_buf(uint8_t *b)
Utility function to get uint16_t from buffer.
void dw1000_arduino_init()
vector in Latitude, Longitude and Altitude
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
static void scale_position(struct DW1000 *dw)
#define DW1000_TIMEOUT
default timeout (in ms)
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
int trilateration_init(struct Anchor *anchors)
Init internal trilateration structures.
Definition: trilateration.c:39
int32_t hmsl
Height above mean sea level in mm.
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:108
int32_t alt
in millimeters above WGS84 reference ellipsoid
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:97
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:111
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:98
struct Anchor anchors[DW1000_NB_ANCHORS]
anchors data
bool updated
new anchor data available
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:88
static const uint16_t ids[]
init arrays from airframe file
static const float offset[]
static const float pos_x[]
float x
in meters
static struct DW1000 dw1000
data structure for GPS information
Definition: gps.h:81
uint32_t tow
GPS time of week in ms.
Definition: gps.h:103
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
Device independent GPS code (interface)
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:99
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:85
static void send_gps_dw1000_small(struct DW1000 *dw)
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
int32_t lon
in degrees*1e7
float time
time of the last received data
Definition: trilateration.h:36
void dw1000_reset_heading_ref(void)
Reset reference heading to current heading AHRS/INS should be aligned before calling this function...
int32_t z
Up.
float distance
last measured distance
Definition: trilateration.h:35
void dw1000_arduino_periodic()
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
uint8_t ck
checksum
uint16_t u16
Unsigned 16-bit integer.
Definition: common.h:43
signed long int32_t
Definition: types.h:19
#define DW1000_INITIAL_HEADING
default initial heading correction between anchors frame and global frame
static void dw1000_arduino_parse(struct DW1000 *dw, uint8_t c)
Data parsing function.
static const float scale[]
static void fill_anchor(struct DW1000 *dw)
Utility function to fill anchor from buffer.
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:109
static bool check_anchor_timeout(struct DW1000 *dw)
check timeout for each anchor
Anchor structure.
Definition: trilateration.h:34
vector in East North Up coordinates
#define DW1000_SCALE
default scale factor, applied to final result not to individual distances
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
uint8_t comp_id
id of current gps
Definition: gps.h:83
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
#define DW_NB_DATA
float z
in meters
struct LtpDef_i ltp_def
ltp reference
#define DW_GET_DATA
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
#define DW_STX
frame sync byte
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:110
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:49
uint8_t num_sv
number of sat in fix
Definition: gps.h:100
struct EnuCoor_f pos
position of the anchor
Definition: trilateration.h:37
#define DW_WAIT_STX
Parsing states.
float initial_heading
initial heading correction
DW1000 positionning system structure.
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:86
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
struct EnuCoor_f pos
local pos in anchors frame
struct GpsState gps_dw1000
"fake" gps structure
void dw1000_arduino_event()
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
int32_t lat
in degrees*1e7
#define DW1000_NB_ANCHORS
Number of anchors.
uint8_t fix
status of fix
Definition: gps.h:101
static float float_from_buf(uint8_t *b)
Utility function to get float from buffer.
float y
in meters
#define DW_GET_CK
static const float pos_y[]
void gps_periodic_check(struct GpsState *gps_s)
Periodic GPS check.
Definition: gps.c:270
#define GPS_DW1000_ID