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 "subsystems/imu.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"

Include dependency graph for stabilization_rate.c:

Go to the source code of this file.

Macros
#define	MAX_SUM_ERR   4000000
#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))
#define	STABILIZATION_RATE_DEADBAND_P   0
#define	STABILIZATION_RATE_DEADBAND_Q   0
#define	STABILIZATION_RATE_DEADBAND_R   200
#define	ROLL_RATE_DEADBAND_EXCEEDED()
#define	PITCH_RATE_DEADBAND_EXCEEDED()
#define	YAW_RATE_DEADBAND_EXCEEDED()

Functions
void	stabilization_rate_init (void)
void	stabilization_rate_read_rc (void)
void	stabilization_rate_read_rc_switched_sticks (void)
void	stabilization_rate_enter (void)
void	stabilization_rate_run (bool_t in_flight)

Variables
struct Int32Rates	stabilization_rate_sp
struct Int32Rates	stabilization_rate_gain
struct Int32Rates	stabilization_rate_igain
struct Int32Rates	stabilization_rate_sum_err
struct Int32Rates	stabilization_rate_fb_cmd

Detailed Description

Rate stabilization for rotorcrafts.

Control loops for angular velocity.

Definition in file stabilization_rate.c.

Macro Definition Documentation

#define MAX_SUM_ERR   4000000

Definition at line 40 of file stabilization_rate.c.

Referenced by stabilization_rate_run().

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

Definition at line 63 of file stabilization_rate.c.

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

Definition at line 64 of file stabilization_rate.c.

Referenced by stabilization_rate_run().

#define PITCH_RATE_DEADBAND_EXCEEDED

(

)

Value:

(radio_control.values[RADIO_PITCH] >  STABILIZATION_RATE_DEADBAND_Q || \
   radio_control.values[RADIO_PITCH] < -STABILIZATION_RATE_DEADBAND_Q)

Definition at line 87 of file stabilization_rate.c.

Referenced by stabilization_rate_read_rc(), and stabilization_rate_read_rc_switched_sticks().

#define ROLL_RATE_DEADBAND_EXCEEDED

(

)

Value:

(radio_control.values[RADIO_ROLL] >  STABILIZATION_RATE_DEADBAND_P || \
   radio_control.values[RADIO_ROLL] < -STABILIZATION_RATE_DEADBAND_P)

Definition at line 83 of file stabilization_rate.c.

Referenced by stabilization_rate_read_rc(), and stabilization_rate_read_rc_switched_sticks().

#define STABILIZATION_RATE_DEADBAND_P   0

Definition at line 74 of file stabilization_rate.c.

#define STABILIZATION_RATE_DEADBAND_Q   0

Definition at line 77 of file stabilization_rate.c.

#define STABILIZATION_RATE_DEADBAND_R   200

Definition at line 80 of file stabilization_rate.c.

#define STABILIZATION_RATE_IGAIN_P   0

Definition at line 43 of file stabilization_rate.c.

Referenced by stabilization_rate_init().

#define STABILIZATION_RATE_IGAIN_Q   0

Definition at line 47 of file stabilization_rate.c.

Referenced by stabilization_rate_init().

#define STABILIZATION_RATE_IGAIN_R   0

Definition at line 51 of file stabilization_rate.c.

Referenced by stabilization_rate_init().

#define YAW_RATE_DEADBAND_EXCEEDED

(

)

Value:

(radio_control.values[RADIO_YAW] >  STABILIZATION_RATE_DEADBAND_R || \
   radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)

Definition at line 91 of file stabilization_rate.c.

Referenced by stabilization_rate_read_rc(), and stabilization_rate_read_rc_switched_sticks().

Function Documentation

void stabilization_rate_enter

(

void

)

Definition at line 181 of file stabilization_rate.c.

References INT_RATES_ZERO, and stabilization_rate_sum_err.

Referenced by guidance_h_mode_changed().

Here is the caller graph for this function:

void stabilization_rate_init

(

void

)

Definition at line 114 of file stabilization_rate.c.

References DefaultPeriodic, INT_RATES_ZERO, RATES_ASSIGN, register_periodic_telemetry(), 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.

Referenced by autopilot_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_rate_read_rc

(

void

)

Definition at line 136 of file stabilization_rate.c.

References MAX_PPRZ, Int32Rates::p, PITCH_RATE_DEADBAND_EXCEEDED, Int32Rates::q, Int32Rates::r, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_YAW, RATE_BFP_OF_REAL, ROLL_RATE_DEADBAND_EXCEEDED, stabilization_rate_sp, THROTTLE_STICK_DOWN, RadioControl::values, and YAW_RATE_DEADBAND_EXCEEDED.

Referenced by guidance_h_read_rc().

Here is the caller graph for this function:

void stabilization_rate_read_rc_switched_sticks

(

void

)

Definition at line 159 of file stabilization_rate.c.

References MAX_PPRZ, Int32Rates::p, PITCH_RATE_DEADBAND_EXCEEDED, Int32Rates::q, Int32Rates::r, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_YAW, RATE_BFP_OF_REAL, ROLL_RATE_DEADBAND_EXCEEDED, stabilization_rate_sp, THROTTLE_STICK_DOWN, RadioControl::values, and YAW_RATE_DEADBAND_EXCEEDED.

Referenced by guidance_h_read_rc().

Here is the caller graph for this function:

void stabilization_rate_run

(

bool_t

in_flight

)

Definition at line 186 of file stabilization_rate.c.

References INT_RATES_ZERO, MAX_PPRZ, MAX_SUM_ERR, OFFSET_AND_ROUND2, Int32Rates::p, Int32Rates::q, Int32Rates::r, RATES_ADD, RATES_BOUND_CUBE, RATES_DIFF, stabilization_cmd, stabilization_rate_fb_cmd, stabilization_rate_gain, stabilization_rate_igain, stabilization_rate_sp, stabilization_rate_sum_err, and stateGetBodyRates_i().

Referenced by guidance_h_run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

struct Int32Rates stabilization_rate_fb_cmd

Definition at line 71 of file stabilization_rate.c.

Referenced by stabilization_rate_run().

struct Int32Rates stabilization_rate_gain

Definition at line 67 of file stabilization_rate.c.

Referenced by stabilization_rate_init(), and stabilization_rate_run().

struct Int32Rates stabilization_rate_igain

Definition at line 68 of file stabilization_rate.c.

Referenced by stabilization_rate_init(), and stabilization_rate_run().

struct Int32Rates stabilization_rate_sp

Definition at line 66 of file stabilization_rate.c.

Referenced by stabilization_rate_init(), stabilization_rate_read_rc(), stabilization_rate_read_rc_switched_sticks(), and stabilization_rate_run().

struct Int32Rates stabilization_rate_sum_err

Definition at line 69 of file stabilization_rate.c.

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