Fixed wing path follower module

Files
file	fixedwingpathfollower.c
	This module compared PositionActual to PathDesired and sets StabilizationDesired. It only does this when the FlightMode field of ManualControlCommand is Auto.

Macros
#define	MAX_QUEUE_SIZE   4
#define	STACK_SIZE_BYTES   1548
#define	TASK_PRIORITY   PIOS_THREAD_PRIO_NORMAL

Functions
static void	pathfollowerTask (void *parameters)
static void	SettingsUpdatedCb (const UAVObjEvent *ev, void *ctx, void *obj, int len)
static void	updatePathVelocity ()
static uint8_t	updateFixedDesiredAttitude ()
static void	airspeedActualUpdatedCb (const UAVObjEvent *ev, void *ctx, void *obj, int len)
int32_t	FixedWingPathFollowerStart ()
int32_t	FixedWingPathFollowerInitialize ()

Variables
static bool	module_enabled = false
static struct pios_thread *	pathfollowerTaskHandle
static PathDesiredData	pathDesired
static PathStatusData	pathStatus
static FixedWingPathFollowerSettingsData	fixedwingpathfollowerSettings
static FixedWingAirspeedsData	fixedWingAirspeeds
static float	northVelIntegral = 0
static float	eastVelIntegral = 0
static float	downVelIntegral = 0
static float	bearingIntegral = 0
static float	speedIntegral = 0
static float	accelIntegral = 0
static float	powerIntegral = 0
static float	airspeedErrorInt =0
static float	indicatedAirspeedActualBias = 0
static bool	path_desired_updated

Detailed Description

Macro Definition Documentation

#define MAX_QUEUE_SIZE   4

Input object: ActiveWaypoint Input object: PositionActual Input object: ManualControlCommand Output object: StabilizationDesired

This module will periodically update the value of the StabilizationDesired object.

The module executes in its own thread in this example.

Modules have no API, all communication to other modules is done through UAVObjects. However modules may use the API exposed by shared libraries.

Definition at line 80 of file fixedwingpathfollower.c.

#define STACK_SIZE_BYTES   1548

Definition at line 81 of file fixedwingpathfollower.c.

#define TASK_PRIORITY   PIOS_THREAD_PRIO_NORMAL

Definition at line 82 of file fixedwingpathfollower.c.

Function Documentation

static void airspeedActualUpdatedCb ( const UAVObjEvent *  ev, void *  ctx, void *  obj, int  len  )

static

Definition at line 707 of file fixedwingpathfollower.c.

int32_t FixedWingPathFollowerInitialize

(

)

Initialise the module, called on startup

Returns

0 on success or -1 if initialisation failed

Definition at line 121 of file fixedwingpathfollower.c.

int32_t FixedWingPathFollowerStart

(

)

Initialise the module, called on startup

Returns

0 on success or -1 if initialisation failed

Definition at line 106 of file fixedwingpathfollower.c.

static void pathfollowerTask ( void *  parameters )

static

Module thread, should not return.

Definition at line 179 of file fixedwingpathfollower.c.

static void SettingsUpdatedCb ( const UAVObjEvent *  ev, void *  ctx, void *  obj, int  len  )

static

Definition at line 696 of file fixedwingpathfollower.c.

static uint8_t updateFixedDesiredAttitude ( )

static

Compute desired attitude from the desired velocity

Takes in NedActual which has the acceleration in the NED frame as the feedback term and then compares the VelocityActual against the VelocityDesired

Compute speed error (required for throttle and pitch)

Compute desired throttle command positive airspeed error means not enough airspeed positive decent_speed_error means decending too fast

Compute desired pitch command

Compute desired roll command

Compute desired yaw command

Definition at line 422 of file fixedwingpathfollower.c.

static void updatePathVelocity ( )

static

Compute desired velocity from the current position and path

Takes in PositionActual and compares it to PathDesired and computes VelocityDesired

Definition at line 347 of file fixedwingpathfollower.c.

Variable Documentation

float accelIntegral = 0

static

Definition at line 168 of file fixedwingpathfollower.c.

float airspeedErrorInt =0

static

Definition at line 170 of file fixedwingpathfollower.c.

float bearingIntegral = 0

static

Definition at line 166 of file fixedwingpathfollower.c.

float downVelIntegral = 0

static

Definition at line 164 of file fixedwingpathfollower.c.

float eastVelIntegral = 0

static

Definition at line 163 of file fixedwingpathfollower.c.

FixedWingAirspeedsData fixedWingAirspeeds

static

Definition at line 91 of file fixedwingpathfollower.c.

FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings

static

Definition at line 90 of file fixedwingpathfollower.c.

float indicatedAirspeedActualBias = 0

static

Definition at line 173 of file fixedwingpathfollower.c.

bool module_enabled = false

static

Definition at line 86 of file fixedwingpathfollower.c.

float northVelIntegral = 0

static

Definition at line 162 of file fixedwingpathfollower.c.

bool path_desired_updated

static

Definition at line 174 of file fixedwingpathfollower.c.

PathDesiredData pathDesired

static

Definition at line 88 of file fixedwingpathfollower.c.

struct pios_thread* pathfollowerTaskHandle

static

Definition at line 87 of file fixedwingpathfollower.c.

PathStatusData pathStatus

static

Definition at line 89 of file fixedwingpathfollower.c.

float powerIntegral = 0

static

Definition at line 169 of file fixedwingpathfollower.c.

float speedIntegral = 0

static

Definition at line 167 of file fixedwingpathfollower.c.