stabilization.c File Reference

Control the UAV attitude to StabilizationDesired. More...

#include "openpilot.h"
#include "stabilization.h"
#include "pios_thread.h"
#include "pios_queue.h"
#include "accels.h"
#include "actuatordesired.h"
#include "attitudeactual.h"
#include "cameradesired.h"
#include "flightstatus.h"
#include "gyros.h"
#include "ratedesired.h"
#include "systemident.h"
#include "stabilizationdesired.h"
#include "stabilizationsettings.h"
#include "subtrim.h"
#include "subtrimsettings.h"
#include "systemsettings.h"
#include "manualcontrolcommand.h"
#include "vbarsettings.h"
#include "lqgsettings.h"
#include "rtkfestimate.h"
#include "lqgsolution.h"
#include "systemalarms.h"
#include "altitudeholdsettings.h"
#include "altitudeholdstate.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "coordinate_conversions.h"
#include "physical_constants.h"
#include "pid.h"
#include "misc_math.h"
#include "smoothcontrol.h"
#include "lqg.h"
#include "sensors.h"
#include "virtualflybar.h"

Go to the source code of this file.

Macros
#define	MAX_QUEUE_SIZE   1
#define	STACK_SIZE_BYTES   1200
#define	TASK_PRIORITY   PIOS_THREAD_PRIO_HIGHEST
#define	FAILSAFE_TIMEOUT_MS   30
#define	COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD   3.0f
#define	COORDINATED_FLIGHT_MAX_YAW_THRESHOLD   0.05f
#define	HORIZON_MODE_MAX_BLEND   0.85f
	Set the stick position that maximally transitions to rate. More...
#define	THROTTLE_EPSILON   0.000001f
	Minimum sane positive value for throttle. More...
#define	get_deadband(axis)   (deadbands ? (deadbands + axis) : NULL)

Enumerations
enum	{   PID_GROUP_RATE, PID_RATE_ROLL = PID_GROUP_RATE, PID_RATE_PITCH, PID_RATE_YAW,   PID_GROUP_ATT, PID_ATT_ROLL = PID_GROUP_ATT, PID_ATT_PITCH, PID_ATT_YAW,   PID_GROUP_VBAR, PID_VBAR_ROLL = PID_GROUP_VBAR, PID_VBAR_PITCH, PID_VBAR_YAW,   PID_COORDINATED_FLIGHT_YAW, PID_MAX }

Functions
	DONT_BUILD_IF ((MAX_AXES+0!=3), stabAxisWrongCount)
	DONT_BUILD_IF ((PID_RATE_ROLL+0!=ROLL)||(PID_ATT_ROLL!=PID_GROUP_ATT+ROLL), stabAxisPidEnumMapping1)
	DONT_BUILD_IF ((PID_RATE_PITCH+0!=PITCH)||(PID_ATT_PITCH!=PID_GROUP_ATT+PITCH), stabAxisPidEnumMapping2)
	DONT_BUILD_IF ((PID_RATE_YAW+0!=YAW)||(PID_ATT_YAW!=PID_GROUP_ATT+YAW), stabAxisPidEnumMapping3)
	DONT_BUILD_IF (STABILIZATIONSETTINGS_MAXLEVELANGLE_ROLL!=ROLL+0, LevelAngleConstantRoll)
	DONT_BUILD_IF (STABILIZATIONSETTINGS_MAXLEVELANGLE_PITCH!=PITCH+0, LevelAngleConstantPitch)
	DONT_BUILD_IF (STABILIZATIONSETTINGS_MAXLEVELANGLE_YAW!=YAW+0, LevelAngleConstantYaw)
static void	stabilizationTask (void *parameters)
static void	zero_pids (void)
static void	calculate_pids (float dT)
static void	update_settings (float dT)
static float	get_throttle (ActuatorDesiredData *actuator_desired, SystemSettingsAirframeTypeOptions *airframe_type)
int32_t	StabilizationStart ()
int32_t	StabilizationInitialize ()
bool	stabilization_failsafe_checks (StabilizationDesiredData *stab_desired, ActuatorDesiredData *actuator_desired, SystemSettingsAirframeTypeOptions airframe_type, float *input, uint8_t *mode)
static void	calculate_attitude_errors (uint8_t *axis_mode, float *raw_input, AttitudeActualData *attitudeActual, float *local_attitude_error, float *horizon_rate_fraction)
	MODULE_HIPRI_INITCALL (StabilizationInitialize, StabilizationStart)
static float	calculate_thrust (StabilizationDesiredThrustModeOptions mode, FlightStatusData *flightStatus, SystemSettingsAirframeTypeOptions airframe_type, float desired_thrust)
static uint8_t	remap_smoothing_mode (uint8_t m)
static void	calculate_vert_pids (float dT)

Variables
static struct pios_thread *	taskHandle
static StabilizationSettingsData	settings
static VbarSettingsData	vbar_settings
static SubTrimData	subTrim
uint16_t	ident_wiggle_points
static float	axis_lock_accum [MAX_AXES] = {0,0,0}
static uint8_t	max_axis_lock = 0
static uint8_t	max_axislock_rate = 0
static float	weak_leveling_kp = 0
static uint8_t	weak_leveling_max = 0
static bool	lowThrottleZeroIntegral
static float	max_rate_alpha = 0.8f
float	vbar_decay = 0.991f
static struct pid	pids [PID_MAX]
static smoothcontrol_state	rc_smoothing
struct pid_deadband *	deadbands = NULL

Detailed Description

Control the UAV attitude to StabilizationDesired.

Author

dRonin, http://dRonin.org/, Copyright (C) 2015-2016

Tau Labs, http://taulabs.org, Copyright (C) 2012-2016

The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. The main control code which keeps the UAV at the attitude requested by StabilizationDesired. This is done by comparing against AttitudeActual to compute the error in roll pitch and yaw then then based on the mode and values in StabilizationSettings computing the desired outputs and placing them in ActuatorDesired.

See Also

The GNU Public License (GPL) Version 3

Definition in file stabilization.c.