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()

bool navdata_init

(

void

)

Initialize the navdata board.

Definition at line 202 of file navdata.c.

References accel_neutral, accel_scale, 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(), gyro_scale, IMU_BOARD_ID, navdata_t::imu_lost, imu_set_defaults_accel(), imu_set_defaults_gyro(), imu_set_defaults_mag(), navdata_t::is_initialized, navdata_t::last_packet_number, navdata_t::lost_imu_frames, mag_scale, navdata, navdata_available, 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:

navdata_publish_imu()

static void navdata_publish_imu ( void  )

static

Definition at line 362 of file navdata.c.

References navdata_measure_t::ax, navdata_measure_t::ay, navdata_measure_t::az, get_sys_time_usec(), IMU_BOARD_ID, navdata_t::measure, navdata_measure_t::mx, navdata_measure_t::my, navdata_measure_t::mz, navdata, navdata_measure_t::temperature_acc, navdata_measure_t::temperature_gyro, 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:

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()

void navdata_update

(

void

)

Update the navdata (event loop)

Definition at line 390 of file navdata.c.

References AGL_SONAR_ARDRONE2_ID, baro_update_logic(), get_sys_time_usec(), navdata_t::is_initialized, navdata_t::last_packet_number, navdata_t::lost_imu_frames, mag_freeze_check(), navdata_t::measure, navdata, navdata_available, navdata_buffer, navdata_cond, navdata_init(), navdata_mutex, 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:

send_navdata()

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

static

Definition at line 160 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, dev, 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

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

struct navdata_t navdata

Definition at line 56 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().

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().