Paparazzi UAS v7.0_unstable
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 "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 */
52static void *navdata_read(void *data __attribute__((unused)));
53static void navdata_cmd_send(uint8_t cmd);
54static bool navdata_baro_calib(void);
55static void mag_freeze_check(void);
56static void baro_update_logic(void);
57
58/* Main navdata structure */
60
64static bool navdata_available = false;
65
66/* syncronization variables */
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
92static const struct FloatRates gyro_scale_f = { 4.359f, 4.359f, 4.359f };
93
97static const struct FloatVect3 accel_scale_f = { 19.5f, 19.5f, 19.5f };
98
99static const struct Int32Vect3 accel_neutral = {
100 2048, 2048, 2048
101};
102
106static const struct FloatVect3 mag_scale_f = {16, 16, 16};
107
111ssize_t full_write(int fd, const uint8_t *buf, size_t count)
112{
113 size_t written = 0;
114
115 while (written < count) {
116 ssize_t n = write(fd, buf + written, count - written);
117 if (n < 0) {
118 if (errno == EAGAIN || errno == EWOULDBLOCK) {
119 continue;
120 }
121 return n;
122 }
123 written += n;
124 }
125 return written;
126}
127
131ssize_t full_read(int fd, uint8_t *buf, size_t count)
132{
133 /* Apologies for illiteracy, but we can't overload |read|.*/
134 size_t readed = 0;
135
136 while (readed < count) {
137 ssize_t n = read(fd, buf + readed, count - readed);
138 if (n < 0) {
139 if (errno == EAGAIN || errno == EWOULDBLOCK) {
140 continue;
141 }
142 return n;
143 }
144 readed += n;
145 }
146 return readed;
147}
148
149#if PERIODIC_TELEMETRY
151
152static void send_navdata(struct transport_tx *trans, struct link_device *dev)
153{
154#pragma GCC diagnostic push
155#pragma GCC diagnostic ignored "-Wpragmas"
156#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
187#pragma GCC diagnostic pop
188}
189#endif
190
195{
197
198 /* Check if the FD isn't already initialized */
199 if (navdata.fd <= 0) {
200 navdata.fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY); /* O_NONBLOCK doesn't work */
201
202 if (navdata.fd < 0) {
203 printf("[navdata] Unable to open navdata board connection(/dev/ttyO1)\n");
204 return false;
205 }
206
207 /* Update the settings of the UART connection */
208 fcntl(navdata.fd, F_SETFL, 0); /* read calls are non blocking */
209 /* set port options */
210 struct termios options;
211 /* Get the current options for the port */
213 /* Set the baud rates to 460800 */
216
217 options.c_cflag |= (CLOCAL | CREAD); /* Enable the receiver and set local mode */
218 options.c_iflag = 0; /* clear input options */
219 options.c_lflag = 0; /* clear local options */
220 options.c_oflag &= ~OPOST; //clear output options (raw output)
221
222 //Set the new options for the port
224 }
225
226 // Reset available flags
227 navdata_available = false;
228 navdata.baro_calibrated = false;
229 navdata.baro_available = false;
230 navdata.imu_lost = false;
231
232 // Set all statistics to 0
238
239 /* Stop acquisition */
241
242 /* Read the baro calibration(blocking) */
243 if (!navdata_baro_calib()) {
244 printf("[navdata] Could not acquire baro calibration!\n");
245 return false;
246 }
248
249 /* Set the default scalings/neutrals */
253
254 /* Start acquisition */
256
257 /* Set navboard gpio control */
260
261 /* Start navdata reading thread */
264 printf("[navdata] Could not create navdata reading thread!\n");
265 return false;
266 }
267 pthread_setname_np(navdata_thread, "pprz_navdata_thread");
268
269#if PERIODIC_TELEMETRY
271#endif
272
273 /* Set to initialized */
274 navdata.is_initialized = true;
275 return true;
276}
277
278
283static void *navdata_read(void *data __attribute__((unused)))
284{
285 /* Buffer insert index for reading/writing */
286 static uint8_t buffer_idx = 0;
287
288 while (TRUE) {
289
290 /* Wait until we are notified to read next data,
291 i.e. buffer has been copied in navdata_update */
293 while (navdata_available) {
295 }
297
298 /* Read new bytes */
299 int newbytes = read(navdata.fd, navdata_buffer + buffer_idx, NAVDATA_PACKET_SIZE - buffer_idx);
300
301 /* When there was no signal interrupt */
302 if (newbytes > 0) {
303 buffer_idx += newbytes;
305 }
306
307 /* If we got a full packet */
308 if (buffer_idx >= NAVDATA_PACKET_SIZE) {
309 /* check if the start byte is correct */
312
313 /* Check if we found the start byte in the read data */
314 if (pint != NULL) {
316 buffer_idx = pint - navdata_buffer;
317 fprintf(stderr, "[navdata] sync error, startbyte not found, resetting...\n");
318 } else {
319 buffer_idx = 0;
320 }
321 continue;
322 }
323
324 /* full packet read with startbyte at the beginning, reset insert index */
325 buffer_idx = 0;
326
327 /* Calculate the checksum */
328 uint16_t checksum = 0;
329 for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) {
330 checksum += navdata_buffer[i] + (navdata_buffer[i + 1] << 8);
331 }
332
334
335 /* Check if the checksum is OK */
336 if (new_measurement->chksum != checksum) {
337 fprintf(stderr, "[navdata] Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",
340 continue;
341 }
342
343 /* Set flag that we have new valid navdata */
345 navdata_available = true;
347 }
348 }
349
350 return NULL;
351}
352
353#include "modules/imu/imu.h"
354static void navdata_publish_imu(void)
355{
357 struct Int32Rates gyro = {
361 };
363
364 struct Int32Vect3 accel = {
366 4096 - navdata.measure.ay,
367 4096 - navdata.measure.az
368 };
370
371 struct Int32Vect3 mag = {
375 };
377}
378
383{
384 /* Check if initialized */
385 if (!navdata.is_initialized) {
386 navdata_init();
388 return;
389 }
390
392 /* If we got a new navdata packet */
393 if (navdata_available) {
394
395 /* Copy the navdata packet */
397
398 /* reset the flag */
399 navdata_available = false;
400 /* signal that we copied the buffer and new packet can be read */
403
404 /* Check if we missed a packet (our counter and the one from the navdata) */
407 fprintf(stderr, "[navdata] Lost frame: %d should have been %d\n",
410 }
412
413 /* Invert byte order so that TELEMETRY works better */
414 uint8_t tmp;
416 tmp = p[0];
417 p[0] = p[1];
418 p[1] = tmp;
420 tmp = p[0];
421 p[0] = p[1];
422 p[1] = tmp;
423
426
427#ifdef USE_SONAR
428 /* Check if there is a new sonar measurement and update the sonar */
429 if (navdata.measure.ultrasound >> 15) {
433 }
434#endif
435
437
439 } else {
440 /* no new packet available, still unlock mutex again */
442 }
443}
444
448static void navdata_cmd_send(uint8_t cmd)
449{
450 full_write(navdata.fd, &cmd, 1);
451}
452
453
457static bool navdata_baro_calib(void)
458{
459 /* Start baro calibration acquisition */
461
462 /* Receive the calibration (blocking) */
464 if (full_read(navdata.fd, calibBuffer, sizeof calibBuffer) < 0) {
465 printf("[navdata] Could not read calibration data.");
466 return false;
467 }
468
469 /* Convert the read bytes */
481
482 return true;
483}
484
489static void mag_freeze_check(void)
490{
491 /* Thanks to Daren.G.Lee for initial fix on 20140530 */
492 static int16_t LastMagValue = 0;
493 static int MagFreezeCounter = 0;
494
497
498 /* Has to have at least 30 times the exact same value to consider it a frozen magnetometer value */
499 if (MagFreezeCounter > 30) {
500 fprintf(stderr, "mag freeze detected, resetting!\n");
501
502 /* set imu_lost flag */
503 navdata.imu_lost = true;
505
506 /* Stop acquisition */
508
509 /* Reset the hardware of the navboard */
511 usleep(20000);
513
514 /* Wait for 40ms for it to boot */
515 usleep(40000);
516
517 /* Start the navdata again and reset the counter */
519 MagFreezeCounter = 0; /* reset counter back to zero */
520 }
521 } else {
522 navdata.imu_lost = false;
523 /* Reset counter if value _does_ change */
525 }
526 /* set last value */
528}
529
535static void baro_update_logic(void)
536{
537 static int32_t lastpressval = 0;
538 static uint16_t lasttempval = 0;
539 static int32_t lastpressval_nospike = 0;
540 static uint16_t lasttempval_nospike = 0;
542 0; /* 0 = press, so we now wait for temp, 1 = temp so we now wait for press */
543
544 static int sync_errors = 0;
545 static int spike_detected = 0;
546
547 if (temp_or_press_was_updated_last == 0) { /* Last update was press so we are now waiting for temp */
548 /* temp was updated */
550
551 /* This means that press must remain constant */
552 if (lastpressval != 0) {
553 /* If pressure was updated: this is a sync error */
555 /* wait for temp again */
557 sync_errors++;
558 //printf("Baro-Logic-Error (expected updated temp, got press)\n");
559 }
560 }
561 } else {
562 /* press was updated */
564
565 /* This means that temp must remain constant */
566 if (lasttempval != 0) {
567 /* If temp was updated: this is a sync error */
569 /* wait for press again */
571 sync_errors++;
572 //printf("Baro-Logic-Error (expected updated press, got temp)\n");
573
574 } else {
575 /* We now got valid pressure and temperature */
576 navdata.baro_available = true;
577 }
578 }
579 }
580
581 /* Detected a pressure swap */
582 if (lastpressval != 0 && lasttempval != 0
584 navdata.baro_available = false;
585 }
586
587 /* Detected a temprature swap */
588 if (lastpressval != 0 && lasttempval != 0
590 navdata.baro_available = false;
591 }
592
595
596 /*
597 * It turns out that a lot of navdata boards have a problem (probably interrupt related)
598 * in which reading I2C data and writing uart output data is interrupted very long (50% of 200Hz).
599 * Afterwards, the 200Hz loop tries to catch up lost time but reads the baro too fast swapping the
600 * pressure and temperature values by exceeding the minimal conversion time of the bosh baro. The
601 * normal Parrot firmware seems to be perfectly capable to fly with this, either with excessive use of
602 * the sonar or with software filtering or spike detection. Paparazzi with its tightly coupled baro-altitude
603 * had problems. Since this problems looks not uncommon a detector was made. A lot of evidence is grabbed
604 * with a logic analyzer on the navboard I2C and serial output. The UART CRC is still perfect, the baro
605 * temp-press-temp-press logic is still perfect, so not easy to detect. Temp and pressure are swapped,
606 * and since both can have almost the same value, the size of the spike is not predictable. However at
607 * every spike of at least 3 broken boards the press and temp are byte-wise exactly the same due to
608 * reading them too quickly (trying to catch up for delay that happened earlier due to still non understood
609 * reasons. As pressure is more likely to quickly change, a small (yet unlikely) spike on temperature together with
610 * press==temp yields very good results as a detector, although theoretically not perfect.
611
612 #samp press temp.
613 50925 39284 34501
614 50926 39287 34501
615 50927 39287 34501
616 50928 39283 34501 // *press good -> baro
617 50929 39283 34501
618 50930 39285 34501 // *press good -> baro
619 50931 39285 34500
620 50932 34500 34500 // press read too soon from chip (<4.5ms) -> ADC register still previous temp value
621 50933 34500 36618 // press not updated, still wrong. Temp is weird: looks like the average of both
622 50934 39284 36618 // new press read, but temp still outdated
623 50935 39284 34501
624 50936 39284 34501 // *press good -> baro
625 50937 39284 34500
626 50938 39281 34500
627 50939 39281 34500
628 50940 39280 34500
629 50941 39280 34502
630 50942 39280 34502
631 50943 39280 34501
632
633 */
634
635 /* If press and temp are same and temp has jump: neglect the next frame */
637 navdata.measure.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40))
638 /* dont use the next 3 packets */
639 spike_detected = 3;
640 }
641
642 if (spike_detected > 0) {
643 /* disable kalman filter use */
644 navdata.baro_available = false;
645
646 // override both to last good
649
650 /* Countdown */
652 } else { // both are good
655 }
656}
Main include for ABI (AirBorneInterface).
#define AGL_SONAR_ARDRONE2_ID
#define IMU_BOARD_ID
static uint8_t checksum
#define ARDRONE_GPIO_PORT
Definition actuators.c:58
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
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
Definition gpio_arch.h:104
static void gpio_clear(ioportid_t port, uint16_t pin)
Clear a gpio output to low level.
Definition gpio_arch.h:114
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
#define B460800
Definition uart_arch.h:50
Some architecture independent helper functions for GPIOs.
angular rates
angular rates
void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition imu.c:612
void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition imu.c:641
void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
Set the defaults for a mag sensor WARNING: Should be called before sensor is publishing messages to e...
Definition imu.c:670
Inertial Measurement Unit interface.
Integrated Navigation System interface.
static float p[2][2]
uint16_t foo
Definition main_demo5.c:58
static void mag_freeze_check(void)
Check if the magneto is frozen Unknown why this bug happens.
Definition navdata.c:489
static bool navdata_baro_calib(void)
Try to receive the baro calibration from the navdata board.
Definition navdata.c:457
static void send_navdata(struct transport_tx *trans, struct link_device *dev)
Definition navdata.c:152
static const struct FloatVect3 mag_scale_f
Default mag scale.
Definition navdata.c:106
ssize_t full_write(int fd, const uint8_t *buf, size_t count)
Write to fd even while being interrupted.
Definition navdata.c:111
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:283
static void baro_update_logic(void)
Handle the baro(pressure/temperature) logic Sometimes the temperature and pressure are switched becau...
Definition navdata.c:535
void navdata_update()
Update the navdata (event loop)
Definition navdata.c:382
static const struct FloatVect3 accel_scale_f
Default accel scale/neutral.
Definition navdata.c:97
static const struct Int32Vect3 accel_neutral
Definition navdata.c:99
static void navdata_cmd_send(uint8_t cmd)
Sends a one byte command.
Definition navdata.c:448
static uint8_t navdata_buffer[NAVDATA_PACKET_SIZE]
Buffer filled in the thread (maximum one navdata packet)
Definition navdata.c:62
static const struct FloatRates gyro_scale_f
Default gyro scale.
Definition navdata.c:92
#define SONAR_SCALE
Sonar scale.
Definition navdata.c:86
struct navdata_t navdata
Definition navdata.c:59
static pthread_mutex_t navdata_mutex
Definition navdata.c:67
bool navdata_init()
Initialize the navdata board.
Definition navdata.c:194
static bool navdata_available
flag to indicate new packet is available in buffer
Definition navdata.c:64
#define SONAR_OFFSET
Sonar offset.
Definition navdata.c:79
static void navdata_publish_imu(void)
Definition navdata.c:354
ssize_t full_read(int fd, uint8_t *buf, size_t count)
Read from fd even while being interrupted.
Definition navdata.c:131
static pthread_cond_t navdata_cond
Definition navdata.c:68
ardrone2 navdata aquisition driver.
int32_t pressure
Definition navdata.h:75
uint16_t nb_echo
Definition navdata.h:68
struct bmp180_calib_t bmp180_calib
BMP180 calibration receieved from navboard.
Definition navdata.h:126
int16_t ac2
Definition navdata.h:89
uint16_t ac4
Definition navdata.h:91
uint32_t totalBytesRead
Definition navdata.h:119
int16_t gradient
Definition navdata.h:71
uint16_t temperature_gyro
Definition navdata.h:55
int16_t ac3
Definition navdata.h:90
uint16_t ay
Definition navdata.h:48
int16_t mc
Definition navdata.h:97
uint16_t us_debut_echo
Definition navdata.h:59
struct navdata_measure_t measure
Main navdata packet receieved from navboard.
Definition navdata.h:125
bool imu_lost
Whenever the imu is lost.
Definition navdata.h:130
#define NAVDATA_CMD_STOP
Definition navdata.h:108
int16_t ac1
Definition navdata.h:88
uint16_t flag_echo_ini
Definition navdata.h:73
uint16_t ac6
Definition navdata.h:93
uint32_t lost_imu_frames
Definition navdata.h:122
bool baro_calibrated
Whenever the baro is calibrated.
Definition navdata.h:128
#define NAVDATA_START_BYTE
Definition navdata.h:106
uint32_t packetsRead
Definition navdata.h:120
uint16_t chksum
Definition navdata.h:82
uint16_t ultrasound
Definition navdata.h:57
uint16_t us_curve_ref
Definition navdata.h:66
uint16_t nu_trame
Definition navdata.h:45
#define ARDRONE_GPIO_PIN_NAVDATA
Definition navdata.h:112
uint16_t taille
Definition navdata.h:44
int16_t b1
Definition navdata.h:94
int fd
The navdata file pointer.
Definition navdata.h:117
uint16_t temperature_acc
Definition navdata.h:54
bool baro_available
Whenever the baro is available.
Definition navdata.h:129
int16_t md
Definition navdata.h:98
uint16_t us_distance_echo
Definition navdata.h:62
uint16_t us_curve_value
Definition navdata.h:65
#define NAVDATA_CMD_START
Definition navdata.h:107
uint32_t checksum_errors
Definition navdata.h:121
#define NAVDATA_CMD_BARO_CALIB
Definition navdata.h:109
int16_t b2
Definition navdata.h:95
uint16_t us_fin_echo
Definition navdata.h:60
uint16_t last_packet_number
Definition navdata.h:123
uint16_t ac5
Definition navdata.h:92
uint32_t sum_echo
Definition navdata.h:70
#define NAVDATA_PACKET_SIZE
Definition navdata.h:105
bool is_initialized
Check if the navdata board is initialized.
Definition navdata.h:116
uint16_t us_association_echo
Definition navdata.h:61
uint16_t az
Definition navdata.h:49
uint16_t us_curve_time
Definition navdata.h:64
int16_t mb
Definition navdata.h:96
uint16_t temperature_pressure
Definition navdata.h:76
uint16_t ax
Definition navdata.h:47
Main navdata structure from the navdata board This is received from the navdata board at ~200Hz.
Definition navdata.h:43
int fd
Definition serial.c:26
#define TRUE
Definition std.h:4
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.