55 .fx_motor_squared = 0.00000735f,
56 .fx_speed_forward = -0.03f,
58 .fy_speed_lateral = -0.008f,
60 .fz_motor_squared = 0.0f,
61 .fz_speed_forward = 0.0f,
62 .fz_speed_vertical = -0.144f,
63 .fz_elevon_speed = 0.0f,
64 .fz_elevon_motor = 0.0f,
66 .mx_elevon_motor_diff = 1.9e-05f,
67 .mx_elevon_speed_diff = 0.344f,
68 .mx_angular_drag = -0.4940217f,
69 .mx_angular_coupling = -2.18f,
71 .my_speed_forward = 0.0f,
72 .my_speed_vertical = -0.0888f,
74 .my_elevon_motor_sum = -0.0000424f,
75 .my_elevon_speed_sum = 0.2525f,
76 .my_angular_sum = 1.262f,
78 .mz_speed_lateral = -0.00371f,
79 .mz_motor_diff = 0.000039f,
80 .mz_speed_roll = -0.0129f,
81 .mz_angular_coupling = -0.4827f,
109 .ce_11 = 0.0f, .ce_12 = 0.0f, .ce_13 = 3.9e-5f, .ce_14 = -3.9e-5f,
110 .ce_21 = -29.917439f, .ce_22 = -29.917439f, .ce_23 = 0.0f, .ce_24 = 0.0f,
111 .ce_31 = -19.968481f, .ce_32 = 19.968481f, .ce_33 = 0.0f, .ce_34 = 0.0f,
112 .ce_41 = 0.0f, .ce_42 = 0.0f, .ce_43 = 7e-6f, .ce_44 = 7e-6f,
118 const float coeff[22],
float F_obm_forces[3])
123 t7 = sqrtf((body_vel[0] * body_vel[0] + body_vel[1] * body_vel[1]) + body_vel[2] * body_vel[2]);
124 F_obm_forces[0] = coeff[3] * t2
125 + coeff[7] * (u[0] * u[2] + u[1] * u[3])
126 + coeff[5] * t7 * body_vel[0]
127 - coeff[4] * t7 * body_vel[2]
128 - coeff[6] * t7 * body_vel[2] * (u[0] + u[1]);
129 F_obm_forces[1] = coeff[2] * t7 * body_vel[1];
130 F_obm_forces[2] = -coeff[0] * t2 + coeff[1] * t7 * body_vel[2];
134 const float body_vel[3],
const float u[4],
135 const float coeff[22],
float F_obm_moments[3])
142 t8 = sqrtf((body_vel[0] * body_vel[0] + body_vel[1] * body_vel[1]) + body_vel[2] * body_vel[2]);
143 F_obm_moments[0] = coeff[19] * (u[2] - u[3])
144 + coeff[18] * t8 * body_vel[1]
145 - coeff[20] * t8 * rates[2]
146 - rates[1] * coeff[21] * rates[2];
147 F_obm_moments[1] = coeff[15] * (t2 + t3)
148 + coeff[14] * (u[2] + u[3])
149 + coeff[13] * t8 * body_vel[0]
150 - coeff[12] * t8 * body_vel[2]
151 - rates[0] * coeff[17] * rates[2]
152 - coeff[16] * t8 * body_vel[2] * (u[0] + u[1]);
153 F_obm_moments[2] = -coeff[8] * (t2 - t3)
154 + coeff[10] * fabsf(rates[2]) * rates[2]
155 - rates[0] * coeff[11] * rates[1]
156 + coeff[9] * t8 * body_vel[2] * (u[0] - u[1]);
161 const float u[4],
const float coeff[22],
float F_stb_u[16])
169 t6 = sqrtf((body_vel[0] * body_vel[0] + body_vel[1] * body_vel[1]) + body_vel[2] * body_vel[2]);
170 t7 = coeff[9] * t6 * body_vel[2];
171 t6 = coeff[16] * t6 * body_vel[2];
173 fv[1] = -t6 + u[2] * coeff[15];
174 fv[2] = t7 - u[2] * coeff[8];
177 fv[5] = -t6 + u[3] * coeff[15];
178 fv[6] = -t7 + u[3] * coeff[8];
181 fv[9] = coeff[14] + u[0] * coeff[15];
182 fv[10] = -coeff[8] * u[0];
185 fv[13] = coeff[14] + u[1] * coeff[15];
186 fv[14] = u[1] * coeff[8];
190 for (i2 = 0; i2 < 16; i2++) {
191 F_stb_u[i1 + (i << 2)] = fv[i2];
202 const float vel_body[3],
const float accel_body[3],
const float u[4],
203 const float coeff[22],
float F_stb_x[4])
215 t8 = vel_body[0] * vel_body[0];
216 t9 = vel_body[1] * vel_body[1];
217 t10 = vel_body[2] * vel_body[2];
219 t15 = sqrtf((t8 + t9) + t10);
220 et1_tmp = fmaxf(1.0E-8F, t15);
222 -accel_body[1] * coeff[18] * t9
223 - accel_body[0] * coeff[18] * vel_body[0] * vel_body[1]
224 - accel_body[2] * coeff[18] * vel_body[1] * vel_body[2]
225 + accel_body[0] * coeff[20] * vel_body[0] * rates[2]
226 + accel_body[1] * coeff[20] * vel_body[1] * rates[2]
227 + accel_body[2] * coeff[20] * vel_body[2] * rates[2]
228 - accel_body[1] * coeff[18] * t15 * et1_tmp
229 + coeff[20] * t15 * ang_accel[2] * et1_tmp
230 + rates[1] * coeff[21] * ang_accel[2] * et1_tmp
231 + rates[2] * coeff[21] * ang_accel[1] * et1_tmp
234 t9 = accel_body[2] * coeff[12];
235 F_stb_x_tmp = accel_body[2] * coeff[16];
236 b_F_stb_x_tmp = accel_body[0] * coeff[16];
237 c_F_stb_x_tmp = accel_body[1] * coeff[16];
238 d_F_stb_x_tmp = F_stb_x_tmp * u[0];
241 -accel_body[0] * coeff[13] * t8 + t9 * t10
242 + accel_body[0] * coeff[12] * vel_body[0] * vel_body[2]
243 - accel_body[1] * coeff[13] * vel_body[0] * vel_body[1]
244 + accel_body[1] * coeff[12] * vel_body[1] * vel_body[2]
245 - accel_body[1] * coeff[12] * vel_body[1] * vel_body[2]
246 - accel_body[2] * coeff[13] * vel_body[0] * vel_body[2]
247 - accel_body[0] * coeff[13] * t15 * et1_tmp
249 + rates[0] * coeff[17] * ang_accel[2] * et1_tmp
250 + rates[2] * coeff[17] * ang_accel[0] * et1_tmp
251 + d_F_stb_x_tmp * t10
253 + b_F_stb_x_tmp * u[0] * vel_body[0] * vel_body[2]
254 + c_F_stb_x_tmp * u[0] * vel_body[1] * vel_body[2]
255 + b_F_stb_x_tmp * u[1] * vel_body[0] * vel_body[2]
256 + c_F_stb_x_tmp * u[1] * vel_body[1] * vel_body[2]
257 + d_F_stb_x_tmp * t15 * et1_tmp
258 + F_stb_x_tmp * t15 * et1_tmp
260 F_stb_x[2] = accel_body[2] * (coeff[9] * t12 * t15 + coeff[9] * t10 * t12 / et1_tmp)
261 - rates[0] * coeff[11] * ang_accel[1]
262 - rates[1] * coeff[11] * ang_accel[0]
263 + 2.0F * coeff[10] * fabsf(rates[2]) * ang_accel[2]
264 + accel_body[0] * coeff[9] * t12 * vel_body[0] * vel_body[2] / et1_tmp
265 + accel_body[1] * coeff[9] * t12 * vel_body[1] * vel_body[2] / et1_tmp;
322 float vel_body_array[3];
325 vel_body_array[0] = vel_body->
x;
326 vel_body_array[1] = vel_body->
y;
327 vel_body_array[2] = vel_body->
z;
349 float rates_array[3];
350 float ang_accel_array[3];
351 float vel_body_array[3];
352 float accel_body_array[3];
354 rates_array[0] = rates->
p;
355 rates_array[1] = rates->
q;
356 rates_array[2] = rates->
r;
358 ang_accel_array[0] = ang_accel->
x;
359 ang_accel_array[1] = ang_accel->
y;
360 ang_accel_array[2] = ang_accel->
z;
362 vel_body_array[0] = vel_body->
x;
363 vel_body_array[1] = vel_body->
y;
364 vel_body_array[2] = vel_body->
z;
366 accel_body_array[0] = accel_body->
x;
367 accel_body_array[1] = accel_body->
y;
368 accel_body_array[2] = accel_body->
z;
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.
static 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])
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.