 |
Paparazzi UAS
v6.1.0_stable
Paparazzi is a free software Unmanned Aircraft System.
|
Go to the documentation of this file.
52 static void *
navdata_read(
void *data __attribute__((unused)));
70 #ifndef NAVDATA_FILTER_ID
71 #define NAVDATA_FILTER_ID 2
79 #define SONAR_OFFSET 880
86 #define SONAR_SCALE 0.00047
96 while (written < count) {
97 ssize_t n = write(
fd, buf + written, count - written);
99 if (errno == EAGAIN || errno == EWOULDBLOCK) {
117 while (readed < count) {
118 ssize_t n = read(
fd, buf + readed, count - readed);
120 if (errno == EAGAIN || errno == EWOULDBLOCK) {
130 #if PERIODIC_TELEMETRY
135 #pragma GCC diagnostic push
136 #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
137 pprz_msg_send_ARDRONE_NAVDATA(trans,
dev, AC_ID,
167 #pragma GCC diagnostic pop
180 navdata.
fd = open(
"/dev/ttyO1", O_RDWR | O_NOCTTY);
183 printf(
"[navdata] Unable to open navdata board connection(/dev/ttyO1)\n");
190 struct termios options;
194 cfsetispeed(&options,
B460800);
195 cfsetospeed(&options,
B460800);
197 options.c_cflag |= (CLOCAL | CREAD);
200 options.c_oflag &= ~OPOST;
203 tcsetattr(
navdata.
fd, TCSANOW, &options);
224 printf(
"[navdata] Could not acquire baro calibration!\n");
237 pthread_t navdata_thread;
238 if (pthread_create(&navdata_thread, NULL,
navdata_read, NULL) != 0) {
239 printf(
"[navdata] Could not create navdata reading thread!\n");
242 pthread_setname_np(navdata_thread,
"pprz_navdata_thread");
244 #if PERIODIC_TELEMETRY
278 buffer_idx += newbytes;
292 fprintf(stderr,
"[navdata] sync error, startbyte not found, resetting...\n");
312 fprintf(stderr,
"[navdata] Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",
371 fprintf(stderr,
"[navdata] Lost frame: %d should have been %d\n",
429 printf(
"[navdata] Could not read calibration data.");
456 static int16_t LastMagValue = 0;
457 static int MagFreezeCounter = 0;
463 if (MagFreezeCounter > 30) {
464 fprintf(stderr,
"mag freeze detected, resetting!\n");
483 MagFreezeCounter = 0;
488 MagFreezeCounter = 0;
501 static int32_t lastpressval = 0;
503 static int32_t lastpressval_nospike = 0;
504 static uint16_t lasttempval_nospike = 0;
505 static uint8_t temp_or_press_was_updated_last =
508 static int sync_errors = 0;
509 static int spike_detected = 0;
511 if (temp_or_press_was_updated_last == 0) {
513 temp_or_press_was_updated_last =
true;
516 if (lastpressval != 0) {
520 temp_or_press_was_updated_last =
false;
527 temp_or_press_was_updated_last =
false;
530 if (lasttempval != 0) {
534 temp_or_press_was_updated_last =
true;
546 if (lastpressval != 0 && lasttempval != 0
552 if (lastpressval != 0 && lasttempval != 0
606 if (spike_detected > 0) {
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
void imu_scale_accel(struct Imu *_imu)
bool is_initialized
Check if the navdata board is initialized.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
struct navdata_measure_t measure
Main navdata packet receieved from navboard.
uint16_t last_packet_number
static void * navdata_read(void *data)
Main reading thread This is done asynchronous because the navdata board doesn't support NON_BLOCKING.
#define SONAR_OFFSET
Sonar offset.
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
static void baro_update_logic(void)
Handle the baro(pressure/temperature) logic Sometimes the temperature and pressure are switched becau...
#define NAVDATA_START_BYTE
void gpio_setup_output(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
void navdata_update()
Update the navdata (event loop)
static bool navdata_baro_calib(void)
Try to receive the baro calibration from the navdata board.
static const struct usb_device_descriptor dev
#define NAVDATA_CMD_BARO_CALIB
uint16_t temperature_gyro
#define AGL_SONAR_ARDRONE2_ID
Main navdata structure from the navdata board This is received from the navdata board at ~200Hz.
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
#define SONAR_SCALE
Sonar scale.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static void gpio_clear(ioportid_t port, uint16_t pin)
Clear a gpio output to low level.
short int16_t
Typedef defining 16 bit short type.
ssize_t full_read(int fd, uint8_t *buf, size_t count)
Read from fd even while being interrupted.
#define RATES_ASSIGN(_ra, _p, _q, _r)
static void navdata_publish_imu(void)
uint16_t temperature_pressure
static void send_navdata(struct transport_tx *trans, struct link_device *dev)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
#define NAVDATA_PACKET_SIZE
bool baro_calibrated
Whenever the baro is calibrated.
struct Imu imu
global IMU state
#define ARDRONE_GPIO_PIN_NAVDATA
static bool navdata_available
flag to indicate new packet is available in buffer
static void navdata_cmd_send(uint8_t cmd)
Sends a one byte command.
uint16_t us_association_echo
int int32_t
Typedef defining 32 bit int type.
#define ARDRONE_GPIO_PORT
bool navdata_init()
Initialize the navdata board.
void imu_scale_mag(struct Imu *_imu)
int fd
The navdata file pointer.
bool baro_available
Whenever the baro is available.
void imu_scale_gyro(struct Imu *_imu)
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
bool imu_lost
Whenever the imu is lost.
static uint8_t navdata_buffer[NAVDATA_PACKET_SIZE]
Buffer filled in the thread (maximum one navdata packet)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define NAVDATA_CMD_START
static pthread_mutex_t navdata_mutex
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
ssize_t full_write(int fd, const uint8_t *buf, size_t count)
Write to fd even while being interrupted.
static void mag_freeze_check(void)
Check if the magneto is frozen Unknown why this bug happens.
struct bmp180_calib_t bmp180_calib
BMP180 calibration receieved from navboard.
#define DefaultPeriodic
Set default periodic telemetry.
uint16_t us_distance_echo
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
static pthread_cond_t navdata_cond