Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/ins/ins.h"
#include "modules/ahrs/ahrs.h"
#include "modules/core/abi.h"
#include "mcu_periph/gpio.h"
#include "modules/datalink/telemetry.h"
#include "modules/imu/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
 
static const struct Int32Rates gyro_scale [2]
 Default gyro scale. More...
 
static const struct Int32Vect3 accel_scale [2]
 Default accel scale/neutral. More...
 
static const struct Int32Vect3 accel_neutral
 
static const struct Int32Vect3 mag_scale [2]
 Default mag scale. More...
 

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

◆ NAVDATA_FILTER_ID

#define NAVDATA_FILTER_ID   2

Definition at line 71 of file navdata.c.

◆ SONAR_OFFSET

#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.

◆ SONAR_SCALE

#define SONAR_SCALE   0.00047

Sonar scale.

Sensor sensitivity in m/adc (float)

Definition at line 86 of file navdata.c.

Function Documentation

◆ baro_update_logic()

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 543 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:

◆ full_read()

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

Read from fd even while being interrupted.

Definition at line 139 of file navdata.c.

References fd.

Referenced by actuators_ardrone_cmd(), and navdata_baro_calib().

+ Here is the caller graph for this function:

◆ full_write()

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

Write to fd even while being interrupted.

Definition at line 119 of file navdata.c.

References fd.

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:

◆ mag_freeze_check()

static void mag_freeze_check ( void  )
static

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

Definition at line 497 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:

◆ navdata_baro_calib()

static bool navdata_baro_calib ( void  )
static

Try to receive the baro calibration from the navdata board.

Definition at line 465 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:

◆ navdata_cmd_send()

static void navdata_cmd_send ( uint8_t  cmd)
static

Sends a one byte command.

Definition at line 456 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:

◆ navdata_init()

◆ navdata_publish_imu()

static void navdata_publish_imu ( void  )
static

◆ navdata_read()

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 291 of file navdata.c.

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

Referenced by navdata_init().

+ Here is the caller graph for this function:

◆ navdata_update()

◆ send_navdata()

Variable Documentation

◆ accel_neutral

const struct Int32Vect3 accel_neutral
static
Initial value:
= {
2048, 2048, 2048
}

Definition at line 68 of file navdata.c.

Referenced by imu_nps_init(), navdata_init(), and sensors_hitl_init().

◆ accel_scale

const struct Int32Vect3 accel_scale[2]
static
Initial value:
= {
{195, 195, 195},
{10, 10, 10}
}

Default accel scale/neutral.

Definition at line 68 of file navdata.c.

Referenced by imu_aspirin_i2c_init(), imu_aspirin_init(), imu_nps_init(), imu_px4_init(), navdata_init(), and sensors_hitl_init().

◆ gyro_scale

const struct Int32Rates gyro_scale[2]
static
Initial value:
= {
{4359, 4359, 4359},
{1000, 1000, 1000}
}

Default gyro scale.

Definition at line 68 of file navdata.c.

Referenced by imu_aspirin_i2c_init(), imu_aspirin_init(), imu_nps_init(), imu_px4_init(), navdata_init(), and sensors_hitl_init().

◆ mag_scale

const struct Int32Vect3 mag_scale[2]
static
Initial value:
= {
{16, 16, 16},
{1, 1, 1}
}

Default mag scale.

Definition at line 68 of file navdata.c.

Referenced by imu_nps_init(), navdata_init(), and sensors_hitl_init().

◆ navdata

◆ navdata_available

bool navdata_available = false
static

flag to indicate new packet is available in buffer

Definition at line 64 of file navdata.c.

Referenced by navdata_init(), navdata_read(), and navdata_update().

◆ navdata_buffer

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(), and navdata_update().

◆ navdata_cond

pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER
static

Definition at line 68 of file navdata.c.

Referenced by navdata_read(), and navdata_update().

◆ navdata_mutex

pthread_mutex_t navdata_mutex = PTHREAD_MUTEX_INITIALIZER
static

Definition at line 67 of file navdata.c.

Referenced by navdata_read(), and navdata_update().