Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
navdata.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 The Paparazzi Team
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  */
20 
21 /* Thanks to TU Delft by assigning students Dino Hensen, Vincent van Hoek for
22  * their inital work and later improvements made in 2015 by Freek van Tienen
23 */
24 
33 #include <stdio.h>
34 #include <stdlib.h>
35 #include <fcntl.h>
36 #include <termios.h> /* for baud rates and options */
37 #include <unistd.h>
38 #include <string.h>
39 #include <math.h>
40 #include <errno.h>
41 #include <assert.h>
42 #include <pthread.h>
43 
44 #include "std.h"
45 #include "navdata.h"
46 #include "subsystems/ins.h"
47 #include "subsystems/ahrs.h"
48 #include "subsystems/abi.h"
49 #include "mcu_periph/gpio.h"
50 
51 /* Internal used functions */
52 static void *navdata_read(void *data __attribute__((unused)));
53 static void navdata_cmd_send(uint8_t cmd);
54 static bool navdata_baro_calib(void);
55 static void mag_freeze_check(void);
56 static void baro_update_logic(void);
57 
58 /* Main navdata structure */
60 
64 static bool navdata_available = false;
65 
66 /* syncronization variables */
67 static pthread_mutex_t navdata_mutex = PTHREAD_MUTEX_INITIALIZER;
68 static pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER;
69 
70 #ifndef NAVDATA_FILTER_ID
71 #define NAVDATA_FILTER_ID 2
72 #endif
73 
78 #ifndef SONAR_OFFSET
79 #define SONAR_OFFSET 880
80 #endif
81 
85 #ifndef SONAR_SCALE
86 #define SONAR_SCALE 0.00047
87 #endif
88 
92 ssize_t full_write(int fd, const uint8_t *buf, size_t count)
93 {
94  size_t written = 0;
95 
96  while (written < count) {
97  ssize_t n = write(fd, buf + written, count - written);
98  if (n < 0) {
99  if (errno == EAGAIN || errno == EWOULDBLOCK) {
100  continue;
101  }
102  return n;
103  }
104  written += n;
105  }
106  return written;
107 }
108 
112 ssize_t full_read(int fd, uint8_t *buf, size_t count)
113 {
114  /* Apologies for illiteracy, but we can't overload |read|.*/
115  size_t readed = 0;
116 
117  while (readed < count) {
118  ssize_t n = read(fd, buf + readed, count - readed);
119  if (n < 0) {
120  if (errno == EAGAIN || errno == EWOULDBLOCK) {
121  continue;
122  }
123  return n;
124  }
125  readed += n;
126  }
127  return readed;
128 }
129 
130 #if PERIODIC_TELEMETRY
132 
133 static void send_navdata(struct transport_tx *trans, struct link_device *dev)
134 {
135 #pragma GCC diagnostic push
136 #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
137  pprz_msg_send_ARDRONE_NAVDATA(trans, dev, AC_ID,
140  &navdata.measure.ax,
141  &navdata.measure.ay,
142  &navdata.measure.az,
143  &navdata.measure.vx,
144  &navdata.measure.vy,
145  &navdata.measure.vz,
162  &navdata.measure.mx,
163  &navdata.measure.my,
164  &navdata.measure.mz,
167 #pragma GCC diagnostic pop
168 }
169 #endif
170 
175 {
176  assert(sizeof(struct navdata_measure_t) == NAVDATA_PACKET_SIZE);
177 
178  /* Check if the FD isn't already initialized */
179  if (navdata.fd <= 0) {
180  navdata.fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY); /* O_NONBLOCK doesn't work */
181 
182  if (navdata.fd < 0) {
183  printf("[navdata] Unable to open navdata board connection(/dev/ttyO1)\n");
184  return false;
185  }
186 
187  /* Update the settings of the UART connection */
188  fcntl(navdata.fd, F_SETFL, 0); /* read calls are non blocking */
189  /* set port options */
190  struct termios options;
191  /* Get the current options for the port */
192  tcgetattr(navdata.fd, &options);
193  /* Set the baud rates to 460800 */
194  cfsetispeed(&options, B460800);
195  cfsetospeed(&options, B460800);
196 
197  options.c_cflag |= (CLOCAL | CREAD); /* Enable the receiver and set local mode */
198  options.c_iflag = 0; /* clear input options */
199  options.c_lflag = 0; /* clear local options */
200  options.c_oflag &= ~OPOST; //clear output options (raw output)
201 
202  //Set the new options for the port
203  tcsetattr(navdata.fd, TCSANOW, &options);
204  }
205 
206  // Reset available flags
207  navdata_available = false;
208  navdata.baro_calibrated = false;
209  navdata.baro_available = false;
210  navdata.imu_lost = false;
211 
212  // Set all statistics to 0
216  navdata.packetsRead = 0;
218 
219  /* Stop acquisition */
221 
222  /* Read the baro calibration(blocking) */
223  if (!navdata_baro_calib()) {
224  printf("[navdata] Could not acquire baro calibration!\n");
225  return false;
226  }
227  navdata.baro_calibrated = true;
228 
229  /* Start acquisition */
231 
232  /* Set navboard gpio control */
235 
236  /* Start navdata reading thread */
237  pthread_t navdata_thread;
238  if (pthread_create(&navdata_thread, NULL, navdata_read, NULL) != 0) {
239  printf("[navdata] Could not create navdata reading thread!\n");
240  return false;
241  }
242  pthread_setname_np(navdata_thread, "pprz_navdata_thread");
243 
244 #if PERIODIC_TELEMETRY
245  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ARDRONE_NAVDATA, send_navdata);
246 #endif
247 
248  /* Set to initialized */
249  navdata.is_initialized = true;
250  return true;
251 }
252 
253 
258 static void *navdata_read(void *data __attribute__((unused)))
259 {
260  /* Buffer insert index for reading/writing */
261  static uint8_t buffer_idx = 0;
262 
263  while (TRUE) {
264 
265  /* Wait until we are notified to read next data,
266  i.e. buffer has been copied in navdata_update */
267  pthread_mutex_lock(&navdata_mutex);
268  while (navdata_available) {
269  pthread_cond_wait(&navdata_cond, &navdata_mutex);
270  }
271  pthread_mutex_unlock(&navdata_mutex);
272 
273  /* Read new bytes */
274  int newbytes = read(navdata.fd, navdata_buffer + buffer_idx, NAVDATA_PACKET_SIZE - buffer_idx);
275 
276  /* When there was no signal interrupt */
277  if (newbytes > 0) {
278  buffer_idx += newbytes;
279  navdata.totalBytesRead += newbytes;
280  }
281 
282  /* If we got a full packet */
283  if (buffer_idx >= NAVDATA_PACKET_SIZE) {
284  /* check if the start byte is correct */
285  if (navdata_buffer[0] != NAVDATA_START_BYTE) {
286  uint8_t *pint = memchr(navdata_buffer, NAVDATA_START_BYTE, buffer_idx);
287 
288  /* Check if we found the start byte in the read data */
289  if (pint != NULL) {
290  memmove(navdata_buffer, pint, NAVDATA_PACKET_SIZE - (pint - navdata_buffer));
291  buffer_idx = pint - navdata_buffer;
292  fprintf(stderr, "[navdata] sync error, startbyte not found, resetting...\n");
293  } else {
294  buffer_idx = 0;
295  }
296  continue;
297  }
298 
299  /* full packet read with startbyte at the beginning, reset insert index */
300  buffer_idx = 0;
301 
302  /* Calculate the checksum */
303  uint16_t checksum = 0;
304  for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) {
305  checksum += navdata_buffer[i] + (navdata_buffer[i + 1] << 8);
306  }
307 
308  struct navdata_measure_t *new_measurement = (struct navdata_measure_t *)navdata_buffer;
309 
310  /* Check if the checksum is OK */
311  if (new_measurement->chksum != checksum) {
312  fprintf(stderr, "[navdata] Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",
313  checksum, new_measurement->chksum, checksum - new_measurement->chksum);
315  continue;
316  }
317 
318  /* Set flag that we have new valid navdata */
319  pthread_mutex_lock(&navdata_mutex);
320  navdata_available = true;
321  pthread_mutex_unlock(&navdata_mutex);
322  }
323  }
324 
325  return NULL;
326 }
327 
328 #include "subsystems/imu.h"
329 static void navdata_publish_imu(void)
330 {
336  imu_scale_mag(&imu);
337  uint32_t now_ts = get_sys_time_usec();
338  AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
339  AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
340  AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
341 }
342 
347 {
348  /* Check if initialized */
349  if (!navdata.is_initialized) {
350  navdata_init();
352  return;
353  }
354 
355  pthread_mutex_lock(&navdata_mutex);
356  /* If we got a new navdata packet */
357  if (navdata_available) {
358 
359  /* Copy the navdata packet */
360  memcpy(&navdata.measure, navdata_buffer, NAVDATA_PACKET_SIZE);
361 
362  /* reset the flag */
363  navdata_available = false;
364  /* signal that we copied the buffer and new packet can be read */
365  pthread_cond_signal(&navdata_cond);
366  pthread_mutex_unlock(&navdata_mutex);
367 
368  /* Check if we missed a packet (our counter and the one from the navdata) */
371  fprintf(stderr, "[navdata] Lost frame: %d should have been %d\n",
374  }
376 
377  /* Invert byte order so that TELEMETRY works better */
378  uint8_t tmp;
380  tmp = p[0];
381  p[0] = p[1];
382  p[1] = tmp;
384  tmp = p[0];
385  p[0] = p[1];
386  p[1] = tmp;
387 
390 
391 #ifdef USE_SONAR
392  /* Check if there is a new sonar measurement and update the sonar */
393  if (navdata.measure.ultrasound >> 15) {
394  uint32_t now_ts = get_sys_time_usec();
395  float sonar_meas = (float)((navdata.measure.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
396  AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, now_ts, sonar_meas);
397  }
398 #endif
399 
401 
403  } else {
404  /* no new packet available, still unlock mutex again */
405  pthread_mutex_unlock(&navdata_mutex);
406  }
407 }
408 
412 static void navdata_cmd_send(uint8_t cmd)
413 {
414  full_write(navdata.fd, &cmd, 1);
415 }
416 
417 
421 static bool navdata_baro_calib(void)
422 {
423  /* Start baro calibration acquisition */
425 
426  /* Receive the calibration (blocking) */
427  uint8_t calibBuffer[22];
428  if (full_read(navdata.fd, calibBuffer, sizeof calibBuffer) < 0) {
429  printf("[navdata] Could not read calibration data.");
430  return false;
431  }
432 
433  /* Convert the read bytes */
434  navdata.bmp180_calib.ac1 = calibBuffer[0] << 8 | calibBuffer[1];
435  navdata.bmp180_calib.ac2 = calibBuffer[2] << 8 | calibBuffer[3];
436  navdata.bmp180_calib.ac3 = calibBuffer[4] << 8 | calibBuffer[5];
437  navdata.bmp180_calib.ac4 = calibBuffer[6] << 8 | calibBuffer[7];
438  navdata.bmp180_calib.ac5 = calibBuffer[8] << 8 | calibBuffer[9];
439  navdata.bmp180_calib.ac6 = calibBuffer[10] << 8 | calibBuffer[11];
440  navdata.bmp180_calib.b1 = calibBuffer[12] << 8 | calibBuffer[13];
441  navdata.bmp180_calib.b2 = calibBuffer[14] << 8 | calibBuffer[15];
442  navdata.bmp180_calib.mb = calibBuffer[16] << 8 | calibBuffer[17];
443  navdata.bmp180_calib.mc = calibBuffer[18] << 8 | calibBuffer[19];
444  navdata.bmp180_calib.md = calibBuffer[20] << 8 | calibBuffer[21];
445 
446  return true;
447 }
448 
453 static void mag_freeze_check(void)
454 {
455  /* Thanks to Daren.G.Lee for initial fix on 20140530 */
456  static int16_t LastMagValue = 0;
457  static int MagFreezeCounter = 0;
458 
459  if (LastMagValue == navdata.measure.mx) {
460  MagFreezeCounter++;
461 
462  /* Has to have at least 30 times the exact same value to consider it a frozen magnetometer value */
463  if (MagFreezeCounter > 30) {
464  fprintf(stderr, "mag freeze detected, resetting!\n");
465 
466  /* set imu_lost flag */
467  navdata.imu_lost = true;
469 
470  /* Stop acquisition */
472 
473  /* Reset the hardware of the navboard */
475  usleep(20000);
477 
478  /* Wait for 40ms for it to boot */
479  usleep(40000);
480 
481  /* Start the navdata again and reset the counter */
483  MagFreezeCounter = 0; /* reset counter back to zero */
484  }
485  } else {
486  navdata.imu_lost = false;
487  /* Reset counter if value _does_ change */
488  MagFreezeCounter = 0;
489  }
490  /* set last value */
491  LastMagValue = navdata.measure.mx;
492 }
493 
499 static void baro_update_logic(void)
500 {
501  static int32_t lastpressval = 0;
502  static uint16_t lasttempval = 0;
503  static int32_t lastpressval_nospike = 0;
504  static uint16_t lasttempval_nospike = 0;
505  static uint8_t temp_or_press_was_updated_last =
506  0; /* 0 = press, so we now wait for temp, 1 = temp so we now wait for press */
507 
508  static int sync_errors = 0;
509  static int spike_detected = 0;
510 
511  if (temp_or_press_was_updated_last == 0) { /* Last update was press so we are now waiting for temp */
512  /* temp was updated */
513  temp_or_press_was_updated_last = true;
514 
515  /* This means that press must remain constant */
516  if (lastpressval != 0) {
517  /* If pressure was updated: this is a sync error */
518  if (lastpressval != navdata.measure.pressure) {
519  /* wait for temp again */
520  temp_or_press_was_updated_last = false;
521  sync_errors++;
522  //printf("Baro-Logic-Error (expected updated temp, got press)\n");
523  }
524  }
525  } else {
526  /* press was updated */
527  temp_or_press_was_updated_last = false;
528 
529  /* This means that temp must remain constant */
530  if (lasttempval != 0) {
531  /* If temp was updated: this is a sync error */
532  if (lasttempval != navdata.measure.temperature_pressure) {
533  /* wait for press again */
534  temp_or_press_was_updated_last = true;
535  sync_errors++;
536  //printf("Baro-Logic-Error (expected updated press, got temp)\n");
537 
538  } else {
539  /* We now got valid pressure and temperature */
540  navdata.baro_available = true;
541  }
542  }
543  }
544 
545  /* Detected a pressure swap */
546  if (lastpressval != 0 && lasttempval != 0
547  && ABS(lastpressval - navdata.measure.pressure) > ABS(lasttempval - navdata.measure.pressure)) {
548  navdata.baro_available = false;
549  }
550 
551  /* Detected a temprature swap */
552  if (lastpressval != 0 && lasttempval != 0
553  && ABS(lasttempval - navdata.measure.temperature_pressure) > ABS(lastpressval - navdata.measure.temperature_pressure)) {
554  navdata.baro_available = false;
555  }
556 
557  lasttempval = navdata.measure.temperature_pressure;
558  lastpressval = navdata.measure.pressure;
559 
560  /*
561  * It turns out that a lot of navdata boards have a problem (probably interrupt related)
562  * in which reading I2C data and writing uart output data is interrupted very long (50% of 200Hz).
563  * Afterwards, the 200Hz loop tries to catch up lost time but reads the baro too fast swapping the
564  * pressure and temperature values by exceeding the minimal conversion time of the bosh baro. The
565  * normal Parrot firmware seems to be perfectly capable to fly with this, either with excessive use of
566  * the sonar or with software filtering or spike detection. Paparazzi with its tightly coupled baro-altitude
567  * had problems. Since this problems looks not uncommon a detector was made. A lot of evidence is grabbed
568  * with a logic analyzer on the navboard I2C and serial output. The UART CRC is still perfect, the baro
569  * temp-press-temp-press logic is still perfect, so not easy to detect. Temp and pressure are swapped,
570  * and since both can have almost the same value, the size of the spike is not predictable. However at
571  * every spike of at least 3 broken boards the press and temp are byte-wise exactly the same due to
572  * reading them too quickly (trying to catch up for delay that happened earlier due to still non understood
573  * reasons. As pressure is more likely to quickly change, a small (yet unlikely) spike on temperature together with
574  * press==temp yields very good results as a detector, although theoretically not perfect.
575 
576  #samp press temp.
577  50925 39284 34501
578  50926 39287 34501
579  50927 39287 34501
580  50928 39283 34501 // *press good -> baro
581  50929 39283 34501
582  50930 39285 34501 // *press good -> baro
583  50931 39285 34500
584  50932 34500 34500 // press read too soon from chip (<4.5ms) -> ADC register still previous temp value
585  50933 34500 36618 // press not updated, still wrong. Temp is weird: looks like the average of both
586  50934 39284 36618 // new press read, but temp still outdated
587  50935 39284 34501
588  50936 39284 34501 // *press good -> baro
589  50937 39284 34500
590  50938 39281 34500
591  50939 39281 34500
592  50940 39280 34500
593  50941 39280 34502
594  50942 39280 34502
595  50943 39280 34501
596 
597  */
598 
599  /* If press and temp are same and temp has jump: neglect the next frame */
601  navdata.measure.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40))
602  /* dont use the next 3 packets */
603  spike_detected = 3;
604  }
605 
606  if (spike_detected > 0) {
607  /* disable kalman filter use */
608  navdata.baro_available = false;
609 
610  // override both to last good
611  navdata.measure.pressure = lastpressval_nospike;
612  navdata.measure.temperature_pressure = lasttempval_nospike;
613 
614  /* Countdown */
615  spike_detected--;
616  } else { // both are good
617  lastpressval_nospike = navdata.measure.pressure;
618  lasttempval_nospike = navdata.measure.temperature_pressure;
619  }
620 }
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Dispatcher to register actual AHRS implementations.
static void gpio_clear(ioportid_t port, uint16_t pin)
Clear a gpio output to low level.
Definition: gpio_arch.h:108
#define IMU_BOARD_ID
Periodic telemetry system header (includes downlink utility and generated code).
Some architecture independent helper functions for GPIOs.
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
Main include for ABI (AirBorneInterface).
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
Integrated Navigation System interface.
struct Imu imu
global IMU state
Definition: imu.c:108
void gpio_setup_output(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.c:33
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:46
#define TRUE
Definition: std.h:4
static uint8_t checksum
Definition: airspeed_uADC.c:61
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Inertial Measurement Unit interface.
void imu_scale_gyro(struct Imu *_imu)
Definition: imu_vectornav.c:43
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:352
#define ARDRONE_GPIO_PORT
Definition: actuators.c:58
int fd
Definition: serial.c:26
#define AGL_SONAR_ARDRONE2_ID
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
static float p[2][2]
void imu_scale_accel(struct Imu *_imu)
Definition: imu_vectornav.c:44
#define B460800
Definition: uart_arch.h:50
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
Definition: gpio_arch.h:98
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78