// Thrust Vector Control ------------------------------------------------------------------------------------------------------------------------
// Note : RD253 can gimbal on 1 axis only
// reference direction of the engines
VECTOR3 vec_RD253dir = _V(0, 0, 1); // thrust direction, all the engines point straight downwards (should be -Z ?)
// Pitch/Yaw mixing functions
PY_mix1 = min(1, max(-1, Plvl + Ylvl));
PY_mix2 = min(1, max(-1, Plvl + -Ylvl));
// Engine 1 matrixes - straightforward, no radial rotation involved (angle = 0) -------------------------------
// used for roll control only
MATRIX3 matrix_eng1 = {
cos(GIMBAL_ANGLE*Rlvl),0,-sin(GIMBAL_ANGLE*Rlvl),
0,1,0,
sin(GIMBAL_ANGLE*Rlvl),0,cos(GIMBAL_ANGLE*Rlvl)
};
VECTOR3 output_vec_eng1 = tmul(matrix_eng1, vec_RD253dir);
SetThrusterDir(th_RD253[0], output_vec_eng1);
// Engine 2 - 60 DEG rotation --------------------------------------------------------------------------
MATRIX3 matrix_rot_eng2 = {
cos(ROTZ_60DEG), -sin(ROTZ_60DEG), 0,
sin(ROTZ_60DEG), cos(ROTZ_60DEG), 0,
0, 0, 1
};
MATRIX3 matrix_eng2x = {
1, 0, 0,
0, cos(GIMBAL_ANGLE*PY_mix2),-sin(GIMBAL_ANGLE*PY_mix2),
0, sin(GIMBAL_ANGLE*PY_mix2), cos(GIMBAL_ANGLE*PY_mix2)
};
MATRIX3 matrix_eng2y = {
cos(0), 0, -sin(0),
0, 1, 0,
sin(0), 0, cos(0),
};
MATRIX3 matrix_total2A = mul(matrix_eng2x, matrix_rot_eng2);
MATRIX3 matrix_total2B = mul(matrix_eng2y, matrix_rot_eng2);
MATRIX3 matrix_total2AB = mul(matrix_total2A, matrix_total2B);
VECTOR3 output_vec_eng2 = tmul(matrix_total2AB, vec_RD253dir);
SetThrusterDir(th_RD253[1], output_vec_eng2);
// Engine 3 - 120 DEG rotation --------------------------------------------------------------------------------------
MATRIX3 matrix_rot_eng3 = {
cos(ROTZ_60DEG*2), -sin(ROTZ_60DEG*2), 0,
sin(ROTZ_60DEG*2), cos(ROTZ_60DEG*2), 0,
0, 0, 1
};
MATRIX3 matrix_eng3x = {
1, 0, 0,
0, cos(GIMBAL_ANGLE*PY_mix1),-sin(GIMBAL_ANGLE*PY_mix1),
0, sin(GIMBAL_ANGLE*PY_mix1), cos(GIMBAL_ANGLE*PY_mix1)
};
MATRIX3 matrix_eng3y = {
cos(0), 0, -sin(0),
0, 1, 0,
sin(0), 0, cos(0),
};
MATRIX3 matrix_total3A = mul(matrix_eng3x, matrix_rot_eng3);
MATRIX3 matrix_total3B = mul(matrix_eng3y, matrix_rot_eng3);
MATRIX3 matrix_total3AB = mul(matrix_total3A, matrix_total3B);
VECTOR3 output_vec_eng3 = tmul(matrix_total3AB, vec_RD253dir);
SetThrusterDir(th_RD253[2], output_vec_eng3);
// Engine 4 - 180 DEG rotation -------------------------------------------------------------------------------------------
// reserved for roll channel
MATRIX3 matrix_eng4 = {
cos(GIMBAL_ANGLE*Rlvl),0,-sin(GIMBAL_ANGLE*Rlvl),
0,1,0,
sin(GIMBAL_ANGLE*Rlvl),0,cos(GIMBAL_ANGLE*Rlvl)
};
VECTOR3 output_vec_eng4 = tmul(matrix_eng4, vec_RD253dir);
SetThrusterDir(th_RD253[3], output_vec_eng4);
// Engine 5 - 240 DEG rotation -------------------------------------------------------------------------------------------
MATRIX3 matrix_rot_eng5 = {
cos(ROTZ_60DEG*4), -sin(ROTZ_60DEG*4), 0,
sin(ROTZ_60DEG*4), cos(ROTZ_60DEG*4), 0,
0, 0, 1
};
MATRIX3 matrix_eng5x = {
1, 0, 0,
0, cos(GIMBAL_ANGLE*PY_mix2),-sin(GIMBAL_ANGLE*PY_mix2),
0, sin(GIMBAL_ANGLE*PY_mix2), cos(GIMBAL_ANGLE*PY_mix2)
};
MATRIX3 matrix_eng5y = {
cos(0), 0, -sin(0),
0, 1, 0,
sin(0), 0, cos(0),
};
MATRIX3 matrix_total5A = mul(matrix_eng5x, matrix_rot_eng5);
MATRIX3 matrix_total5B = mul(matrix_eng5y, matrix_rot_eng5);
MATRIX3 matrix_total5AB = mul(matrix_total5A, matrix_total5B);
VECTOR3 output_vec_eng5 = tmul(matrix_total5AB, vec_RD253dir);
SetThrusterDir(th_RD253[4], output_vec_eng5);
// Engine 6 - 300 DEG rotation ------------------------------------------------------------------------------------------------
MATRIX3 matrix_rot_eng6 = {
cos(ROTZ_60DEG*5), -sin(ROTZ_60DEG*5), 0,
sin(ROTZ_60DEG*5), cos(ROTZ_60DEG*5), 0,
0, 0, 1
};
MATRIX3 matrix_eng6x = {
1, 0, 0,
0, cos(GIMBAL_ANGLE*PY_mix1),-sin(GIMBAL_ANGLE*PY_mix1),
0, sin(GIMBAL_ANGLE*PY_mix1), cos(GIMBAL_ANGLE*PY_mix1)
};
MATRIX3 matrix_eng6y = {
cos(0), 0, -sin(0),
0, 1, 0,
sin(0), 0, cos(0),
};
MATRIX3 matrix_total6A = mul(matrix_eng6x, matrix_rot_eng6);
MATRIX3 matrix_total6B = mul(matrix_eng6y, matrix_rot_eng6);
MATRIX3 matrix_total6AB = mul(matrix_total6A, matrix_total6B);
VECTOR3 output_vec_eng6 = tmul(matrix_total6AB, vec_RD253dir);
SetThrusterDir(th_RD253[5], output_vec_eng6);