Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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) 2013 Dino Hensen, Vincent van Hoek
3  * 2015 Freek van Tienen <freek.v.tienen@gmail.com>
4  *
5  * This file is part of Paparazzi.
6  *
7  * Paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * Paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with Paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
31 #include <stdio.h>
32 #include <stdlib.h>
33 #include <fcntl.h>
34 #include <termios.h> // for baud rates and options
35 #include <unistd.h>
36 #include <string.h>
37 #include <math.h>
38 #include <errno.h>
39 #include <assert.h>
40 #include <pthread.h>
41 
42 #include "std.h"
43 #include "navdata.h"
44 #include "subsystems/ins.h"
45 #include "subsystems/ahrs.h"
46 #include "subsystems/abi.h"
47 #include "mcu_periph/gpio.h"
48 
49 /* Internal used functions */
50 static void *navdata_read(void *data __attribute__((unused)));
51 static void navdata_cmd_send(uint8_t cmd);
52 static bool_t navdata_baro_calib(void);
53 static void mag_freeze_check(void);
54 static void baro_update_logic(void);
55 
56 /* Main navdata structure */
58 
62 static bool_t navdata_available = FALSE;
63 
64 /* syncronization variables */
65 static pthread_mutex_t navdata_mutex = PTHREAD_MUTEX_INITIALIZER;
66 static pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER;
67 
68 #ifndef NAVDATA_FILTER_ID
69 #define NAVDATA_FILTER_ID 2
70 #endif
71 
76 #ifndef SONAR_OFFSET
77 #define SONAR_OFFSET 880
78 #endif
79 
83 #ifndef SONAR_SCALE
84 #define SONAR_SCALE 0.00047
85 #endif
86 
90 ssize_t full_write(int fd, const uint8_t *buf, size_t count)
91 {
92  size_t written = 0;
93 
94  while (written < count) {
95  ssize_t n = write(fd, buf + written, count - written);
96  if (n < 0) {
97  if (errno == EAGAIN || errno == EWOULDBLOCK) {
98  continue;
99  }
100  return n;
101  }
102  written += n;
103  }
104  return written;
105 }
106 
110 ssize_t full_read(int fd, uint8_t *buf, size_t count)
111 {
112  /* Apologies for illiteracy, but we can't overload |read|.*/
113  size_t readed = 0;
114 
115  while (readed < count) {
116  ssize_t n = read(fd, buf + readed, count - readed);
117  if (n < 0) {
118  if (errno == EAGAIN || errno == EWOULDBLOCK) {
119  continue;
120  }
121  return n;
122  }
123  readed += n;
124  }
125  return readed;
126 }
127 
128 #if PERIODIC_TELEMETRY
130 
131 static void send_navdata(struct transport_tx *trans, struct link_device *dev)
132 {
133  pprz_msg_send_ARDRONE_NAVDATA(trans, dev, AC_ID,
136  &navdata.measure.ax,
137  &navdata.measure.ay,
138  &navdata.measure.az,
139  &navdata.measure.vx,
140  &navdata.measure.vy,
141  &navdata.measure.vz,
158  &navdata.measure.mx,
159  &navdata.measure.my,
160  &navdata.measure.mz,
163 }
164 #endif
165 
169 bool_t navdata_init()
170 {
171  assert(sizeof(struct navdata_measure_t) == NAVDATA_PACKET_SIZE);
172 
173  // Check if the FD isn't already initialized
174  if (navdata.fd <= 0) {
175  navdata.fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY); //O_NONBLOCK doesn't work
176 
177  if (navdata.fd < 0) {
178  printf("[navdata] Unable to open navdata board connection(/dev/ttyO1)\n");
179  return FALSE;
180  }
181 
182  // Update the settings of the UART connection
183  fcntl(navdata.fd, F_SETFL, 0); //read calls are non blocking
184  //set port options
185  struct termios options;
186  //Get the current options for the port
187  tcgetattr(navdata.fd, &options);
188  //Set the baud rates to 460800
189  cfsetispeed(&options, B460800);
190  cfsetospeed(&options, B460800);
191 
192  options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
193  options.c_iflag = 0; //clear input options
194  options.c_lflag = 0; //clear local options
195  options.c_oflag &= ~OPOST; //clear output options (raw output)
196 
197  //Set the new options for the port
198  tcsetattr(navdata.fd, TCSANOW, &options);
199  }
200 
201  // Reset available flags
202  navdata_available = FALSE;
206 
207  // Set all statistics to 0
211  navdata.packetsRead = 0;
213 
214  // Stop acquisition
216 
217  // Read the baro calibration(blocking)
218  if (!navdata_baro_calib()) {
219  printf("[navdata] Could not acquire baro calibration!\n");
220  return FALSE;
221  }
223 
224  // Start acquisition
226 
227  // Set navboard gpio control
230 
231  // Start navdata reading thread
232  pthread_t navdata_thread;
233  if (pthread_create(&navdata_thread, NULL, navdata_read, NULL) != 0) {
234  printf("[navdata] Could not create navdata reading thread!\n");
235  return FALSE;
236  }
237 
238 #if PERIODIC_TELEMETRY
239  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ARDRONE_NAVDATA, send_navdata);
240 #endif
241 
242  // Set to initialized
244  return TRUE;
245 }
246 
247 
252 static void *navdata_read(void *data __attribute__((unused)))
253 {
254  /* Buffer insert index for reading/writing */
255  static uint8_t buffer_idx = 0;
256 
257  while (TRUE) {
258 
259  // Wait until we are notified to read next data,
260  // i.e. buffer has been copied in navdata_update
261  pthread_mutex_lock(&navdata_mutex);
262  while (navdata_available) {
263  pthread_cond_wait(&navdata_cond, &navdata_mutex);
264  }
265  pthread_mutex_unlock(&navdata_mutex);
266 
267  // Read new bytes
268  int newbytes = read(navdata.fd, navdata_buffer + buffer_idx, NAVDATA_PACKET_SIZE - buffer_idx);
269 
270  // When there was no signal interrupt
271  if (newbytes > 0) {
272  buffer_idx += newbytes;
273  navdata.totalBytesRead += newbytes;
274  }
275 
276  // If we got a full packet
277  if (buffer_idx >= NAVDATA_PACKET_SIZE) {
278  // check if the start byte is correct
279  if (navdata_buffer[0] != NAVDATA_START_BYTE) {
280  uint8_t *pint = memchr(navdata_buffer, NAVDATA_START_BYTE, buffer_idx);
281 
282  // Check if we found the start byte in the read data
283  if (pint != NULL) {
284  memmove(navdata_buffer, pint, NAVDATA_PACKET_SIZE - (pint - navdata_buffer));
285  buffer_idx = pint - navdata_buffer;
286  fprintf(stderr, "[navdata] sync error, startbyte not found, resetting...\n");
287  } else {
288  buffer_idx = 0;
289  }
290  continue;
291  }
292 
293  /* full packet read with startbyte at the beginning, reset insert index */
294  buffer_idx = 0;
295 
296  // Calculating the checksum
297  uint16_t checksum = 0;
298  for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) {
299  checksum += navdata_buffer[i] + (navdata_buffer[i + 1] << 8);
300  }
301 
302  struct navdata_measure_t *new_measurement = (struct navdata_measure_t *)navdata_buffer;
303 
304  // Check if the checksum is ok
305  if (new_measurement->chksum != checksum) {
306  fprintf(stderr, "[navdata] Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",
307  checksum, new_measurement->chksum, checksum - new_measurement->chksum);
309  continue;
310  }
311 
312  // set flag that we have new valid navdata
313  pthread_mutex_lock(&navdata_mutex);
314  navdata_available = TRUE;
315  pthread_mutex_unlock(&navdata_mutex);
316  }
317  }
318 
319  return NULL;
320 }
321 
322 #include "subsystems/imu.h"
323 static void navdata_publish_imu(void)
324 {
330  imu_scale_mag(&imu);
331  uint32_t now_ts = get_sys_time_usec();
332  AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
333  AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
334  AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
335 }
336 
341 {
342  // Check if initialized
343  if (!navdata.is_initialized) {
344  navdata_init();
346  return;
347  }
348 
349  pthread_mutex_lock(&navdata_mutex);
350  // If we got a new navdata packet
351  if (navdata_available) {
352 
353  // Copy the navdata packet
354  memcpy(&navdata.measure, navdata_buffer, NAVDATA_PACKET_SIZE);
355 
356  // reset the flag
357  navdata_available = FALSE;
358  // signal that we copied the buffer and new packet can be read
359  pthread_cond_signal(&navdata_cond);
360  pthread_mutex_unlock(&navdata_mutex);
361 
362  // Check if we missed a packet (our counter and the one from the navdata)
365  fprintf(stderr, "[navdata] Lost frame: %d should have been %d\n",
368  }
370 
371  // Invert byte order so that TELEMETRY works better
372  uint8_t tmp;
374  tmp = p[0];
375  p[0] = p[1];
376  p[1] = tmp;
378  tmp = p[0];
379  p[0] = p[1];
380  p[1] = tmp;
381 
384 
385 #ifdef USE_SONAR
386  // Check if there is a new sonar measurement and update the sonar
387  if (navdata.measure.ultrasound >> 15) {
388  float sonar_meas = (float)((navdata.measure.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
389  AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, sonar_meas);
390  }
391 #endif
392 
394 
396  } else {
397  // no new packet available, still unlock mutex again
398  pthread_mutex_unlock(&navdata_mutex);
399  }
400 }
401 
405 static void navdata_cmd_send(uint8_t cmd)
406 {
407  full_write(navdata.fd, &cmd, 1);
408 }
409 
410 
414 static bool_t navdata_baro_calib(void)
415 {
416  // Start baro calibration acquisition
418 
419  // Receive the calibration (blocking)
420  uint8_t calibBuffer[22];
421  if (full_read(navdata.fd, calibBuffer, sizeof calibBuffer) < 0) {
422  printf("[navdata] Could not read calibration data.");
423  return FALSE;
424  }
425 
426  //Convert the read bytes
427  navdata.bmp180_calib.ac1 = calibBuffer[0] << 8 | calibBuffer[1];
428  navdata.bmp180_calib.ac2 = calibBuffer[2] << 8 | calibBuffer[3];
429  navdata.bmp180_calib.ac3 = calibBuffer[4] << 8 | calibBuffer[5];
430  navdata.bmp180_calib.ac4 = calibBuffer[6] << 8 | calibBuffer[7];
431  navdata.bmp180_calib.ac5 = calibBuffer[8] << 8 | calibBuffer[9];
432  navdata.bmp180_calib.ac6 = calibBuffer[10] << 8 | calibBuffer[11];
433  navdata.bmp180_calib.b1 = calibBuffer[12] << 8 | calibBuffer[13];
434  navdata.bmp180_calib.b2 = calibBuffer[14] << 8 | calibBuffer[15];
435  navdata.bmp180_calib.mb = calibBuffer[16] << 8 | calibBuffer[17];
436  navdata.bmp180_calib.mc = calibBuffer[18] << 8 | calibBuffer[19];
437  navdata.bmp180_calib.md = calibBuffer[20] << 8 | calibBuffer[21];
438 
439  return TRUE;
440 }
441 
446 static void mag_freeze_check(void)
447 {
448  // Thanks to Daren.G.Lee for initial fix on 20140530
449  static int16_t LastMagValue = 0;
450  static int MagFreezeCounter = 0;
451 
452  if (LastMagValue == navdata.measure.mx) {
453  MagFreezeCounter++;
454 
455  // has to have at least 30 times the same value to consider it a frozen magnetometer value
456  if (MagFreezeCounter > 30) {
457  fprintf(stderr, "mag freeze detected, resetting!\n");
458 
459  // set imu_lost flag
462 
463  // Stop acquisition
465 
466  // Reset the hardware of the navboard
468  usleep(20000);
470 
471  // Wait for 40ms for it to boot
472  usleep(40000);
473 
474  // Start the navdata again and reset the counter
476  MagFreezeCounter = 0; // reset counter back to zero
477  }
478  } else {
480  // Reset counter if value _does_ change
481  MagFreezeCounter = 0;
482  }
483  // set last value
484  LastMagValue = navdata.measure.mx;
485 }
486 
492 static void baro_update_logic(void)
493 {
494  static int32_t lastpressval = 0;
495  static uint16_t lasttempval = 0;
496  static int32_t lastpressval_nospike = 0;
497  static uint16_t lasttempval_nospike = 0;
498  static uint8_t temp_or_press_was_updated_last =
499  0; // 0 = press, so we now wait for temp, 1 = temp so we now wait for press
500 
501  static int sync_errors = 0;
502  static int spike_detected = 0;
503 
504  if (temp_or_press_was_updated_last == 0) { // Last update was press so we are now waiting for temp
505  // temp was updated
506  temp_or_press_was_updated_last = TRUE;
507 
508  // This means that press must remain constant
509  if (lastpressval != 0) {
510  // If pressure was updated: this is a sync error
511  if (lastpressval != navdata.measure.pressure) {
512  // wait for temp again
513  temp_or_press_was_updated_last = FALSE;
514  sync_errors++;
515  //printf("Baro-Logic-Error (expected updated temp, got press)\n");
516  }
517  }
518  } else {
519  // press was updated
520  temp_or_press_was_updated_last = FALSE;
521 
522  // This means that temp must remain constant
523  if (lasttempval != 0) {
524  // If temp was updated: this is a sync error
525  if (lasttempval != navdata.measure.temperature_pressure) {
526  // wait for press again
527  temp_or_press_was_updated_last = TRUE;
528  sync_errors++;
529  //printf("Baro-Logic-Error (expected updated press, got temp)\n");
530 
531  } else {
532  // We now got valid pressure and temperature
534  }
535  }
536  }
537 
538  // Detected a pressure switch
539  if (lastpressval != 0 && lasttempval != 0
540  && ABS(lastpressval - navdata.measure.pressure) > ABS(lasttempval - navdata.measure.pressure)) {
542  }
543 
544  // Detected a temprature switch
545  if (lastpressval != 0 && lasttempval != 0
546  && ABS(lasttempval - navdata.measure.temperature_pressure) > ABS(lastpressval - navdata.measure.temperature_pressure)) {
548  }
549 
550  lasttempval = navdata.measure.temperature_pressure;
551  lastpressval = navdata.measure.pressure;
552 
553  /*
554  * It turns out that a lot of navdata boards have a problem (probably interrupt related)
555  * in which reading I2C data and writing uart output data is interrupted very long (50% of 200Hz).
556  * Afterwards, the 200Hz loop tries to catch up lost time but reads the baro too fast swapping the
557  * pressure and temperature values by exceeding the minimal conversion time of the bosh baro. The
558  * normal Parrot firmware seems to be perfectly capable to fly with this, either with excessive use of
559  * the sonar or with software filtering or spike detection. Paparazzi with its tightly coupled baro-altitude
560  * had problems. Since this problems looks not uncommon a detector was made. A lot of evidence is grabbed
561  * with a logic analyzer on the navboard I2C and serial output. The UART CRC is still perfect, the baro
562  * temp-press-temp-press logic is still perfect, so not easy to detect. Temp and pressure are swapped,
563  * and since both can have almost the same value, the size of the spike is not predictable. However at
564  * every spike of at least 3 broken boards the press and temp are byte-wise exactly the same due to
565  * reading them too quickly (trying to catch up for delay that happened earlier due to still non understood
566  * reasons. As pressure is more likely to quickly change, a small (yet unlikely) spike on temperature together with
567  * press==temp yields very good results as a detector, although theoretically not perfect.
568 
569  #samp press temp.
570  50925 39284 34501
571  50926 39287 34501
572  50927 39287 34501
573  50928 39283 34501 // *press good -> baro
574  50929 39283 34501
575  50930 39285 34501 // *press good -> baro
576  50931 39285 34500
577  50932 34500 34500 // press read too soon from chip (<4.5ms) -> ADC register still previous temp value
578  50933 34500 36618 // press not updated, still wrong. Temp is weird: looks like the average of both
579  50934 39284 36618 // new press read, but temp still outdated
580  50935 39284 34501
581  50936 39284 34501 // *press good -> baro
582  50937 39284 34500
583  50938 39281 34500
584  50939 39281 34500
585  50940 39280 34500
586  50941 39280 34502
587  50942 39280 34502
588  50943 39280 34501
589 
590  */
591 
592  // if press and temp are same and temp has jump: neglect the next frame
594  navdata.measure.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40))
595  // dont use next 3 packets
596  spike_detected = 3;
597  }
598 
599  if (spike_detected > 0) {
600  // disable kalman filter use
602 
603  // override both to last good
604  navdata.measure.pressure = lastpressval_nospike;
605  navdata.measure.temperature_pressure = lasttempval_nospike;
606 
607  // Countdown
608  spike_detected--;
609  } else { // both are good
610  lastpressval_nospike = navdata.measure.pressure;
611  lasttempval_nospike = navdata.measure.temperature_pressure;
612  }
613 }
unsigned short uint16_t
Definition: types.h:16
Dispatcher to register actual AHRS implementations.
Generic transmission transport header.
Definition: transport.h:89
void imu_scale_gyro(struct Imu *_imu)
Definition: ahrs_gx3.c:349
#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.
#define FALSE
Definition: std.h:5
static uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.h:39
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:52
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:50
#define TRUE
Definition: std.h:4
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:43
void gpio_set(uint32_t port, uint16_t pin)
Set a gpio output to high level.
Definition: gpio_ardrone.c:54
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:43
void gpio_clear(uint32_t port, uint16_t pin)
Clear a gpio output to low level.
Definition: gpio_ardrone.c:70
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:44
Inertial Measurement Unit interface.
signed long int32_t
Definition: types.h:19
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
struct adc_buf * buf
Definition: adc_arch.c:587
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:51
static float p[2][2]
void gpio_setup_output(uint32_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_ardrone.c:102
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:42