Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
dragspeed.c File Reference
#include "modules/dragspeed/dragspeed.h"
#include "subsystems/abi.h"
#include "subsystems/abi_common.h"
#include "subsystems/datalink/telemetry.h"
#include <stdio.h>
+ Include dependency graph for dragspeed.c:

Go to the source code of this file.

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

#define DRAGSPEED_ACCEL_ID   ABI_BROADCAST

Definition at line 52 of file dragspeed.c.

Referenced by dragspeed_init().

#define DRAGSPEED_COEFF_X   1.0

Definition at line 56 of file dragspeed.c.

Referenced by dragspeed_init().

#define DRAGSPEED_COEFF_Y   1.0

Definition at line 60 of file dragspeed.c.

Referenced by dragspeed_init().

#define DRAGSPEED_FILTER   0.8

Definition at line 68 of file dragspeed.c.

Referenced by dragspeed_init().

#define DRAGSPEED_R   0.25

Definition at line 64 of file dragspeed.c.

Referenced by accel_cb().

#define DRAGSPEED_SEND_ABI_MESSAGE   TRUE

Definition at line 48 of file dragspeed.c.

Function Documentation

static void accel_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct Int32Vect3 accel 
)
static
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, EnuCoor_f::x, Int32Vect3::x, FloatVect2::y, EnuCoor_f::y, Int32Vect3::y, dragspeed_t::zero, and dragspeed_t::zero_calibrated.

Referenced by accel_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void calibrate_zero ( struct Int32Vect3 accel)
static

Calibrate zero velocity by measuring the accelerations while the drone hovers in-place.

The zero-velocity readings can change between flights, e.g. because of small differences in battery or outer hull positions.

Definition at line 216 of file dragspeed.c.

References ACCEL_FLOAT_OF_BFP, dragspeed_t::calibrate_zero, dragspeed, FALSE, stateGetSpeedEnu_f(), TRUE, FloatVect2::x, EnuCoor_f::x, Int32Vect3::x, FloatVect2::y, EnuCoor_f::y, Int32Vect3::y, dragspeed_t::zero, and dragspeed_t::zero_calibrated.

Referenced by accel_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool dragspeed_calibrate_coeff ( void  )

Definition at line 105 of file dragspeed.c.

References dragspeed_t::calibrate_coeff, dragspeed, FALSE, and TRUE.

bool dragspeed_calibrate_zero ( void  )

Definition at line 111 of file dragspeed.c.

References dragspeed_t::calibrate_zero, dragspeed, FALSE, and TRUE.

bool dragspeed_is_calibrating ( void  )

Definition at line 117 of file dragspeed.c.

References dragspeed_t::calibrate_coeff, dragspeed_t::calibrate_zero, and dragspeed.

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

Definition at line 251 of file dragspeed.c.

References dragspeed, FloatEulers::psi, stateGetNedToBodyEulers_f(), stateGetSpeedEnu_f(), dragspeed_t::vel, FloatVect2::x, EnuCoor_f::x, FloatVect2::y, and EnuCoor_f::y.

Referenced by dragspeed_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

abi_event accel_ev
static

Definition at line 73 of file dragspeed.c.