HI @Romain_Chiappinelli , thanks once a gain for your response. Firstly, I’m a total beginner with c++. However, I can program in matlab and a bit in python so I have tried to joggle a few things as follows:

- I added a new parameter SIH_L_ARM in the src/modules/sih/sih_params.c file, this is because the tricopter has three different lengths, one corresponding to the roll, two separate ones which relate to pitch, where I have assumed the one between the two front rotors and CG along x-direction to be L_pitch and then the new length I defined is the one between the tail rotor and the CG along the x-direction, which is the length of each arm in essence.
- Added new default values for L_ROLL and L_PITCH.
- Modified the sih_parameters_updated() function to include the newly added parameter.
- Changed the model to that representing a tricopter

A snap shot of all these is below:

…

void Sih::parameters_updated()

{

_T_MAX = _sih_t_max.get();

_Q_MAX = _sih_q_max.get();

_L_ROLL = _sih_l_roll.get();

_L_PITCH = _sih_l_pitch.get();

_L_ARM = _sih_l_arm.get();

_KDV = _sih_kdv.get();

_KDW = _sih_kdw.get();

_H0 = _sih_h0.get();

…

void Sih::read_motors()

{

actuator_outputs_s actuators_out;

```
if (_actuator_out_sub.update(&actuators_out)) {
for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
if (i < NB_MOTORS) {
float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
} else {
float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f);
}
_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
}
}
```

}

Please see if the read_motors() one makes sense as per limiting the servo to {-1,1}.

void Sih::generate_force_and_torques()

{

_T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] * cos(_u[3])));

_Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1]),

_L_PITCH * _T_MAX * (+_u[0] + _u[1]) - (_L_ARM * _T_MAX * (_u[2] * cos(_u[3]))) - (_Q_MAX * (_u[2] * sin(_u[3]))),

_Q_MAX * (-_u[0] - _u[1] - _u[2] * cos(_u[3])) + (_L_ARM * _T_MAX * (_u[2] * sin(_u[3])));

The corresponding equations are:

So Ti = kt x omega_i^2, and Qi = kq x omega_i^2, where i = 1,2,3. Note I ignored Fy.

You can view it better on my fork here: PX4-Autopilot/sih.cpp at sih_tri · DanAbara/PX4-Autopilot · GitHub

and here PX4-Autopilot/sih_params.c at sih_tri · DanAbara/PX4-Autopilot · GitHub

Thanks a million in advance.

Best wishes,

Danielx