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