obm_cyclone.c File Reference

#include "firmwares/rotorcraft/stabilization/stabilization_andi.h"
#include <math.h>

Include dependency graph for obm_cyclone.c:

Go to the source code of this file.

Data Structures
union	CycloneCoefficients
union	CeMatrix
struct	CycloneCoefficients.__unnamed286__
struct	CeMatrix.__unnamed288__

Functions
static void	cyclone_obm_forces (const float body_vel[3], const float u[4], const float coeff[22], float F_obm_forces[3])
static void	cyclone_obm_moments (const float rates[3], const float body_vel[3], const float u[4], const float coeff[22], float F_obm_moments[3])
static void	cyclone_f_stb_u (const float body_vel[3], const float u[4], const float coeff[22], float F_stb_u[16])
void	cyclone_f_stb_x (const float rates[3], const float ang_accel[3], const float vel_body[3], const float accel_body[3], const float u[4], const float coeff[22], float F_stb_x[4])
struct FloatVect3	evaluate_obm_forces (const struct FloatRates *rates, const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT])
	Evaluate total force acting on the vehicle from the OBM.
struct FloatVect3	evaluate_obm_moments (const struct FloatRates *rates, const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT])
	Evaluate total moments acting on the vehicle from the OBM.
void	evaluate_obm_f_stb_u (float fu_mat[ANDI_NUM_ACT *ANDI_OUTPUTS], const struct FloatRates *rates, const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT])
	Evaluate the state-dependent control effectiveness matrix F_u for stabilization.
void	evaluate_obm_f_stb_x (float nu_obm[ANDI_OUTPUTS], const struct FloatRates *rates, const struct FloatVect3 *vel_body, const struct FloatVect3 *ang_accel, const struct FloatVect3 *accel_body, const float actuator_state[ANDI_NUM_ACT])
	Evaluate the state-dependent contribution F_x * x_dot for stabilization.
float	evaluate_obm_thrust_z (const float actuator_state[ANDI_NUM_ACT])
	Compute total thrust produced by the current actuator state.

Variables
union CycloneCoefficients	obm_coefficients
union CeMatrix	ce_mat_tmp

---

Data Structure Documentation

CycloneCoefficients

union CycloneCoefficients

Definition at line 12 of file obm_cyclone.c.

Collaboration diagram for CycloneCoefficients:

Data Fields
struct CycloneCoefficients.__unnamed286__	__unnamed__
float	data[22]

CeMatrix

union CeMatrix

Definition at line 84 of file obm_cyclone.c.

Collaboration diagram for CeMatrix:

Data Fields
struct CeMatrix.__unnamed288__	__unnamed__
float	data[16]

CycloneCoefficients.__unnamed286__

struct CycloneCoefficients.__unnamed286__

Definition at line 13 of file obm_cyclone.c.

Data Fields
float	fx_motor_squared
float	fx_speed_forward
float	fy_speed_lateral
float	fz_elevon_motor
float	fz_elevon_speed
float	fz_motor_squared
float	fz_speed_forward
float	fz_speed_vertical
float	mx_angular_coupling
float	mx_angular_drag
float	mx_elevon_motor_diff
float	mx_elevon_speed_diff
float	my_angular_sum
float	my_elevon_motor_sum
float	my_elevon_speed_sum
float	my_motor_sum
float	my_speed_forward
float	my_speed_vertical
float	mz_angular_coupling
float	mz_motor_diff
float	mz_speed_lateral
float	mz_speed_roll

CeMatrix.__unnamed288__

struct CeMatrix.__unnamed288__

Definition at line 85 of file obm_cyclone.c.

Data Fields
float	ce_11
float	ce_12
float	ce_13
float	ce_14
float	ce_21
float	ce_22
float	ce_23
float	ce_24
float	ce_31
float	ce_32
float	ce_33
float	ce_34
float	ce_41
float	ce_42
float	ce_43
float	ce_44

Function Documentation

cyclone_f_stb_u()

static void cyclone_f_stb_u ( const float  body_vel[3], const float  u[4], const float  coeff[22], float  F_stb_u[16]  )

static

Definition at line 160 of file obm_cyclone.c.

References foo.

Referenced by evaluate_obm_f_stb_u().

Here is the caller graph for this function:

cyclone_f_stb_x()

void cyclone_f_stb_x	(	const float	rates[3],
		const float	ang_accel[3],
		const float	vel_body[3],
		const float	accel_body[3],
		const float	u[4],
		const float	coeff[22],
		float	F_stb_x[4]
	)

Definition at line 201 of file obm_cyclone.c.

References foo.

Referenced by evaluate_obm_f_stb_x().

Here is the caller graph for this function:

cyclone_obm_forces()

static void cyclone_obm_forces ( const float  body_vel[3], const float  u[4], const float  coeff[22], float  F_obm_forces[3]  )

static

Definition at line 117 of file obm_cyclone.c.

References foo.

Referenced by evaluate_obm_forces().

Here is the caller graph for this function:

cyclone_obm_moments()

static void cyclone_obm_moments ( const float  rates[3], const float  body_vel[3], const float  u[4], const float  coeff[22], float  F_obm_moments[3]  )

static

Definition at line 133 of file obm_cyclone.c.

References foo.

Referenced by evaluate_obm_moments().

Here is the caller graph for this function:

evaluate_obm_f_stb_u()

void evaluate_obm_f_stb_u	(	float	fu_mat[ANDI_NUM_ACT *ANDI_OUTPUTS],
		const struct FloatRates *	rates,
		const struct FloatVect3 *	vel_body,
		const float	actuator_state[ANDI_NUM_ACT]
	)

Evaluate the state-dependent control effectiveness matrix F_u for stabilization.

This function computes the mapping from actuator inputs to aerodynamic/motor outputs for the stabilization model. The output is written into the provided, pre-allocated array fu_mat of size ANDI_NUM_ACT * ANDI_OUTPUTS.

The produced matrix represents the partial derivatives of the modeled outputs with respect to actuator commands (∂f/∂u) evaluated at the current state and actuator conditions. It is intended to be implemented per-airframe (see obm_cyclone for an example).

Parameters

[out]	fu_mat	Pre-allocated array (size ANDI_NUM_ACT * ANDI_OUTPUTS) receiving the flattened control-effectiveness matrix. Convention: element for output i and actuator j should be written to fu_mat[i * ANDI_NUM_ACT + j].
[in]	rates	Current angular rates (typically p, q, r) used by the model (rad/s).
[in]	vel_body	Body-frame linear velocity vector (u, v, w) (m/s).
[in]	actuator_state	Current actuator commands/deflections array (length ANDI_NUM_ACT). Units and normalization depend on the airframe and actuator type (e.g. radians, throttle fraction).

Note

Implementations are airframe-specific and must account for the particular actuator layout and aerodynamic/motor characteristics of the platform.

See also

obm_cyclone.c

Definition at line 319 of file obm_cyclone.c.

References actuator_state, ANDI_NUM_ACT, ce_mat_tmp, cyclone_f_stb_u(), CeMatrix::data, CycloneCoefficients::data, foo, obm_coefficients, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

evaluate_obm_f_stb_x()

void evaluate_obm_f_stb_x	(	float	nu_obm[ANDI_OUTPUTS],
		const struct FloatRates *	rates,
		const struct FloatVect3 *	vel_body,
		const struct FloatVect3 *	ang_accel,
		const struct FloatVect3 *	accel_body,
		const float	actuator_state[ANDI_NUM_ACT]
	)

Evaluate the state-dependent contribution F_x * x_dot for stabilization.

Computes nu_obm = F_x * x_dot, the portion of the open-body-model outputs that depends on the current state rates/accelerations (i.e. the state-dependent term). This is the term that captures how changes in state drive the modeled outputs independent of actuator inputs.

Note

This contribution is often neglected for INDI, but can be included for ANDI.

Parameters

[out]	nu_obm	Pre-allocated output vector of length ANDI_OUTPUTS receiving the computed F_x * x_dot values.
[in]	rates	Current angular rates (p, q, r) used by the model (rad/s).
[in]	vel_body	Body-frame velocity vector (u, v, w) (m/s).
[in]	ang_accel	Body angular accelerations (p_dot, q_dot, r_dot) (rad/s^2).
[in]	accel_body	Body-frame linear accelerations (ax, ay, az) (m/s^2).
[in]	actuator_state	Current actuator commands/deflections array (length ANDI_NUM_ACT). Some terms of F_x may be actuator-state dependent (e.g. rotor wake effects).

Note

The function computes only the state-dependent part of the model. The full modeled output is typically nu_obm + F_u * delta_u (where F_u is provided by evaluate_obm_f_stb_u). Implementations must be provided per airframe.

See also

obm_cyclone.c

Definition at line 345 of file obm_cyclone.c.

References actuator_state, cyclone_f_stb_x(), CycloneCoefficients::data, foo, nu_obm, obm_coefficients, FloatRates::p, FloatRates::q, FloatRates::r, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

evaluate_obm_forces()

struct FloatVect3 evaluate_obm_forces	(	const struct FloatRates *	rates,
		const struct FloatVect3 *	vel_body,
		const float	actuator_state[ANDI_NUM_ACT]
	)

Evaluate total force acting on the vehicle from the OBM.

Computes the full set of forces modeled by the on board model for the given airframe as a function of the state and input.

This function is used for complementary filtering in ANDI.

Parameters

[in]	rates	Current angular rates (p, q, r) used by the model (rad/s).
[in]	vel_body	Body-frame linear velocity vector (u, v, w) (m/s).
[in]	actuator_state	Current actuator commands/deflections array (length ANDI_NUM_ACT). Units and normalization depend on the airframe and actuator type (e.g. radians, throttle fraction).

Returns

Computed force vector produced by the actuators as per the OBM.

Note

Implementation is airframe-specific.

See also

obm_cyclone.c

Definition at line 273 of file obm_cyclone.c.

References actuator_state, cyclone_obm_forces(), CycloneCoefficients::data, foo, obm_coefficients, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

evaluate_obm_moments()

struct FloatVect3 evaluate_obm_moments	(	const struct FloatRates *	rates,
		const struct FloatVect3 *	vel_body,
		const float	actuator_state[ANDI_NUM_ACT]
	)

Evaluate total moments acting on the vehicle from the OBM.

Computes the full set of moments modeled by the on board model for the given airframe as a function of the state and input.

This function is used for complementary filtering in ANDI.

Parameters

[in]	rates	Current angular rates (p, q, r) used by the model (rad/s).
[in]	vel_body	Body-frame linear velocity vector (u, v, w) (m/s).
[in]	actuator_state	Current actuator commands/deflections array (length ANDI_NUM_ACT). Units and normalization depend on the airframe and actuator type (e.g. radians, throttle fraction).

Returns

Computed moment vector produced by the actuators as per the OBM.

Note

Implementation is airframe-specific.

See also

obm_cyclone.c

Definition at line 294 of file obm_cyclone.c.

References actuator_state, cyclone_obm_moments(), CycloneCoefficients::data, foo, moments, obm_coefficients, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

evaluate_obm_thrust_z()

float evaluate_obm_thrust_z

(

const float

actuator_state[ANDI_NUM_ACT]

)

Compute total thrust produced by the current actuator state.

Returns the aggregate thrust generated by the set of actuators described by actuator_state. This value is typically used by the stabilization/obm code when converting actuator commands to net force for direct thrust control.

Note

This function does not intend to accurately compute the thrust force; it only returns an approximation of the total specific thrust in the body z-direction. The intended use is as feedback for direct throttle control in ANDI stabilization.

Parameters

[in]

actuator_state

Current actuator commands/deflections array (length ANDI_NUM_ACT). Units and normalization depend on the airframe (e.g. rotor collective, throttle fraction).

Returns

Total specific thrust produced by the actuators (SI units, m/s^2). If the airframe model uses a normalized thrust unit, document that convention in the airframe implementation.

Note

Implementation is airframe-specific.

See also

obm_cyclone.c

Definition at line 375 of file obm_cyclone.c.

References actuator_state, and obm_coefficients.

Referenced by stabilization_andi_enter(), and stabilization_andi_run().

Here is the caller graph for this function:

Variable Documentation

ce_mat_tmp

union CeMatrix ce_mat_tmp

Initial value:

= {
  .ce_11 = 0.0f,        .ce_12 = 0.0f,        .ce_13 = 3.9e-5f,     .ce_14 = -3.9e-5f, 
  .ce_21 = -29.917439f, .ce_22 = -29.917439f, .ce_23 = 0.0f,        .ce_24 = 0.0f,     
  .ce_31 = -19.968481f, .ce_32 = 19.968481f,  .ce_33 = 0.0f,        .ce_34 = 0.0f,     
  .ce_41 = 0.0f,        .ce_42 = 0.0f,        .ce_43 = 7e-6f,       .ce_44 = 7e-6f,    
}

Definition at line 108 of file obm_cyclone.c.

Referenced by evaluate_obm_f_stb_u().

obm_coefficients

union CycloneCoefficients obm_coefficients

Initial value:

= {
  .fx_motor_squared     = 0.00000735f,
  .fx_speed_forward     = -0.03f,
 
  .fy_speed_lateral     = -0.008f,
 
  .fz_motor_squared     = 0.0f,
  .fz_speed_forward     = 0.0f,
  .fz_speed_vertical    = -0.144f,
  .fz_elevon_speed      = 0.0f,
  .fz_elevon_motor      = 0.0f,
 
  .mx_elevon_motor_diff = 1.9e-05f,
  .mx_elevon_speed_diff = 0.344f,
  .mx_angular_drag      = -0.4940217f,
  .mx_angular_coupling  = -2.18f,
 
  .my_speed_forward     = 0.0f,
  .my_speed_vertical    = -0.0888f,
  .my_motor_sum         = 0.0f,
  .my_elevon_motor_sum  = -0.0000424f,
  .my_elevon_speed_sum  = 0.2525f,
  .my_angular_sum       = 1.262f,
 
  .mz_speed_lateral     = -0.00371f,
  .mz_motor_diff        = 0.000039f,
  .mz_speed_roll        = -0.0129f,
  .mz_angular_coupling  = -0.4827f,
}

Definition at line 54 of file obm_cyclone.c.

Referenced by evaluate_obm_f_stb_u(), evaluate_obm_f_stb_x(), evaluate_obm_forces(), evaluate_obm_moments(), and evaluate_obm_thrust_z().