stabilization_rate.c File Reference

Rate stabilization for rotorcrafts. More...

#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
#include "state.h"
#include "modules/imu/imu.h"
#include "modules/radio_control/radio_control.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for stabilization_rate.c:

Go to the source code of this file.

Macros
#define	MAX_SUM_ERR   40000
#define	STABILIZATION_RATE_IGAIN_P   0
#define	STABILIZATION_RATE_IGAIN_Q   0
#define	STABILIZATION_RATE_IGAIN_R   0
#define	OFFSET_AND_ROUND(_a, _b)   (((_a)+(1<<((_b)-1)))>>(_b))
#define	OFFSET_AND_ROUND2(_a, _b)   (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))

Functions
static void	send_rate (struct transport_tx *trans, struct link_device *dev)
void	stabilization_rate_init (void)
void	stabilization_rate_enter (void)
void	stabilization_rate_run (bool in_flight, struct StabilizationSetpoint *rate_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
struct StabilizationSetpoint	stabilization_rate_read_rc (struct RadioControl *rc)

Variables
static struct FloatRates	stabilization_rate_sp
static struct FloatRates	stabilization_rate_sum_err
static struct FloatRates	stabilization_rate_fb_cmd
struct FloatRates	stabilization_rate_gain
struct FloatRates	stabilization_rate_igain

Detailed Description

Rate stabilization for rotorcrafts.

Control loops for angular velocity.

Definition in file stabilization_rate.c.

Macro Definition Documentation

MAX_SUM_ERR

#define MAX_SUM_ERR   40000

Definition at line 40 of file stabilization_rate.c.

OFFSET_AND_ROUND

#define OFFSET_AND_ROUND	(		_a,
			_b
	)		(((_a)+(1<<((_b)-1)))>>(_b))

Definition at line 64 of file stabilization_rate.c.

OFFSET_AND_ROUND2

#define OFFSET_AND_ROUND2	(		_a,
			_b
	)		(((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))

Definition at line 65 of file stabilization_rate.c.

STABILIZATION_RATE_IGAIN_P

#define STABILIZATION_RATE_IGAIN_P   0

Definition at line 43 of file stabilization_rate.c.

STABILIZATION_RATE_IGAIN_Q

#define STABILIZATION_RATE_IGAIN_Q   0

Definition at line 47 of file stabilization_rate.c.

STABILIZATION_RATE_IGAIN_R

#define STABILIZATION_RATE_IGAIN_R   0

Definition at line 51 of file stabilization_rate.c.

Function Documentation

send_rate()

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

static

Definition at line 76 of file stabilization_rate.c.

References Stabilization::cmd, dev, foo, FloatRates::p, FloatRates::q, FloatRates::r, stabilization, stabilization_rate_fb_cmd, stabilization_rate_sp, and stabilization_rate_sum_err.

Referenced by stabilization_rate_init().

Here is the caller graph for this function:

stabilization_rate_enter()

void stabilization_rate_enter

(

void

)

Definition at line 113 of file stabilization_rate.c.

References FLOAT_RATES_ZERO, and stabilization_rate_sum_err.

Referenced by stabilization_mode_changed().

Here is the caller graph for this function:

stabilization_rate_init()

void stabilization_rate_init

(

void

)

Definition at line 92 of file stabilization_rate.c.

References DefaultPeriodic, FLOAT_RATES_ZERO, foo, RATES_ASSIGN, register_periodic_telemetry(), send_rate(), stabilization_rate_gain, stabilization_rate_igain, STABILIZATION_RATE_IGAIN_P, STABILIZATION_RATE_IGAIN_Q, STABILIZATION_RATE_IGAIN_R, stabilization_rate_sp, and stabilization_rate_sum_err.

Here is the call graph for this function:

stabilization_rate_read_rc()

struct StabilizationSetpoint stabilization_rate_read_rc

(

struct RadioControl *

rc

)

Definition at line 158 of file stabilization_rate.c.

References FLOAT_RATES_ZERO, foo, MAX_PPRZ, PITCH_RATE_DEADBAND_EXCEEDED, RC_RATE_P, RC_RATE_Q, RC_RATE_R, ROLL_RATE_DEADBAND_EXCEEDED, stab_sp_from_rates_f(), THROTTLE_STICK_DOWN_FROM_RC, and YAW_RATE_DEADBAND_EXCEEDED.

Referenced by rc_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

stabilization_rate_run()

void stabilization_rate_run	(	bool	in_flight,
		struct StabilizationSetpoint *	rate_sp,
		struct ThrustSetpoint *	thrust,
		int32_t *	cmd
	)

Definition at line 118 of file stabilization_rate.c.

References FLOAT_RATES_ZERO, foo, MAX_PPRZ, MAX_SUM_ERR, FloatRates::p, FloatRates::q, FloatRates::r, RATES_ADD, RATES_BOUND_CUBE, RATES_DIFF, RATES_SDIV, stab_sp_to_rates_f(), stabilization_rate_fb_cmd, stabilization_rate_gain, stabilization_rate_igain, stabilization_rate_sp, stabilization_rate_sum_err, stateGetBodyRates_f(), th_sp_to_thrust_i(), and THRUST_AXIS_Z.

Referenced by stabilization_run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

stabilization_rate_fb_cmd

struct FloatRates stabilization_rate_fb_cmd

static

Definition at line 69 of file stabilization_rate.c.

Referenced by send_rate(), and stabilization_rate_run().

stabilization_rate_gain

struct FloatRates stabilization_rate_gain

Definition at line 70 of file stabilization_rate.c.

Referenced by stabilization_rate_init(), and stabilization_rate_run().

stabilization_rate_igain

struct FloatRates stabilization_rate_igain

Definition at line 71 of file stabilization_rate.c.

Referenced by stabilization_rate_init(), and stabilization_rate_run().

stabilization_rate_sp

struct FloatRates stabilization_rate_sp

static

Definition at line 67 of file stabilization_rate.c.

Referenced by send_rate(), stabilization_rate_init(), and stabilization_rate_run().

stabilization_rate_sum_err

struct FloatRates stabilization_rate_sum_err

static

Definition at line 68 of file stabilization_rate.c.

Referenced by send_rate(), stabilization_rate_enter(), stabilization_rate_init(), and stabilization_rate_run().