Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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  *
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
30 #include <stdio.h>
31 #include <stdlib.h>
32 #include <fcntl.h> // for O_RDWR, O_NOCTTY, O_NONBLOCK
33 #include <termios.h> // for baud rates and options
34 #include <unistd.h>
35 #include <string.h>
36 #include <math.h>
37 #include <errno.h>
38 #include <assert.h>
39 
40 #include "std.h"
41 #include "navdata.h"
42 #include "subsystems/ins.h"
43 #include "subsystems/abi.h"
44 #include "mcu_periph/gpio.h"
45 
46 #define NAVDATA_PACKET_SIZE 60
47 #define NAVDATA_START_BYTE 0x3a
48 
49 #define ARDRONE_GPIO_PORT 0x32524
50 #define ARDRONE_GPIO_PIN_NAVDATA 177
51 
52 static inline bool_t acquire_baro_calibration(void);
53 static void navdata_cropbuffer(int cropsize);
54 
56 static int nav_fd = 0;
58 
63 #ifndef SONAR_OFFSET
64 #define SONAR_OFFSET 880
65 #endif
66 
70 #ifndef SONAR_SCALE
71 #define SONAR_SCALE 0.00047
72 #endif
73 
74 // FIXME(ben): there must be a better home for these
75 ssize_t full_write(int fd, const uint8_t *buf, size_t count)
76 {
77  size_t written = 0;
78 
79  while(written < count)
80  {
81  ssize_t n = write(fd, buf + written, count - written);
82  if (n < 0)
83  {
84  if (errno == EAGAIN || errno == EWOULDBLOCK)
85  continue;
86  return n;
87  }
88  written += n;
89  }
90  return written;
91 }
92 
93 ssize_t full_read(int fd, uint8_t *buf, size_t count)
94 {
95  // Apologies for illiteracy, but we can't overload |read|.
96  size_t readed = 0;
97 
98  while(readed < count)
99  {
100  ssize_t n = read(fd, buf + readed, count - readed);
101  if (n < 0)
102  {
103  if (errno == EAGAIN || errno == EWOULDBLOCK)
104  continue;
105  return n;
106  }
107  readed += n;
108  }
109  return readed;
110 }
111 
112 static void navdata_write(const uint8_t *buf, size_t count)
113 {
114  if (full_write(nav_fd, buf, count) < 0)
115  perror("navdata_write: Write failed");
116 }
117 
118 #if PERIODIC_TELEMETRY
120 
121 static void send_navdata(void) {
122  DOWNLINK_SEND_ARDRONE_NAVDATA(DefaultChannel, DefaultDevice,
123  &navdata.taille,
124  &navdata.nu_trame,
125  &navdata.ax,
126  &navdata.ay,
127  &navdata.az,
128  &navdata.vx,
129  &navdata.vy,
130  &navdata.vz,
131  &navdata.temperature_acc,
132  &navdata.temperature_gyro,
133  &navdata.ultrasound,
134  &navdata.us_debut_echo,
135  &navdata.us_fin_echo,
136  &navdata.us_association_echo,
137  &navdata.us_distance_echo,
138  &navdata.us_curve_time,
139  &navdata.us_curve_value,
140  &navdata.us_curve_ref,
141  &navdata.nb_echo,
142  &navdata.sum_echo,
143  &navdata.gradient,
144  &navdata.flag_echo_ini,
145  &navdata.pressure,
146  &navdata.temperature_pressure,
147  &navdata.mx,
148  &navdata.my,
149  &navdata.mz,
150  &navdata.chksum,
151  &nav_port.checksum_errors);
152 }
153 #endif
154 
155 bool_t navdata_init()
156 {
157  if (nav_fd <= 0) {
158  nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
159 
160  if (nav_fd == -1) {
161  perror("navdata_init: Unable to open /dev/ttyO1 - ");
162  return FALSE;
163  }
164  }
165 
166  fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking
167  //set port options
168  struct termios options;
169  //Get the current options for the port
170  tcgetattr(nav_fd, &options);
171  //Set the baud rates to 460800
172  cfsetispeed(&options, B460800);
173  cfsetospeed(&options, B460800);
174 
175  options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
176  options.c_iflag = 0; //clear input options
177  options.c_lflag = 0; //clear local options
178  options.c_oflag &= ~OPOST; //clear output options (raw output)
179 
180  //Set the new options for the port
181  tcsetattr(nav_fd, TCSANOW, &options);
182 
183  // stop acquisition
184  uint8_t cmd=0x02;
185  navdata_write(&cmd, 1);
186 
187  // read some potential dirt
188  // wait 10 milliseconds
189  char tmp[100];
190  for(int i = 0; i < 12; i++) {
191  uint16_t dirt = read(nav_fd, tmp, sizeof tmp);
192  (void) dirt;
193 
194  usleep(1000);
195  }
196 
199  return FALSE;
200 
201  // start acquisition
202  cmd = 0x01;
203  navdata_write(&cmd, 1);
204 
207 
208  nav_port.checksum_errors = 0;
209  nav_port.lost_imu_frames = 0;
210  nav_port.bytesRead = 0;
211  nav_port.totalBytesRead = 0;
212  nav_port.packetsRead = 0;
213  nav_port.isInitialized = TRUE;
214  nav_port.last_packet_number = 0;
215 
216  // set navboard gpio control
219 
220 #if PERIODIC_TELEMETRY
221  register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata);
222 #endif
223 
224  return TRUE;
225 }
226 
227 static inline bool_t acquire_baro_calibration(void)
228 {
229  // start baro calibration acquisition
230  uint8_t cmd=0x17; // send cmd 23
231  navdata_write(&cmd, 1);
232 
233  // wait 20ms to retrieve data
234  for (int i=0;i<22;i++)
235  {
236  usleep(1000);
237  }
238 
239  uint8_t calibBuffer[22];
240 
241  if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0)
242  {
243  perror("acquire_baro_calibration: read failed");
244  return FALSE;
245  }
246 
247  baro_calibration.ac1 = calibBuffer[0] << 8 | calibBuffer[1];
248  baro_calibration.ac2 = calibBuffer[2] << 8 | calibBuffer[3];
249  baro_calibration.ac3 = calibBuffer[4] << 8 | calibBuffer[5];
250  baro_calibration.ac4 = calibBuffer[6] << 8 | calibBuffer[7];
251  baro_calibration.ac5 = calibBuffer[8] << 8 | calibBuffer[9];
252  baro_calibration.ac6 = calibBuffer[10] << 8 | calibBuffer[11];
253  baro_calibration.b1 = calibBuffer[12] << 8 | calibBuffer[13];
254  baro_calibration.b2 = calibBuffer[14] << 8 | calibBuffer[15];
255  baro_calibration.mb = calibBuffer[16] << 8 | calibBuffer[17];
256  baro_calibration.mc = calibBuffer[18] << 8 | calibBuffer[19];
257  baro_calibration.md = calibBuffer[20] << 8 | calibBuffer[21];
258 
259  printf("Calibration AC1: %d\n", baro_calibration.ac1);
260  printf("Calibration AC2: %d\n", baro_calibration.ac2);
261  printf("Calibration AC3: %d\n", baro_calibration.ac3);
262  printf("Calibration AC4: %d\n", baro_calibration.ac4);
263  printf("Calibration AC5: %d\n", baro_calibration.ac5);
264  printf("Calibration AC6: %d\n", baro_calibration.ac6);
265 
266  printf("Calibration B1: %d\n", baro_calibration.b1);
267  printf("Calibration B2: %d\n", baro_calibration.b2);
268 
269  printf("Calibration MB: %d\n", baro_calibration.mb);
270  printf("Calibration MC: %d\n", baro_calibration.mc);
271  printf("Calibration MD: %d\n", baro_calibration.md);
272 
274  return TRUE;
275 }
276 
278 {
279  int newbytes = read(nav_fd, nav_port.buffer+nav_port.bytesRead, NAVDATA_BUFFER_SIZE-nav_port.bytesRead);
280 
281  // because non-blocking read returns -1 when no bytes available
282  if (newbytes > 0)
283  {
284  nav_port.bytesRead += newbytes;
285  nav_port.totalBytesRead += newbytes;
286  }
287 }
288 
289 static void mag_freeze_check(void) {
290  // from daren.g.lee paparazzi-l 20140530
291  static int16_t LastMagValue = 0;
292  static int MagFreezeCounter = 0;
293 
294  // printf("lm: %d, mx: %d, mfc: %d\n", LastMagValue, navdata.mx, MagFreezeCounter);
295 
296  if (LastMagValue == navdata.mx) {
297  MagFreezeCounter++;
298  // Re-initialize the serial port here, in paparazzi this should be ~150 ms
299  // considering it updates at 200 Hz
300  if (MagFreezeCounter > 30) {
301  printf("Mag needs resetting, Values are frozen!!! %d , %d \n", LastMagValue, navdata.mx);
302  printf("Setting GPIO 177 to reset PIC Navigation Board \n");
303 
304  // stop acquisition
305  uint8_t cmd=0x02;
306  navdata_write(&cmd, 1);
307 
308  // do the navboard reset via GPIOs
311 
312  // wait 20ms to retrieve data
313  usleep(20000);
314 
315  // restart acquisition
316  cmd = 0x01;
317  navdata_write(&cmd, 1);
318 
319  MagFreezeCounter = 0; // reset counter back to zero
320  }
321  }
322  else {
323  // remember to reset if value _does_ change
324  MagFreezeCounter = 0;
325  }
326  // set last value
327  LastMagValue = navdata.mx;
328 }
329 
330 static void baro_update_logic(void)
331 {
332  static int32_t lastpressval = 0;
333  static uint16_t lasttempval = 0;
334  static int32_t lastpressval_nospike = 0;
335  static uint16_t lasttempval_nospike = 0;
336  static uint8_t temp_or_press_was_updated_last = 0; // 0 = press, so we now wait for temp, 1 = temp so we now wait for press
337 
338  static int sync_errors = 0;
339  static int spikes = 0;
340  static int spike_detected = 0;
341 
342  if (temp_or_press_was_updated_last == 0) // Last update was press so we are now waiting for temp
343  {
344  // temp was updated
345  temp_or_press_was_updated_last = TRUE;
346 
347  // This means that press must remain constant
348  if (lastpressval != 0)
349  {
350  // If pressure was updated: this is a sync error
351  if (lastpressval != navdata.pressure)
352  {
353  // wait for temp again
354  temp_or_press_was_updated_last = FALSE;
355  sync_errors++;
356  printf("Baro-Logic-Error (expected updated temp, got press)\n");
357  }
358  }
359  }
360  else
361  {
362  // press was updated
363  temp_or_press_was_updated_last = FALSE;
364 
365  // This means that temp must remain constant
366  if (lasttempval != 0)
367  {
368  // If temp was updated: this is a sync error
369  if (lasttempval != navdata.temperature_pressure)
370  {
371  // wait for press again
372  temp_or_press_was_updated_last = TRUE;
373  sync_errors++;
374  printf("Baro-Logic-Error (expected updated press, got temp)\n");
375 
376  }
377  else {
378  // We now got valid pressure and temperature
380  }
381  }
382  }
383 
384  // Detected a pressure switch
385  if(lastpressval != 0 && lasttempval != 0 && ABS(lastpressval - navdata.pressure) > ABS(lasttempval - navdata.pressure)) {
387  }
388 
389  // Detected a temprature switch
390  if(lastpressval != 0 && lasttempval != 0 && ABS(lasttempval - navdata.temperature_pressure) > ABS(lastpressval - navdata.temperature_pressure)) {
392  }
393 
394  lasttempval = navdata.temperature_pressure;
395  lastpressval = navdata.pressure;
396 
397  /*
398  * It turns out that a lot of navdata boards have a problem (probably interrupt related)
399  * in which reading I2C data and writing uart output data is interrupted very long (50% of 200Hz).
400  * Afterwards, the 200Hz loop tries to catch up lost time but reads the baro too fast swapping the
401  * pressure and temperature values by exceeding the minimal conversion time of the bosh baro. The
402  * normal Parrot firmware seems to be perfectly capable to fly with this, either with excessive use of
403  * the sonar or with software filtering or spike detection. Paparazzi with its tightly coupled baro-altitude
404  * had problems. Since this problems looks not uncommon a detector was made. A lot of evidence is grabbed
405  * with a logic analyzer on the navboard I2C and serial output. The UART CRC is still perfect, the baro
406  * temp-press-temp-press logic is still perfect, so not easy to detect. Temp and pressure are swapped,
407  * and since both can have almost the same value, the size of the spike is not predictable. However at
408  * every spike of at least 3 broken boards the press and temp are byte-wise exactly the same due to
409  * reading them too quickly (trying to catch up for delay that happened earlier due to still non understood
410  * reasons. As pressure is more likely to quickly change, a small (yet unlikely) spike on temperature together with
411  * press==temp yields very good results as a detector, although theoretically not perfect.
412 
413 #samp press temp.
414 50925 39284 34501
415 50926 39287 34501
416 50927 39287 34501
417 50928 39283 34501 // *press good -> baro
418 50929 39283 34501
419 50930 39285 34501 // *press good -> baro
420 50931 39285 34500
421 50932 34500 34500 // press read too soon from chip (<4.5ms) -> ADC register still previous temp value
422 50933 34500 36618 // press not updated, still wrong. Temp is weird: looks like the average of both
423 50934 39284 36618 // new press read, but temp still outdated
424 50935 39284 34501
425 50936 39284 34501 // *press good -> baro
426 50937 39284 34500
427 50938 39281 34500
428 50939 39281 34500
429 50940 39280 34500
430 50941 39280 34502
431 50942 39280 34502
432 50943 39280 34501
433 
434  */
435 
436  // if press and temp are same and temp has jump: neglect the next frame
437  if (navdata.temperature_pressure == navdata.pressure) // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40))
438  {
439  // dont use next 3 packets
440  spike_detected = 3;
441 
442  spikes++;
443  printf("Spike! # %d\n",spikes);
444  }
445 
446  if (spike_detected > 0)
447  {
448  // disable kalman filter use
450 
451  // override both to last good
452  navdata.pressure = lastpressval_nospike;
453  navdata.temperature_pressure = lasttempval_nospike;
454 
455  // Countdown
456  spike_detected--;
457  }
458  else // both are good
459  {
460  lastpressval_nospike = navdata.pressure;
461  lasttempval_nospike = navdata.temperature_pressure;
462  }
463 
464 // printf("%d %d %d\r\n", navdata.temperature_pressure, navdata.pressure, spike_detected);
465 // printf(",%d,%d",spike_detected,spikes);
466 }
467 
469 {
470  static bool_t last_checksum_wrong = FALSE;
471  // Check if initialized
472  if (!nav_port.isInitialized) {
473  navdata_init();
474  return;
475  }
476 
477  // Start reading
478  navdata_read();
479 
480  // while there is something interesting to do...
481  while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE)
482  {
483  if (nav_port.buffer[0] == NAVDATA_START_BYTE)
484  {
485  assert(sizeof navdata == NAVDATA_PACKET_SIZE);
486  memcpy(&navdata, nav_port.buffer, NAVDATA_PACKET_SIZE);
487 
488  // Calculating the checksum
489  uint16_t checksum = 0;
490  for(int i = 2; i < NAVDATA_PACKET_SIZE-2; i += 2) {
491  checksum += nav_port.buffer[i] + (nav_port.buffer[i+1] << 8);
492  }
493 
494  // When checksum is incorrect
495  if(navdata.chksum != checksum) {
496  printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",checksum , navdata.chksum, checksum-navdata.chksum);
497  nav_port.checksum_errors++;
498  }
499 
500  nav_port.last_packet_number++;
501  if (nav_port.last_packet_number != navdata.nu_trame)
502  {
503  printf("Lost Navdata frame: %d should have been %d\n",navdata.nu_trame, nav_port.last_packet_number);
504  nav_port.lost_imu_frames++;
505  }
506  nav_port.last_packet_number = navdata.nu_trame;
507  //printf("%d\r",navdata.nu_trame);
508 
509  // When we already dropped a packet or checksum is correct
510  if(last_checksum_wrong || navdata.chksum == checksum) {
511  // Invert byte order so that TELEMETRY works better
512  uint8_t tmp;
513  uint8_t* p = (uint8_t*) &(navdata.pressure);
514  tmp = p[0];
515  p[0] = p[1];
516  p[1] = tmp;
517  p = (uint8_t*) &(navdata.temperature_pressure);
518  tmp = p[0];
519  p[0] = p[1];
520  p[1] = tmp;
521 
522 // printf("%d,%d,%d",navdata.nu_trame, navdata.pressure, navdata.temperature_pressure);
523 
525 
526 // printf(",%d,%d,%d\n", navdata.pressure, navdata.temperature_pressure, (int)navdata_baro_available);
527 
529 
530 #ifdef USE_SONAR
531  // Check if there is a new sonar measurement and update the sonar
532  if (navdata.ultrasound >> 15)
533  {
534  float sonar_meas = (float)((navdata.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
535  AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas);
536  }
537 #endif
538 
540  last_checksum_wrong = FALSE;
541  nav_port.packetsRead++;
542  }
543 
544  // Crop the buffer
545  navdata_cropbuffer(NAVDATA_PACKET_SIZE);
546  }
547  else
548  {
549  // find start byte, copy all data from startbyte to buffer origin, update bytesread
550  uint8_t * pint;
551  pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead);
552 
553  if (pint != NULL) {
554  navdata_cropbuffer(pint - nav_port.buffer);
555  } else {
556  // if the start byte was not found, it means there is junk in the buffer
557  nav_port.bytesRead = 0;
558  }
559  }
560  }
561 
562 }
563 
564 static void navdata_cropbuffer(int cropsize)
565 {
566  if (nav_port.bytesRead - cropsize < 0) {
567  // TODO think about why the amount of bytes read minus the cropsize gets below zero
568  printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", nav_port.bytesRead, cropsize, nav_port.buffer);
569  return;
570  }
571 
572  memmove(nav_port.buffer, nav_port.buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
573  nav_port.bytesRead -= cropsize;
574 }
int16_t mz
Definition: navdata.h:74
unsigned short uint16_t
Definition: types.h:16
int16_t gradient
Definition: navdata.h:65
uint16_t us_curve_time
Definition: navdata.h:58
uint8_t buffer[NAVDATA_BUFFER_SIZE]
Definition: navdata.h:107
Periodic telemetry system header (includes downlink utility and generated code).
uint16_t us_fin_echo
Definition: navdata.h:54
Some architecture independent helper functions for GPIOs.
uint16_t us_association_echo
Definition: navdata.h:55
void gpio_set(uint32_t port, uint16_t pin)
Set a gpio output to high level.
Main include for ABI (AirBorneInterface).
uint32_t totalBytesRead
Definition: navdata.h:102
Integrated Navigation System interface.
int fd
int16_t vx
Definition: navdata.h:45
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
uint16_t us_debut_echo
Definition: navdata.h:53
uint16_t ay
Definition: navdata.h:42
uint16_t bytesRead
Definition: navdata.h:101
#define FALSE
Definition: imu_chimu.h:141
uint32_t lost_imu_frames
Definition: navdata.h:105
int16_t vz
Definition: navdata.h:47
uint16_t az
Definition: navdata.h:43
uint16_t temperature_pressure
Definition: navdata.h:70
int16_t my
Definition: navdata.h:72
uint32_t sum_echo
Definition: navdata.h:64
uint16_t taille
Definition: navdata.h:38
int32_t pressure
Definition: navdata.h:69
signed short int16_t
Definition: types.h:17
uint16_t us_curve_value
Definition: navdata.h:59
int16_t mx
Definition: navdata.h:73
uint16_t nu_trame
Definition: navdata.h:39
signed long int32_t
Definition: types.h:19
uint16_t last_packet_number
Definition: navdata.h:106
void gpio_clear(uint32_t port, uint16_t pin)
Clear a gpio output to low level.
#define TRUE
Definition: imu_chimu.h:144
struct adc_buf * buf
Definition: adc_arch.c:586
uint16_t chksum
Definition: navdata.h:76
unsigned char uint8_t
Definition: types.h:14
static void gpio_setup_output(uint32_t port, uint32_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.h:76
uint32_t packetsRead
Definition: navdata.h:103
uint16_t us_distance_echo
Definition: navdata.h:56
#define AGL_SONAR_ARDRONE2_ID
static float p[2][2]
uint16_t us_curve_ref
Definition: navdata.h:60
uint16_t temperature_gyro
Definition: navdata.h:49
uint32_t checksum_errors
Definition: navdata.h:104
int16_t vy
Definition: navdata.h:46
uint16_t flag_echo_ini
Definition: navdata.h:67
uint16_t nb_echo
Definition: navdata.h:62
uint16_t ultrasound
Definition: navdata.h:51
uint16_t ax
Definition: navdata.h:41
uint16_t temperature_acc
Definition: navdata.h:48
uint8_t isInitialized
Definition: navdata.h:100