// Angular Acceleration
public static void angular_acceleration(ref Vector <float> omegadot, Vector <float> speeds, Vector <float> omega, Matrix <float> I, float L, float b, float k, QS_FRAME_MODE frame_mode)
{
Vector <float> tau = Vector <float> .Build.Dense(3, 0);
// The torques are NOT the same in (+) and (×) mode
if (frame_mode == QS_FRAME_MODE.QS_FRAME_MODE_PL)
{
torques_plus(ref tau, speeds, L, b, k);
}
else if (frame_mode == QS_FRAME_MODE.QS_FRAME_MODE_XH)
{
torques_xh(ref tau, speeds, L, b, k);
}
omegadot = I.Inverse() * (tau - Cross(omega, I * omega));
}
// =================================================================
// ========================== METHODS ==============================
// =================================================================
/*
*
* Constructor
*
*/
public Quadcopter()
{
// init frame mode
frame_mode = QS_FRAME_MODE_DEFAULT;
// initialize thread variables
thread_running = false;
// instanciate thread variable
simulation_mutex = new Mutex(true);
simulation_variables_mutex = new Mutex(true);
x = Vector <float> .Build.Dense(3, 0);
x_shared = Vector <float> .Build.Dense(3, 0);
xdot = Vector <float> .Build.Dense(3, 0);
xdot_shared = Vector <float> .Build.Dense(3, 0);
xdotdot = Vector <float> .Build.Dense(3, 0);
theta = Vector <float> .Build.Dense(3, 0);
theta_shared = Vector <float> .Build.Dense(3, 0);
thetadot = Vector <float> .Build.Dense(3, 0);
theta_user = Vector <float> .Build.Dense(3, 0);
pwmDutyCycle = Vector <float> .Build.Dense(4, 0);
rpm = Vector <float> .Build.Dense(4, 0);
rpm_shared = Vector <float> .Build.Dense(4, 0);
Inertia = Matrix <float> .Build.Dense(3, 3, 0);
escMotor0 = new esc_motor();
escMotor1 = new esc_motor();
escMotor2 = new esc_motor();
escMotor3 = new esc_motor();
accel = new Accelerometer();
gyros = new Gyroscope();
magne = new Magnetometer();
barom = new Barometer();
xdotdot_bf_sensor = Vector <float> .Build.Dense(3, 0);
thetadot_bf_sensor = Vector <float> .Build.Dense(3, 0);
magneticField_bf_sensor = Vector <float> .Build.Dense(3, 0);
sf = new sensorFusion();
theta_ef_sensor_fusion = Vector <float> .Build.Dense(3, 0);
thetadot_ef_sensor_fusion = Vector <float> .Build.Dense(3, 0);;
receiver = new receiver();
stabilizer = new stabilizer();
STAB_POSITION[2] = Z_START;
STAB_POSITION[0] = X_START;
STAB_POSITION[1] = Y_START;
trajectory_tracking_controller = new trajectory_tracking(CURRENT_POINTS, STAB_POSITION);
}
