C Module: thrForceMapping

Auxiliary Files

• Unit tests

Example usage

• scenarioAttitudeFeedback2T_stateEffTH

• scenarioAttitudeFeedback2T_TH

• scenarioMomentumDumping

Executive Summary

This module is reads in a desired attitude control torque vector and maps it onto a set of thrusters.

The module works for both on-pulsing (nominal thruster state is off such as with RCS thrusters) and off-pulsing (nominal thruster state in such as with DV thrusters). More information can be found in the PDF Description.

Message Connection Descriptions

The following diagram and table list all the module input and output messages. The module message connection is set by the user from Python. The message type contains a link to the message structure definition, while the description provides information on what this message is used for.

Module I/O Messages

Msg Variable Name	Msg Type	Description
thrForceCmdOutMsg	THRArrayCmdForceMsgPayload	thruster force output message
cmdTorqueInMsg	CmdTorqueBodyMsgPayload	commanded attitude control torque vector input message
thrConfigInMsg	THRArrayConfigMsgPayload	Thruster array configuration input message
vehConfigInMsg	VehicleConfigMsgPayload	spacecraft configuration input message

Module Assumptions and Limitations

The variable controlAxes_B holds up to three orthonormal control axes \(\hat{\mathbf{c}}_j\), stored row by row in body frame components as the matrix \([C]\). Unused rows are zero-padded. Reset() normalizes each axis, and logs a BSK_WARNING if any two axes are not orthogonal, as the math below then no longer holds.

Let \({}^{\cal B}\mathbf{L}_{\mathrm{cmd}}\) be the input torque request and \(\mathbf{F}_{\mathrm{nom}}\) the nominal thruster forces. For on-pulsing (thrForceSign = +1), \(\mathbf{F}_{\mathrm{nom}}\) is zero and the commands \(\mathbf{F}\) are nonnegative forces. For off-pulsing (thrForceSign = -1), \(\mathbf{F}_{\mathrm{nom}} = \mathbf{F}_{\max}\) and \(\mathbf{F}\) represents nonpositive changes from those nominal forces: a zero command leaves a thruster fully on, while \(F_i = -F_{\max,i}\) requests that it remain off for the control period. The allocator subtracts the nominal thruster torque before projecting the request:

\[{}^{\cal B}\mathbf{L}_r = {}^{\cal B}\mathbf{L}_{\mathrm{cmd}} - [D]\mathbf{F}_{\mathrm{nom}}.\]

This adjusted target is reduced onto the control axes, written in body frame components, as

\[{}^{\cal B}\bar{\mathbf{L}}_r = [C]^T [C] \, {}^{\cal B}\mathbf{L}_r\]

The allocator attempts to find thruster forces \(\mathbf{F}\) satisfying \([C][D]\mathbf{F} = [C]\,{}^{\cal B}\mathbf{L}_r\). For off-pulsing, positive commands no larger than 64 * DBL_EPSILON times the largest absolute command are clamped to zero. The original two-stage allocation is otherwise retained if every command is nonpositive. A larger positive command triggers a nonnegative least-squares fallback over all thrusters:

\[\min_{\mathbf{u}\geq 0}\left\|-[C][D]\mathbf{u}-[C]\,{}^{\cal B}\mathbf{L}_r\right\|_2^2, \qquad \mathbf{F}=-\mathbf{u}.\]

Among allocations with the best achievable controlled torque, a second step minimizes \(\|\mathbf{u}\|_2\). This gives a unique minimum-force solution, including for proportional thruster columns, and avoids selecting a large weak-thruster command when smaller commands can produce the same torque. The solver does not impose thrust-magnitude limits. Both steps use deterministic active-set methods that add and release thrusters and solve their subproblems using reorthogonalized QR. The first step needs at most three independent thruster columns; the second can distribute force over all thrusters. A row-space projection of the current feasible forces gives the second step’s minimum-norm trial while preserving its achieved torque. This avoids cancellation that can cause cycling at zero-force boundaries. One common map normalization preserves relative torque authority, and target normalization makes the numerical tests relative to the problem scale. Gradient comparisons use a tolerance to handle numerical ties. The two steps share a limit of 6 * MAX_EFF_CNT active-set steps (216 with the current 36-thruster limit). If they cannot converge within that budget or numerical rank prevents further progress, it logs BSK_WARNING and retains its last nonpositive allocation. All allocations then use the saturation policy below. Existing singular-matrix handling in the two-stage allocator is unchanged; an all-zero allocation does not trigger sign repair.

The magnitudes of the commanded force changes \(\mathbf{F}\) can exceed the individual thruster limits \(F_{\max,i}\). The angle calculation uses the saturation-clipped commands \(\mathbf{F}_{\mathrm{clip}}\) and their torque contribution:

\[F_{\mathrm{clip},i} = \min\left(F_{\max,i}, \max\left(-F_{\max,i}, F_i\right)\right), \qquad {}^{\cal B}\boldsymbol{\tau} = [D]\mathbf{F}_{\mathrm{clip}}.\]

The corresponding ideal average net thruster torque is \({}^{\cal B}\boldsymbol{\tau}_{\mathrm{net}} = [D](\mathbf{F}_{\mathrm{nom}} + \mathbf{F}_{\mathrm{clip}})\). In off-pulsing mode, \(\boldsymbol{\tau}\) is the change from nominal torque, and differs from \(\boldsymbol{\tau}_{\mathrm{net}}\) when the nominal thrust produces a nonzero torque.

The output outTorqAngErr is the angle between this torque contribution and the reduced adjusted target \({}^{\cal B}\bar{\mathbf{L}}_r\), both in body frame components:

\[\theta = \arccos \left( \frac{\boldsymbol{\tau} \cdot \bar{\mathbf{L}}_r}{|\boldsymbol{\tau}| \, |\bar{\mathbf{L}}_r|} \right)\]

This angle is invariant under reordering the control axes or choosing another orthonormal basis for the same controlled subspace. Changing the controlled subspace can change the angle and force solution.

If this angle is larger than angErrThresh [rad] and any commanded force exceeds its limit, all commanded forces are scaled by the same positive factor to satisfy every thruster limit. The resulting torque contribution preserves the direction of the unscaled contribution \([D]\mathbf{F}\). It aligns with \(\bar{\mathbf{L}}_r\) only if that unscaled torque was already aligned; residual torque outside the controlled subspace is also scaled and can leave a nonzero angle error. In off-pulsing mode, these direction statements concern the change from nominal torque; the net torque also includes the unchanged nominal contribution \([D]\mathbf{F}_{\max}\). The module recomputes outTorqAngErr after scaling. If the angle is at or below the threshold, the output force commands can remain above their limits even though the angle calculation uses clipped forces. The default value of angErrThresh is 0 rad. Setting it to a value larger than \(\pi\) rad disables scaling.

Thruster locations, force directions, and maximum thrust values are read from thrConfigInMsg during Reset() and remain cached until the next reset. The spacecraft center of mass is read from vehConfigInMsg on every update, and the module recomputes the thruster moment arms using the current CoM_B. Center-of-mass changes therefore do not require a reset.

---

Functions

void SelfInit_thrForceMapping(thrForceMappingConfig *configData, int64_t moduleID)

self init method

Parameters:

• configData – The configuration data associated with this module

• moduleID – The ID associated with the configData

void Update_thrForceMapping(thrForceMappingConfig *configData, uint64_t callTime, int64_t moduleID)

The module takes a body frame torque vector and projects it onto available RCS or DV thrusters.

Parameters:

• configData – The configuration data associated with the module

• callTime – The clock time at which the function was called (nanoseconds)

• moduleID – The ID associated with the configData

void Reset_thrForceMapping(thrForceMappingConfig *configData, uint64_t callTime, int64_t moduleID)

This method performs a complete reset of the module. Local module variables that retain time varying states between function calls are reset to their default values.

Parameters:

• configData – The configuration data associated with the module

• callTime – The clock time at which the function was called (nanoseconds)

• moduleID – The ID associated with the configData

void substractMin(double *F, uint32_t size)

Take a stack of force values find the smallest value, and subtract if from all force values. Here the smallest values will become zero, while other forces increase. This assumes that the thrusters are aligned such that if all thrusters are firing, then no torque or force is applied. This ensures only positive force values are computed.

void findMinimumNormForce(thrForceMappingConfig *configData, double D[3][MAX_EFF_CNT], double Lr_B[3], uint32_t numForces, double F[MAX_EFF_CNT])

Use a least square inverse to determine the smallest set of thruster forces that yield the desired torque vector. Note that this routine does not constrain yet the forces to be either positive or negative

double computeTorqueAngErr(double D[3][MAX_EFF_CNT], double BLr_B[3], uint32_t numForces, double epsilon, double F[MAX_EFF_CNT], double FMag[MAX_EFF_CNT])

Determine the angle between the desired torque vector and the actual torque vector.

Note

Both torque vectors must be expressed in body frame components.

Parameters:

• D – Mapping matrix from thruster forces to body frame torque [m]

• BLr_B – Desired reduced control torque [C]^T [C] Lr in body frame components [Nm]

• numForces – Number of thruster forces to evaluate

• epsilon – Small value below which the desired torque is treated as zero [Nm]

• F – Commanded thruster forces [N]

• FMag – Thruster force saturation limits [N]

Returns:

Angle between the desired and the actual body frame torque vector [rad]

struct thrForceMappingConfig

#include <thrForceMapping.h>

Data structure for module to map a command torque onto thruster forces.

Public Members

double controlAxes_B[3 * 3]

[] array of the control unit axes

double rThruster_B[MAX_EFF_CNT][3]

[m] local copy of the thruster locations

double gtThruster_B[MAX_EFF_CNT][3]

[] local copy of the thruster force unit direction vectors

int32_t thrForceSign

[] Flag indicating if pos (+1) or negative (-1) thruster solutions are found

double angErrThresh

[r] Angular error at which thruster forces are scaled to not be super-saturated

double epsilon

variable specifying what is considered a small number

uint32_t use2ndLoop

[] flag indicating if the 2nd least squares fitting loop should be used (1) or not used (0 - default)

uint32_t numControlAxes

[] counter indicating how many orthogonal axes are controlled

uint32_t numThrusters

[] The number of thrusters available on vehicle

double outTorqAngErr

[r] Angular error of effector torque

double thrForcMag[MAX_EFF_CNT]

vector of thruster force magnitudes

THRArrayCmdForceMsg_C thrForceCmdOutMsg

The name of the output thruster force message.

CmdTorqueBodyMsg_C cmdTorqueInMsg

The name of the vehicle control (Lr) Input message.

THRArrayConfigMsg_C thrConfigInMsg

The name of the thruster cluster Input message.

VehicleConfigMsg_C vehConfigInMsg

The name of the Input message.

VehicleConfigMsgPayload sc

spacecraft configuration message

BSKLogger *bskLogger

BSK Logging.