#include "modules/dragspeed/dragspeed.h"
#include "modules/core/abi.h"
#include "modules/core/abi_common.h"
#include "modules/datalink/telemetry.h"
#include <stdio.h>

Include dependency graph for dragspeed.c:

Macros
#define	DRAGSPEED_SEND_ABI_MESSAGE TRUE

#define	DRAGSPEED_ACCEL_ID ABI_BROADCAST

#define	DRAGSPEED_COEFF_X 1.0

#define	DRAGSPEED_COEFF_Y 1.0

#define	DRAGSPEED_R 0.25

#define	DRAGSPEED_FILTER 0.8

Functions
static void	accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)

static void	send_dragspeed (struct transport_tx trans, struct link_device dev)

static void	calibrate_coeff (struct Int32Vect3 *accel)
	Calibrate drag coefficient by comparing accelerometer measurements to INS velocities. More...

static void	calibrate_zero (struct Int32Vect3 *accel)
	Calibrate zero velocity by measuring the accelerations while the drone hovers in-place. More...

void	dragspeed_init (void)

bool	dragspeed_calibrate_coeff (void)

bool	dragspeed_calibrate_zero (void)

bool	dragspeed_is_calibrating (void)

Variables
struct dragspeed_t	dragspeed

static abi_event	accel_ev

Detailed Description

Author: Tom van Dijk This module estimates the velocity of rotorcraft by measuring the drag force using the accelerometer.

During flight, gravity, thrust and drag act on the quadrotor. Only the drag is measured along the quadrotor's horizontal axes. Using a simple linear drag model, this deceleration can be transformed back into an estimate of the drone's airspeed.

This module makes the following assumptions:

The drag acting on the quadrotor grows linearly with velocity.
The size and weight of the drone are constant (changes in size and weight require re-calibration).
Pitch and roll angles are small (< ~20 deg).
No wind (when used as ground speed).

Definition in file dragspeed.c.

Macro Definition Documentation

◆ DRAGSPEED_ACCEL_ID

#define DRAGSPEED_ACCEL_ID ABI_BROADCAST

Definition at line 52 of file dragspeed.c.

◆ DRAGSPEED_COEFF_X

#define DRAGSPEED_COEFF_X 1.0

Definition at line 56 of file dragspeed.c.

◆ DRAGSPEED_COEFF_Y

#define DRAGSPEED_COEFF_Y 1.0

Definition at line 60 of file dragspeed.c.

◆ DRAGSPEED_FILTER

#define DRAGSPEED_FILTER 0.8

Definition at line 68 of file dragspeed.c.

◆ DRAGSPEED_R

#define DRAGSPEED_R 0.25

Definition at line 64 of file dragspeed.c.

◆ DRAGSPEED_SEND_ABI_MESSAGE

#define DRAGSPEED_SEND_ABI_MESSAGE TRUE

Definition at line 48 of file dragspeed.c.

Function Documentation

◆ accel_cb()

static void accel_cb	(	uint8_t	sender_id,
		uint32_t	stamp,
		struct Int32Vect3 *	accel
	)

static

Definition at line 122 of file dragspeed.c.

References ACCEL_FLOAT_OF_BFP, calibrate_coeff(), dragspeed_t::calibrate_coeff, calibrate_zero(), dragspeed_t::calibrate_zero, dragspeed_t::coeff, dragspeed, DRAGSPEED_R, dragspeed_t::filter, dragspeed_t::vel, VEL_DRAGSPEED_ID, FloatVect2::x, Int32Vect3::x, FloatVect2::y, Int32Vect3::y, and dragspeed_t::zero.

Referenced by dragspeed_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calibrate_coeff()

static void calibrate_coeff ( struct Int32Vect3 * accel )

static

Calibrate drag coefficient by comparing accelerometer measurements to INS velocities.

Should be performed with VEL_DRAGSPEED_ID set to ABI_DISABLE, but for safety the ABI messages are also disabled automatically when calibration is active.

This routine requires the accelerometers to have been zeroed beforehand, otherwise it will return without changing the drag coefficient!

Definition at line 154 of file dragspeed.c.

References ACCEL_FLOAT_OF_BFP, dragspeed_t::calibrate_coeff, dragspeed_t::coeff, dragspeed, FALSE, FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetSpeedEnu_f(), FloatVect2::x, Int32Vect3::x, EnuCoor_f::x, FloatVect2::y, Int32Vect3::y, EnuCoor_f::y, dragspeed_t::zero, and dragspeed_t::zero_calibrated.