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 494 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 448 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 416 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 407 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:

bool navdata_init

(

void

)

Initialize the navdata board.

Definition at line 171 of file navdata.c.

References ARDRONE_GPIO_PIN_NAVDATA, ARDRONE_GPIO_PORT, B460800, navdata_t::baro_available, navdata_t::baro_calibrated, navdata_t::checksum_errors, DefaultPeriodic, navdata_t::fd, gpio_set(), gpio_setup_output(), navdata_t::imu_lost, navdata_t::is_initialized, navdata_t::last_packet_number, navdata_t::lost_imu_frames, navdata, navdata_baro_calib(), navdata_cmd_send(), NAVDATA_CMD_START, NAVDATA_CMD_STOP, NAVDATA_PACKET_SIZE, navdata_read(), navdata_t::packetsRead, register_periodic_telemetry(), send_navdata(), and navdata_t::totalBytesRead.

Referenced by imu_ardrone2_init(), and navdata_update().

Here is the call graph for this function:

Here is the caller graph for this function:

static void navdata_publish_imu ( void  )

static

Definition at line 325 of file navdata.c.

References Imu::accel, Imu::accel_unscaled, navdata_measure_t::ax, navdata_measure_t::ay, navdata_measure_t::az, get_sys_time_usec(), Imu::gyro, Imu::gyro_unscaled, imu, IMU_BOARD_ID, imu_scale_accel(), imu_scale_gyro(), imu_scale_mag(), Imu::mag, Imu::mag_unscaled, navdata_t::measure, navdata_measure_t::mx, navdata_measure_t::my, navdata_measure_t::mz, navdata, RATES_ASSIGN, VECT3_ASSIGN, navdata_measure_t::vx, navdata_measure_t::vy, and navdata_measure_t::vz.

Referenced by navdata_update().

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

void navdata_update

(

void

)

Update the navdata (event loop)

Definition at line 342 of file navdata.c.

References AGL_SONAR_ARDRONE2_ID, baro_update_logic(), navdata_t::is_initialized, navdata_t::last_packet_number, navdata_t::lost_imu_frames, mag_freeze_check(), navdata_t::measure, navdata, navdata_init(), NAVDATA_PACKET_SIZE, navdata_publish_imu(), navdata_measure_t::nu_trame, p, navdata_t::packetsRead, navdata_measure_t::pressure, SONAR_OFFSET, SONAR_SCALE, navdata_measure_t::temperature_pressure, and navdata_measure_t::ultrasound.

Here is the call graph for this function:

static void send_navdata ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 133 of file navdata.c.

References navdata_measure_t::ax, navdata_measure_t::ay, navdata_measure_t::az, navdata_t::checksum_errors, navdata_measure_t::chksum, navdata_measure_t::flag_echo_ini, navdata_measure_t::gradient, navdata_t::measure, navdata_measure_t::mx, navdata_measure_t::my, navdata_measure_t::mz, navdata, navdata_measure_t::nb_echo, navdata_measure_t::nu_trame, navdata_measure_t::pressure, navdata_measure_t::sum_echo, navdata_measure_t::taille, navdata_measure_t::temperature_acc, navdata_measure_t::temperature_gyro, navdata_measure_t::temperature_pressure, navdata_measure_t::ultrasound, navdata_measure_t::us_association_echo, navdata_measure_t::us_curve_ref, navdata_measure_t::us_curve_time, navdata_measure_t::us_curve_value, navdata_measure_t::us_debut_echo, navdata_measure_t::us_distance_echo, navdata_measure_t::us_fin_echo, navdata_measure_t::vx, navdata_measure_t::vy, and navdata_measure_t::vz.

Referenced by navdata_init().

Here is the caller graph for this function:

Variable Documentation

struct navdata_t navdata

Definition at line 59 of file navdata.c.

Referenced by ardrone_baro_event(), baro_apply_calibration(), baro_apply_calibration_temp(), baro_update_logic(), mag_freeze_check(), navdata_baro_calib(), navdata_cmd_send(), navdata_init(), navdata_publish_imu(), navdata_read(), navdata_update(), and send_navdata().

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.