Paparazzi UAS  v5.10_stable-5-g83a0da5-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  pprz_msg_send_ARDRONE_NAVDATA(trans, dev, AC_ID,
138  &navdata.measure.ax,
139  &navdata.measure.ay,
140  &navdata.measure.az,
141  &navdata.measure.vx,
142  &navdata.measure.vy,
143  &navdata.measure.vz,
160  &navdata.measure.mx,
161  &navdata.measure.my,
162  &navdata.measure.mz,
165 }
166 #endif
167 
172 {
173  assert(sizeof(struct navdata_measure_t) == NAVDATA_PACKET_SIZE);
174 
175  /* Check if the FD isn't already initialized */
176  if (navdata.fd <= 0) {
177  navdata.fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY); /* O_NONBLOCK doesn't work */
178 
179  if (navdata.fd < 0) {
180  printf("[navdata] Unable to open navdata board connection(/dev/ttyO1)\n");
181  return false;
182  }
183 
184  /* Update the settings of the UART connection */
185  fcntl(navdata.fd, F_SETFL, 0); /* read calls are non blocking */
186  /* set port options */
187  struct termios options;
188  /* Get the current options for the port */
189  tcgetattr(navdata.fd, &options);
190  /* Set the baud rates to 460800 */
191  cfsetispeed(&options, B460800);
192  cfsetospeed(&options, B460800);
193 
194  options.c_cflag |= (CLOCAL | CREAD); /* Enable the receiver and set local mode */
195  options.c_iflag = 0; /* clear input options */
196  options.c_lflag = 0; /* clear local options */
197  options.c_oflag &= ~OPOST; //clear output options (raw output)
198 
199  //Set the new options for the port
200  tcsetattr(navdata.fd, TCSANOW, &options);
201  }
202 
203  // Reset available flags
204  navdata_available = false;
205  navdata.baro_calibrated = false;
206  navdata.baro_available = false;
207  navdata.imu_lost = false;
208 
209  // Set all statistics to 0
213  navdata.packetsRead = 0;
215 
216  /* Stop acquisition */
218 
219  /* Read the baro calibration(blocking) */
220  if (!navdata_baro_calib()) {
221  printf("[navdata] Could not acquire baro calibration!\n");
222  return false;
223  }
224  navdata.baro_calibrated = true;
225 
226  /* Start acquisition */
228 
229  /* Set navboard gpio control */
232 
233  /* Start navdata reading thread */
234  pthread_t navdata_thread;
235  if (pthread_create(&navdata_thread, NULL, navdata_read, NULL) != 0) {
236  printf("[navdata] Could not create navdata reading thread!\n");
237  return false;
238  }
239 
240 #if PERIODIC_TELEMETRY
241  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ARDRONE_NAVDATA, send_navdata);
242 #endif
243 
244  /* Set to initialized */
245  navdata.is_initialized = true;
246  return true;
247 }
248 
249 
254 static void *navdata_read(void *data __attribute__((unused)))
255 {
256  /* Buffer insert index for reading/writing */
257  static uint8_t buffer_idx = 0;
258 
259  while (TRUE) {
260 
261  /* Wait until we are notified to read next data,
262  i.e. buffer has been copied in navdata_update */
263  pthread_mutex_lock(&navdata_mutex);
264  while (navdata_available) {
265  pthread_cond_wait(&navdata_cond, &navdata_mutex);
266  }
267  pthread_mutex_unlock(&navdata_mutex);
268 
269  /* Read new bytes */
270  int newbytes = read(navdata.fd, navdata_buffer + buffer_idx, NAVDATA_PACKET_SIZE - buffer_idx);
271 
272  /* When there was no signal interrupt */
273  if (newbytes > 0) {
274  buffer_idx += newbytes;
275  navdata.totalBytesRead += newbytes;
276  }
277 
278  /* If we got a full packet */
279  if (buffer_idx >= NAVDATA_PACKET_SIZE) {
280  /* check if the start byte is correct */
281  if (navdata_buffer[0] != NAVDATA_START_BYTE) {
282  uint8_t *pint = memchr(navdata_buffer, NAVDATA_START_BYTE, buffer_idx);
283 
284  /* Check if we found the start byte in the read data */
285  if (pint != NULL) {
286  memmove(navdata_buffer, pint, NAVDATA_PACKET_SIZE - (pint - navdata_buffer));
287  buffer_idx = pint - navdata_buffer;
288  fprintf(stderr, "[navdata] sync error, startbyte not found, resetting...\n");
289  } else {
290  buffer_idx = 0;
291  }
292  continue;
293  }
294 
295  /* full packet read with startbyte at the beginning, reset insert index */
296  buffer_idx = 0;
297 
298  /* Calculate the checksum */
299  uint16_t checksum = 0;
300  for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) {
301  checksum += navdata_buffer[i] + (navdata_buffer[i + 1] << 8);
302  }
303 
304  struct navdata_measure_t *new_measurement = (struct navdata_measure_t *)navdata_buffer;
305 
306  /* Check if the checksum is OK */
307  if (new_measurement->chksum != checksum) {
308  fprintf(stderr, "[navdata] Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",
309  checksum, new_measurement->chksum, checksum - new_measurement->chksum);
311  continue;
312  }
313 
314  /* Set flag that we have new valid navdata */
315  pthread_mutex_lock(&navdata_mutex);
316  navdata_available = true;
317  pthread_mutex_unlock(&navdata_mutex);
318  }
319  }
320 
321  return NULL;
322 }
323 
324 #include "subsystems/imu.h"
325 static void navdata_publish_imu(void)
326 {
332  imu_scale_mag(&imu);
333  uint32_t now_ts = get_sys_time_usec();
334  AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
335  AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
336  AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
337 }
338 
343 {
344  /* Check if initialized */
345  if (!navdata.is_initialized) {
346  navdata_init();
348  return;
349  }
350 
351  pthread_mutex_lock(&navdata_mutex);
352  /* If we got a new navdata packet */
353  if (navdata_available) {
354 
355  /* Copy the navdata packet */
356  memcpy(&navdata.measure, navdata_buffer, NAVDATA_PACKET_SIZE);
357 
358  /* reset the flag */
359  navdata_available = false;
360  /* signal that we copied the buffer and new packet can be read */
361  pthread_cond_signal(&navdata_cond);
362  pthread_mutex_unlock(&navdata_mutex);
363 
364  /* Check if we missed a packet (our counter and the one from the navdata) */
367  fprintf(stderr, "[navdata] Lost frame: %d should have been %d\n",
370  }
372 
373  /* Invert byte order so that TELEMETRY works better */
374  uint8_t tmp;
376  tmp = p[0];
377  p[0] = p[1];
378  p[1] = tmp;
380  tmp = p[0];
381  p[0] = p[1];
382  p[1] = tmp;
383 
386 
387 #ifdef USE_SONAR
388  /* Check if there is a new sonar measurement and update the sonar */
389  if (navdata.measure.ultrasound >> 15) {
390  float sonar_meas = (float)((navdata.measure.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
391  AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, sonar_meas);
392  }
393 #endif
394 
396 
398  } else {
399  /* no new packet available, still unlock mutex again */
400  pthread_mutex_unlock(&navdata_mutex);
401  }
402 }
403 
407 static void navdata_cmd_send(uint8_t cmd)
408 {
409  full_write(navdata.fd, &cmd, 1);
410 }
411 
412 
416 static bool navdata_baro_calib(void)
417 {
418  /* Start baro calibration acquisition */
420 
421  /* Receive the calibration (blocking) */
422  uint8_t calibBuffer[22];
423  if (full_read(navdata.fd, calibBuffer, sizeof calibBuffer) < 0) {
424  printf("[navdata] Could not read calibration data.");
425  return false;
426  }
427 
428  /* Convert the read bytes */
429  navdata.bmp180_calib.ac1 = calibBuffer[0] << 8 | calibBuffer[1];
430  navdata.bmp180_calib.ac2 = calibBuffer[2] << 8 | calibBuffer[3];
431  navdata.bmp180_calib.ac3 = calibBuffer[4] << 8 | calibBuffer[5];
432  navdata.bmp180_calib.ac4 = calibBuffer[6] << 8 | calibBuffer[7];
433  navdata.bmp180_calib.ac5 = calibBuffer[8] << 8 | calibBuffer[9];
434  navdata.bmp180_calib.ac6 = calibBuffer[10] << 8 | calibBuffer[11];
435  navdata.bmp180_calib.b1 = calibBuffer[12] << 8 | calibBuffer[13];
436  navdata.bmp180_calib.b2 = calibBuffer[14] << 8 | calibBuffer[15];
437  navdata.bmp180_calib.mb = calibBuffer[16] << 8 | calibBuffer[17];
438  navdata.bmp180_calib.mc = calibBuffer[18] << 8 | calibBuffer[19];
439  navdata.bmp180_calib.md = calibBuffer[20] << 8 | calibBuffer[21];
440 
441  return true;
442 }
443 
448 static void mag_freeze_check(void)
449 {
450  /* Thanks to Daren.G.Lee for initial fix on 20140530 */
451  static int16_t LastMagValue = 0;
452  static int MagFreezeCounter = 0;
453 
454  if (LastMagValue == navdata.measure.mx) {
455  MagFreezeCounter++;
456 
457  /* Has to have at least 30 times the exact same value to consider it a frozen magnetometer value */
458  if (MagFreezeCounter > 30) {
459  fprintf(stderr, "mag freeze detected, resetting!\n");
460 
461  /* set imu_lost flag */
462  navdata.imu_lost = true;
464 
465  /* Stop acquisition */
467 
468  /* Reset the hardware of the navboard */
470  usleep(20000);
472 
473  /* Wait for 40ms for it to boot */
474  usleep(40000);
475 
476  /* Start the navdata again and reset the counter */
478  MagFreezeCounter = 0; /* reset counter back to zero */
479  }
480  } else {
481  navdata.imu_lost = false;
482  /* Reset counter if value _does_ change */
483  MagFreezeCounter = 0;
484  }
485  /* set last value */
486  LastMagValue = navdata.measure.mx;
487 }
488 
494 static void baro_update_logic(void)
495 {
496  static int32_t lastpressval = 0;
497  static uint16_t lasttempval = 0;
498  static int32_t lastpressval_nospike = 0;
499  static uint16_t lasttempval_nospike = 0;
500  static uint8_t temp_or_press_was_updated_last =
501  0; /* 0 = press, so we now wait for temp, 1 = temp so we now wait for press */
502 
503  static int sync_errors = 0;
504  static int spike_detected = 0;
505 
506  if (temp_or_press_was_updated_last == 0) { /* Last update was press so we are now waiting for temp */
507  /* temp was updated */
508  temp_or_press_was_updated_last = true;
509 
510  /* This means that press must remain constant */
511  if (lastpressval != 0) {
512  /* If pressure was updated: this is a sync error */
513  if (lastpressval != navdata.measure.pressure) {
514  /* wait for temp again */
515  temp_or_press_was_updated_last = false;
516  sync_errors++;
517  //printf("Baro-Logic-Error (expected updated temp, got press)\n");
518  }
519  }
520  } else {
521  /* press was updated */
522  temp_or_press_was_updated_last = false;
523 
524  /* This means that temp must remain constant */
525  if (lasttempval != 0) {
526  /* If temp was updated: this is a sync error */
527  if (lasttempval != navdata.measure.temperature_pressure) {
528  /* wait for press again */
529  temp_or_press_was_updated_last = true;
530  sync_errors++;
531  //printf("Baro-Logic-Error (expected updated press, got temp)\n");
532 
533  } else {
534  /* We now got valid pressure and temperature */
535  navdata.baro_available = true;
536  }
537  }
538  }
539 
540  /* Detected a pressure swap */
541  if (lastpressval != 0 && lasttempval != 0
542  && ABS(lastpressval - navdata.measure.pressure) > ABS(lasttempval - navdata.measure.pressure)) {
543  navdata.baro_available = false;
544  }
545 
546  /* Detected a temprature swap */
547  if (lastpressval != 0 && lasttempval != 0
548  && ABS(lasttempval - navdata.measure.temperature_pressure) > ABS(lastpressval - navdata.measure.temperature_pressure)) {
549  navdata.baro_available = false;
550  }
551 
552  lasttempval = navdata.measure.temperature_pressure;
553  lastpressval = navdata.measure.pressure;
554 
555  /*
556  * It turns out that a lot of navdata boards have a problem (probably interrupt related)
557  * in which reading I2C data and writing uart output data is interrupted very long (50% of 200Hz).
558  * Afterwards, the 200Hz loop tries to catch up lost time but reads the baro too fast swapping the
559  * pressure and temperature values by exceeding the minimal conversion time of the bosh baro. The
560  * normal Parrot firmware seems to be perfectly capable to fly with this, either with excessive use of
561  * the sonar or with software filtering or spike detection. Paparazzi with its tightly coupled baro-altitude
562  * had problems. Since this problems looks not uncommon a detector was made. A lot of evidence is grabbed
563  * with a logic analyzer on the navboard I2C and serial output. The UART CRC is still perfect, the baro
564  * temp-press-temp-press logic is still perfect, so not easy to detect. Temp and pressure are swapped,
565  * and since both can have almost the same value, the size of the spike is not predictable. However at
566  * every spike of at least 3 broken boards the press and temp are byte-wise exactly the same due to
567  * reading them too quickly (trying to catch up for delay that happened earlier due to still non understood
568  * reasons. As pressure is more likely to quickly change, a small (yet unlikely) spike on temperature together with
569  * press==temp yields very good results as a detector, although theoretically not perfect.
570 
571  #samp press temp.
572  50925 39284 34501
573  50926 39287 34501
574  50927 39287 34501
575  50928 39283 34501 // *press good -> baro
576  50929 39283 34501
577  50930 39285 34501 // *press good -> baro
578  50931 39285 34500
579  50932 34500 34500 // press read too soon from chip (<4.5ms) -> ADC register still previous temp value
580  50933 34500 36618 // press not updated, still wrong. Temp is weird: looks like the average of both
581  50934 39284 36618 // new press read, but temp still outdated
582  50935 39284 34501
583  50936 39284 34501 // *press good -> baro
584  50937 39284 34500
585  50938 39281 34500
586  50939 39281 34500
587  50940 39280 34500
588  50941 39280 34502
589  50942 39280 34502
590  50943 39280 34501
591 
592  */
593 
594  /* If press and temp are same and temp has jump: neglect the next frame */
596  navdata.measure.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40))
597  /* dont use the next 3 packets */
598  spike_detected = 3;
599  }
600 
601  if (spike_detected > 0) {
602  /* disable kalman filter use */
603  navdata.baro_available = false;
604 
605  // override both to last good
606  navdata.measure.pressure = lastpressval_nospike;
607  navdata.measure.temperature_pressure = lasttempval_nospike;
608 
609  /* Countdown */
610  spike_detected--;
611  } else { // both are good
612  lastpressval_nospike = navdata.measure.pressure;
613  lasttempval_nospike = navdata.measure.temperature_pressure;
614  }
615 }
unsigned short uint16_t
Definition: types.h:16
Dispatcher to register actual AHRS implementations.
void imu_scale_gyro(struct Imu *_imu)
Definition: ahrs_gx3.c:349
static void gpio_clear(ioportid_t port, uint16_t pin)
Clear a gpio output to low level.
Definition: gpio_arch.h:103
#define IMU_BOARD_ID
Periodic telemetry system header (includes downlink utility and generated code).
void imu_scale_accel(struct Imu *_imu)
Definition: ahrs_gx3.c:350
Some architecture independent helper functions for GPIOs.
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:124
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:329
Main include for ABI (AirBorneInterface).
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:60
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
#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.
signed long int32_t
Definition: types.h:19
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
unsigned char uint8_t
Definition: types.h:14
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:351
#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]
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
Definition: gpio_arch.h:93
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38