Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
navdata.c File Reference

ARDrone2 navdata aquisition driver. More...

#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <errno.h>
#include <assert.h>
#include <pthread.h>
#include "std.h"
#include "navdata.h"
#include "subsystems/ins.h"
#include "subsystems/ahrs.h"
#include "subsystems/abi.h"
#include "mcu_periph/gpio.h"
#include "subsystems/datalink/telemetry.h"
#include "subsystems/imu.h"
+ Include dependency graph for navdata.c:

Go to the source code of this file.

Macros

#define NAVDATA_FILTER_ID   2
 
#define SONAR_OFFSET   880
 Sonar offset. More...
 
#define SONAR_SCALE   0.00047
 Sonar scale. More...
 

Functions

static void * navdata_read (void *data)
 Main reading thread This is done asynchronous because the navdata board doesn't support NON_BLOCKING. More...
 
static void navdata_cmd_send (uint8_t cmd)
 Sends a one byte command. More...
 
static bool navdata_baro_calib (void)
 Try to receive the baro calibration from the navdata board. More...
 
static void mag_freeze_check (void)
 Check if the magneto is frozen Unknown why this bug happens. More...
 
static void baro_update_logic (void)
 Handle the baro(pressure/temperature) logic Sometimes the temperature and pressure are switched because of a bug in the navdata board firmware. More...
 
ssize_t full_write (int fd, const uint8_t *buf, size_t count)
 Write to fd even while being interrupted. More...
 
ssize_t full_read (int fd, uint8_t *buf, size_t count)
 Read from fd even while being interrupted. More...
 
static void send_navdata (struct transport_tx *trans, struct link_device *dev)
 
bool navdata_init ()
 Initialize the navdata board. More...
 
static void navdata_publish_imu (void)
 
void navdata_update ()
 Update the navdata (event loop) More...
 

Variables

struct navdata_t navdata
 
static uint8_t navdata_buffer [NAVDATA_PACKET_SIZE]
 Buffer filled in the thread (maximum one navdata packet) More...
 
static bool navdata_available = false
 flag to indicate new packet is available in buffer More...
 
static pthread_mutex_t navdata_mutex = PTHREAD_MUTEX_INITIALIZER
 
static pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER
 

Detailed Description

ARDrone2 navdata aquisition driver.

The ADRrone2 provides a navdata stream of packets containing info about all sensors at a rate of 200Hz.

Definition in file navdata.c.

Macro Definition Documentation

#define NAVDATA_FILTER_ID   2

Definition at line 71 of file navdata.c.

#define SONAR_OFFSET   880

Sonar offset.

Offset value in ADC equals to the ADC value so that height is zero

Definition at line 79 of file navdata.c.

Referenced by navdata_update().

#define SONAR_SCALE   0.00047

Sonar scale.

Sensor sensitivity in m/adc (float)

Definition at line 86 of file navdata.c.

Referenced by navdata_update().

Function Documentation

static void baro_update_logic ( void  )
static

Handle the baro(pressure/temperature) logic Sometimes the temperature and pressure are switched because of a bug in the navdata board firmware.

Definition at line 496 of file navdata.c.

References navdata_t::baro_available, navdata_t::measure, navdata, navdata_measure_t::pressure, and navdata_measure_t::temperature_pressure.

Referenced by navdata_update().

+ Here is the caller graph for this function:

ssize_t full_read ( int  fd,
uint8_t buf,
size_t  count 
)

Read from fd even while being interrupted.

Definition at line 112 of file navdata.c.

Referenced by actuators_ardrone_cmd(), and navdata_baro_calib().

+ Here is the caller graph for this function:

ssize_t full_write ( int  fd,
const uint8_t buf,
size_t  count 
)

Write to fd even while being interrupted.

Definition at line 92 of file navdata.c.

Referenced by actuators_ardrone_cmd(), actuators_ardrone_set_leds(), actuators_ardrone_set_pwm(), and navdata_cmd_send().

+ Here is the caller graph for this function:

static void mag_freeze_check ( void  )
static

Check if the magneto is frozen Unknown why this bug happens.

Definition at line 450 of file navdata.c.

References ARDRONE_GPIO_PIN_NAVDATA, ARDRONE_GPIO_PORT, gpio_clear(), gpio_set(), navdata_t::imu_lost, navdata_t::lost_imu_frames, navdata_t::measure, navdata_measure_t::mx, navdata, navdata_cmd_send(), NAVDATA_CMD_START, and NAVDATA_CMD_STOP.

Referenced by navdata_update().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static bool navdata_baro_calib ( void  )
static

Try to receive the baro calibration from the navdata board.

Definition at line 418 of file navdata.c.

References bmp180_calib_t::ac1, bmp180_calib_t::ac2, bmp180_calib_t::ac3, bmp180_calib_t::ac4, bmp180_calib_t::ac5, bmp180_calib_t::ac6, bmp180_calib_t::b1, bmp180_calib_t::b2, navdata_t::bmp180_calib, navdata_t::fd, full_read(), bmp180_calib_t::mb, bmp180_calib_t::mc, bmp180_calib_t::md, navdata, NAVDATA_CMD_BARO_CALIB, and navdata_cmd_send().

Referenced by navdata_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void navdata_cmd_send ( uint8_t  cmd)
static

Sends a one byte command.

Definition at line 409 of file navdata.c.

References navdata_t::fd, full_write(), and navdata.

Referenced by mag_freeze_check(), navdata_baro_calib(), and navdata_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void * navdata_read ( void *  data)
static

Main reading thread This is done asynchronous because the navdata board doesn't support NON_BLOCKING.

Definition at line 255 of file navdata.c.

References checksum, navdata_t::checksum_errors, navdata_measure_t::chksum, navdata_t::fd, navdata, navdata_buffer, NAVDATA_PACKET_SIZE, NAVDATA_START_BYTE, navdata_t::totalBytesRead, and TRUE.

Referenced by navdata_init().

+ Here is the caller graph for this function:

Variable Documentation

bool navdata_available = false
static

flag to indicate new packet is available in buffer

Definition at line 64 of file navdata.c.

uint8_t navdata_buffer[NAVDATA_PACKET_SIZE]
static

Buffer filled in the thread (maximum one navdata packet)

Definition at line 62 of file navdata.c.

Referenced by navdata_read().

pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER
static

Definition at line 68 of file navdata.c.

pthread_mutex_t navdata_mutex = PTHREAD_MUTEX_INITIALIZER
static

Definition at line 67 of file navdata.c.