Actuator Module

Files
file	actuator.c
	Actuator module. Drives the actuators (servos, motors etc).

Data Structures
struct	dshot_command

Macros
#define	MAX_QUEUE_SIZE   2
#define	STACK_SIZE_BYTES   1336
#define	TASK_PRIORITY   PIOS_THREAD_PRIO_HIGHEST
#define	FAILSAFE_TIMEOUT_MS   100
#define	MAX_MIX_ACTUATORS   ACTUATORCOMMAND_CHANNEL_NUMELEM
#define	MIXER_SCALE   128
#define	ACTUATOR_EPSILON   0.00001f
#define	DSHOT_COMMAND_DONTSPIN   0
#define	DSHOT_COMMAND_NO3DMODE   9
#define	DSHOT_COMMAND_3DMODE   10
#define	DSHOT_COUNT_EXCESSIVE   24
#define	compute_one_token_paste(b)   compute_one_mixer(b-1, &mixerSettings.Mixer ## b ## Vector, mixerSettings.Mixer ## b ## Type, scale_adjustment)

Functions
	DONT_BUILD_IF (ACTUATORSETTINGS_TIMERUPDATEFREQ_NUMELEM > PIOS_SERVO_MAX_BANKS, TooManyServoBanks)
	DONT_BUILD_IF (MAX_MIX_ACTUATORS > ACTUATORCOMMAND_CHANNEL_NUMELEM, TooManyMixers)
	DONT_BUILD_IF ((MIXERSETTINGS_MIXER1VECTOR_NUMELEM-MIXERSETTINGS_MIXER1VECTOR_ACCESSORY0)< MANUALCONTROLCOMMAND_ACCESSORY_NUMELEM, AccessoryMismatch)
static void	actuator_task (void *parameters)
	Main Actuator module task. More...
static float	scale_channel (float value, int idx, bool active_cmd)
static void	set_failsafe ()
static float	collective_curve (const float input, const float *curve, uint8_t num_points)
static bool	actuator_get_dshot_command (bool armed)
int	actuator_send_dshot_command (uint8_t cmd_id, uint8_t num_to_send, uint16_t channel_mask)
static int	actuator_send_dshot_command_now (uint8_t cmd_id, uint8_t num_to_send, uint16_t channel_mask)
int32_t	ActuatorStart ()
	Module initialization. More...
int32_t	ActuatorInitialize ()
	Module initialization. More...
	MODULE_HIPRI_INITCALL (ActuatorInitialize, ActuatorStart)
static float	get_curve2_source (ActuatorDesiredData *desired, SystemSettingsAirframeTypeOptions airframe_type, MixerSettingsCurve2SourceOptions source, float throttle_val)
static void	compute_one_mixer (int mixnum, int16_t(*vals)[MIXERSETTINGS_MIXER1VECTOR_NUMELEM], MixerSettingsMixer1TypeOptions type, float scale_adjustment)
static void	compute_mixer ()
static void	fill_desired_vector (ActuatorDesiredData *desired, float val1, float val2, float(*cmd_vector)[MIXERSETTINGS_MIXER1VECTOR_NUMELEM])
static void	post_process_scale_and_commit (float *motor_vect, float *desired_vect, float dT, bool armed, bool spin_while_armed, bool stabilize_now, bool flip_over_mode, float *maxpoweradd_bucket)
static void	normalize_input_data (uint32_t this_systime, float(*desired_vect)[MIXERSETTINGS_MIXER1VECTOR_NUMELEM], bool *armed, bool *spin_while_armed, bool *stabilize_now, bool *flip_over_mode)
static void	actuator_settings_update ()
static float	scale_channel_dshot (float value, int idx, bool active_command)
static float	channel_failsafe_value (int idx)

Variables
static struct pios_queue *	queue
static struct pios_thread *	taskHandle
static float	hangtime_leakybucket_timeconstant = 0.3f
static volatile bool	flight_status_updated = true
static volatile bool	manual_control_cmd_updated = true
static volatile bool	settings_updated = true
static MixerSettingsMixer1TypeOptions	types_mixer [MAX_MIX_ACTUATORS]
static float	motor_mixer [MAX_MIX_ACTUATORS *MIXERSETTINGS_MIXER1VECTOR_NUMELEM]
static ActuatorSettingsData	actuatorSettings
static SystemSettingsAirframeTypeOptions	airframe_type
static float	curve2 [MIXERSETTINGS_THROTTLECURVE2_NUMELEM]
static MixerSettingsCurve2SourceOptions	curve2_src
enum actuator_interlock	actuator_interlock = ACTUATOR_INTERLOCK_OK
struct dshot_command	__attribute__
static volatile struct dshot_command	pending_cmd
static struct dshot_command	cur_cmd
static uint16_t	desired_3d_mask

Detailed Description

Macro Definition Documentation

#define ACTUATOR_EPSILON   0.00001f

Definition at line 77 of file actuator.c.

#define compute_one_token_paste

(

b

)

compute_one_mixer(b-1, &mixerSettings.Mixer ## b ## Vector, mixerSettings.Mixer ## b ## Type, scale_adjustment)

Definition at line 365 of file actuator.c.

#define DSHOT_COMMAND_3DMODE   10

Definition at line 138 of file actuator.c.

#define DSHOT_COMMAND_DONTSPIN   0

Definition at line 136 of file actuator.c.

#define DSHOT_COMMAND_NO3DMODE   9

Definition at line 137 of file actuator.c.

#define DSHOT_COUNT_EXCESSIVE   24

Definition at line 140 of file actuator.c.

#define FAILSAFE_TIMEOUT_MS   100

Definition at line 66 of file actuator.c.

#define MAX_MIX_ACTUATORS   ACTUATORCOMMAND_CHANNEL_NUMELEM

Definition at line 69 of file actuator.c.

#define MAX_QUEUE_SIZE   2

Definition at line 57 of file actuator.c.

#define MIXER_SCALE   128

Definition at line 76 of file actuator.c.

#define STACK_SIZE_BYTES   1336

Definition at line 62 of file actuator.c.

#define TASK_PRIORITY   PIOS_THREAD_PRIO_HIGHEST

Definition at line 65 of file actuator.c.

Function Documentation

static bool actuator_get_dshot_command ( bool  armed )

static

Definition at line 142 of file actuator.c.

int actuator_send_dshot_command	(	uint8_t	cmd_id,
		uint8_t	num_to_send,
		uint16_t	channel_mask
	)

Definition at line 170 of file actuator.c.

static int actuator_send_dshot_command_now ( uint8_t  cmd_id, uint8_t  num_to_send, uint16_t  channel_mask  )

static

Definition at line 187 of file actuator.c.

static void actuator_settings_update ( )

static

Definition at line 741 of file actuator.c.

static void actuator_task ( void *  parameters )

static

Main Actuator module task.

Universal matrix based mixer for VTOL, helis and fixed wing. Converts desired roll,pitch,yaw and throttle to servo/ESC outputs.

Because of how the Throttle ranges from 0 to 1, the motors should too!

Note this code depends on the UAVObjects for the mixers being all being the same and in sequence. If you change the object definition, make sure you check the code!

Returns

-1 if error, 0 if success

Definition at line 774 of file actuator.c.

int32_t ActuatorInitialize

(

)

Module initialization.

Returns

0

Definition at line 221 of file actuator.c.

int32_t ActuatorStart

(

)

Module initialization.

Returns

0

Definition at line 205 of file actuator.c.

static float channel_failsafe_value ( int  idx )

static

Definition at line 1083 of file actuator.c.

static float collective_curve ( float const  input, float const *  curve, uint8_t  num_points  )

static

Interpolate a collective curve

we need to accept input in [-1,1] so that the neutral point may be set arbitrarily within the typical channel input range, which is [-1,1]

Parameters

input	The input value, in [-1,1]
curve	Array of points in the curve
num_points	Number of points in the curve

Returns

the output value, in [-1,1]

Definition at line 933 of file actuator.c.

static void compute_mixer ( )

static

Definition at line 367 of file actuator.c.

static void compute_one_mixer ( int  mixnum, int16_t(*)  vals[MIXERSETTINGS_MIXER1VECTOR_NUMELEM], MixerSettingsMixer1TypeOptions  type, float  scale_adjustment  )

static

Definition at line 320 of file actuator.c.

DONT_BUILD_IF	(	ACTUATORSETTINGS_TIMERUPDATEFREQ_NUMELEM	,
		PIOS_SERVO_MAX_BANKS	,
		TooManyServoBanks
	)

DONT_BUILD_IF	(	MAX_MIX_ACTUATORS	,
		ACTUATORCOMMAND_CHANNEL_NUMELEM	,
		TooManyMixers
	)

DONT_BUILD_IF

(

)

static void fill_desired_vector ( ActuatorDesiredData *  desired, float  val1, float  val2, float(*)  cmd_vector[MIXERSETTINGS_MIXER1VECTOR_NUMELEM]  )

static

Definition at line 413 of file actuator.c.

static float get_curve2_source ( ActuatorDesiredData *  desired, SystemSettingsAirframeTypeOptions  airframe_type, MixerSettingsCurve2SourceOptions  source, float  throttle_val  )

static

Definition at line 265 of file actuator.c.

MODULE_HIPRI_INITCALL	(	ActuatorInitialize	,
		ActuatorStart
	)

static void normalize_input_data ( uint32_t  this_systime, float(*)  desired_vect[MIXERSETTINGS_MIXER1VECTOR_NUMELEM], bool *  armed, bool *  spin_while_armed, bool *  stabilize_now, bool *  flip_over_mode  )

static

Definition at line 670 of file actuator.c.

static void post_process_scale_and_commit ( float *  motor_vect, float *  desired_vect, float  dT, bool  armed, bool  spin_while_armed, bool  stabilize_now, bool  flip_over_mode, float *  maxpoweradd_bucket  )

static

Definition at line 429 of file actuator.c.

static float scale_channel ( float  value, int  idx, bool  active_cmd  )

static

Convert channel from -1/+1 to servo pulse duration in microseconds

Definition at line 1034 of file actuator.c.

static float scale_channel_dshot ( float  value, int  idx, bool  active_command  )

static

Definition at line 938 of file actuator.c.

static void set_failsafe ( )

static

Set actuator output to the neutral values (failsafe)

Definition at line 1101 of file actuator.c.

Variable Documentation

struct dshot_command __attribute__

enum actuator_interlock actuator_interlock = ACTUATOR_INTERLOCK_OK

Definition at line 118 of file actuator.c.

ActuatorSettingsData actuatorSettings

static

Definition at line 102 of file actuator.c.

SystemSettingsAirframeTypeOptions airframe_type

static

Definition at line 103 of file actuator.c.

struct dshot_command cur_cmd

static

Definition at line 132 of file actuator.c.

float curve2[MIXERSETTINGS_THROTTLECURVE2_NUMELEM]

static

Definition at line 105 of file actuator.c.

MixerSettingsCurve2SourceOptions curve2_src

static

Definition at line 107 of file actuator.c.

uint16_t desired_3d_mask

static

Definition at line 134 of file actuator.c.

volatile bool flight_status_updated = true

static

Definition at line 89 of file actuator.c.

float hangtime_leakybucket_timeconstant = 0.3f

static

Definition at line 85 of file actuator.c.

volatile bool manual_control_cmd_updated = true

static

Definition at line 90 of file actuator.c.

float motor_mixer[MAX_MIX_ACTUATORS *MIXERSETTINGS_MIXER1VECTOR_NUMELEM]

static

Definition at line 99 of file actuator.c.

volatile struct dshot_command pending_cmd

static

Definition at line 131 of file actuator.c.

struct pios_queue* queue

static

Definition at line 82 of file actuator.c.

volatile bool settings_updated = true

static

Definition at line 91 of file actuator.c.

struct pios_thread* taskHandle

static

Definition at line 83 of file actuator.c.

MixerSettingsMixer1TypeOptions types_mixer[MAX_MIX_ACTUATORS]

static

Definition at line 93 of file actuator.c.