1220 lines
52 KiB
C
1220 lines
52 KiB
C
//
|
|
// Created by SJQ on 2024/1/3.
|
|
//
|
|
|
|
#include "controller.h"
|
|
#include "cal_phi0_dot.h"
|
|
#include "arm_math.h"
|
|
/* Function Definitions */
|
|
/*
|
|
* Arguments : float phi_1
|
|
* float phi_4
|
|
* float d_phi1
|
|
* float d_phi4
|
|
* Return Type : float
|
|
*/
|
|
float get_dphi0(float phi_1, float phi_4, float d_phi1, float d_phi4)
|
|
{
|
|
//空间换时间 提前算好三角函数
|
|
|
|
float sin_phi1 = arm_sin_f32(phi_1);
|
|
float sin_phi4 = arm_sin_f32(phi_4);
|
|
float cos_phi1 = arm_cos_f32(phi_1);
|
|
float cos_phi4 = arm_cos_f32(phi_4);
|
|
|
|
float a;
|
|
float a_tmp;
|
|
float ab_a;
|
|
float ac_a;
|
|
float ad_a;
|
|
float b_a;
|
|
float b_a_tmp;
|
|
float b_d_phi0_tmp;
|
|
float b_d_phi0_tmp_tmp;
|
|
float b_out;
|
|
float bb_a;
|
|
float bc_a;
|
|
float bd_a;
|
|
float c_a;
|
|
float c_d_phi0_tmp;
|
|
float c_d_phi0_tmp_tmp;
|
|
float c_out;
|
|
float cb_a;
|
|
float cc_a;
|
|
float cd_a;
|
|
float d_a;
|
|
float d_d_phi0_tmp;
|
|
float d_d_phi0_tmp_tmp;
|
|
float d_out;
|
|
float d_phi0_tmp;
|
|
float d_phi0_tmp_tmp;
|
|
float db_a;
|
|
float dc_a;
|
|
float dd_a;
|
|
float e_a;
|
|
float e_d_phi0_tmp;
|
|
float e_d_phi0_tmp_tmp;
|
|
float e_out;
|
|
float eb_a;
|
|
float ec_a;
|
|
float ed_a;
|
|
float f_a;
|
|
float f_d_phi0_tmp;
|
|
float f_out;
|
|
float fb_a;
|
|
float fc_a;
|
|
float fd_a;
|
|
float g_a;
|
|
float g_d_phi0_tmp;
|
|
float g_out;
|
|
float gb_a;
|
|
float gc_a;
|
|
float gd_a;
|
|
float h_a;
|
|
float h_d_phi0_tmp;
|
|
float h_out;
|
|
float hb_a;
|
|
float hc_a;
|
|
float i_a;
|
|
float i_d_phi0_tmp;
|
|
float i_out;
|
|
float ib_a;
|
|
float ic_a;
|
|
float j_a;
|
|
float j_d_phi0_tmp;
|
|
float j_out;
|
|
float jb_a;
|
|
float jc_a;
|
|
float k_a;
|
|
float k_d_phi0_tmp;
|
|
float k_out;
|
|
float kb_a;
|
|
float kc_a;
|
|
float l_a;
|
|
float l_d_phi0_tmp;
|
|
float l_out;
|
|
float lb_a;
|
|
float lc_a;
|
|
float m_a;
|
|
float m_out;
|
|
float mb_a;
|
|
float mc_a;
|
|
float n_a;
|
|
float n_out;
|
|
float nb_a;
|
|
float nc_a;
|
|
float o_a;
|
|
float o_out;
|
|
float ob_a;
|
|
float oc_a;
|
|
float out;
|
|
float p_a;
|
|
float p_out;
|
|
float pb_a;
|
|
float pc_a;
|
|
float q_a;
|
|
float q_out;
|
|
float qb_a;
|
|
float qc_a;
|
|
float r_a;
|
|
float rb_a;
|
|
float rc_a;
|
|
float s_a;
|
|
float sb_a;
|
|
float sc_a;
|
|
float t_a;
|
|
float tb_a;
|
|
float tc_a;
|
|
float u_a;
|
|
float ub_a;
|
|
float uc_a;
|
|
float v_a;
|
|
float vb_a;
|
|
float vc_a;
|
|
float w_a;
|
|
float wb_a;
|
|
float wc_a;
|
|
float x_a;
|
|
float xb_a;
|
|
float xc_a;
|
|
float y_a;
|
|
float yb_a;
|
|
float yc_a;
|
|
d_phi0_tmp = cos_phi1;
|
|
d_phi0_tmp_tmp = sin_phi1;
|
|
b_d_phi0_tmp_tmp = 0.15F * d_phi0_tmp_tmp;
|
|
c_d_phi0_tmp_tmp = sin_phi4;
|
|
b_d_phi0_tmp = b_d_phi0_tmp_tmp - 0.15F * c_d_phi0_tmp_tmp;
|
|
d_d_phi0_tmp_tmp = 0.15F * d_phi0_tmp;
|
|
e_d_phi0_tmp_tmp = cos_phi4;
|
|
c_d_phi0_tmp = (0.15F - d_d_phi0_tmp_tmp) + 0.15F * e_d_phi0_tmp_tmp;
|
|
d_d_phi0_tmp = 0.54F * c_d_phi0_tmp;
|
|
e_d_phi0_tmp = 0.54F * b_d_phi0_tmp;
|
|
f_d_phi0_tmp = 0.3F * d_phi0_tmp * b_d_phi0_tmp;
|
|
g_d_phi0_tmp = 0.3F * d_phi0_tmp_tmp * c_d_phi0_tmp;
|
|
h_d_phi0_tmp = 2.0F * (f_d_phi0_tmp + g_d_phi0_tmp);
|
|
i_d_phi0_tmp = 0.0874800086F * d_phi0_tmp * b_d_phi0_tmp;
|
|
j_d_phi0_tmp = 0.0874800086F * d_phi0_tmp_tmp * c_d_phi0_tmp;
|
|
d_phi0_tmp *= 0.0810000077F;
|
|
f_d_phi0_tmp = (0.0810000077F * d_phi0_tmp_tmp + f_d_phi0_tmp) + g_d_phi0_tmp;
|
|
g_d_phi0_tmp = 0.3F * e_d_phi0_tmp_tmp * b_d_phi0_tmp;
|
|
d_phi0_tmp_tmp = 0.3F * c_d_phi0_tmp_tmp * c_d_phi0_tmp;
|
|
k_d_phi0_tmp = 2.0F * (g_d_phi0_tmp + d_phi0_tmp_tmp);
|
|
b_d_phi0_tmp *= 0.0874800086F * e_d_phi0_tmp_tmp;
|
|
c_d_phi0_tmp *= 0.0874800086F * c_d_phi0_tmp_tmp;
|
|
l_d_phi0_tmp = 0.0810000077F * e_d_phi0_tmp_tmp;
|
|
g_d_phi0_tmp =
|
|
(0.0810000077F * c_d_phi0_tmp_tmp + g_d_phi0_tmp) + d_phi0_tmp_tmp;
|
|
d_phi0_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
e_d_phi0_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
c_d_phi0_tmp_tmp = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
out = (e_d_phi0_tmp_tmp * e_d_phi0_tmp_tmp +
|
|
c_d_phi0_tmp_tmp * c_d_phi0_tmp_tmp) +
|
|
0.0729000047F;
|
|
e_d_phi0_tmp_tmp = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
c_d_phi0_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
b_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
c_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
d_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
e_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
f_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
b_out = (e_a * e_a + f_a * f_a) + 0.0729000047F;
|
|
e_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
f_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
g_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
h_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
i_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
j_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
c_out = (i_a * i_a + j_a * j_a) + 0.0729000047F;
|
|
i_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
j_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
k_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
a_tmp = 0.54F * ((0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4);
|
|
j_a = (((j_a * j_a + k_a * k_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
k_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
l_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
m_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
d_out = (l_a * l_a + m_a * m_a) + 0.0729000047F;
|
|
l_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
b_a_tmp = 0.54F * (0.15F * sin_phi1 - 0.15F * sin_phi4);
|
|
k_a = sqrtf((0.291600019F * (k_a * k_a) -
|
|
(d_out - 0.0729000047F) * (d_out - 0.0729000047F)) +
|
|
0.291600019F * (l_a * l_a)) -
|
|
b_a_tmp;
|
|
l_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
m_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
l_a = (((l_a * l_a + m_a * m_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
m_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
n_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
o_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
d_out = (n_a * n_a + o_a * o_a) + 0.0729000047F;
|
|
n_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
o_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
p_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
q_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
r_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
s_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
e_out = (r_a * r_a + s_a * s_a) + 0.0729000047F;
|
|
r_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
s_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
t_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
u_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
v_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
w_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
f_out = (v_a * v_a + w_a * w_a) + 0.0729000047F;
|
|
v_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
w_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
x_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
y_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ab_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
bb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
cb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
db_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
g_out = (cb_a * cb_a + db_a * db_a) + 0.0729000047F;
|
|
cb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
db_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
eb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
fb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
gb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
hb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
h_out = (gb_a * gb_a + hb_a * hb_a) + 0.0729000047F;
|
|
gb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
hb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ib_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
hb_a =
|
|
(((hb_a * hb_a + ib_a * ib_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
ib_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
jb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
kb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
i_out = (jb_a * jb_a + kb_a * kb_a) + 0.0729000047F;
|
|
jb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ib_a = sqrtf((0.291600019F * (ib_a * ib_a) -
|
|
(i_out - 0.0729000047F) * (i_out - 0.0729000047F)) +
|
|
0.291600019F * (jb_a * jb_a)) -
|
|
b_a_tmp;
|
|
jb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
kb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
jb_a =
|
|
(((jb_a * jb_a + kb_a * kb_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
kb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
lb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
mb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
i_out = (lb_a * lb_a + mb_a * mb_a) + 0.0729000047F;
|
|
lb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
kb_a = (0.27F * arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (kb_a * kb_a) -
|
|
(i_out - 0.0729000047F) *
|
|
(i_out - 0.0729000047F)) +
|
|
0.291600019F * (lb_a * lb_a)) -
|
|
b_a_tmp) /
|
|
((((mb_a * mb_a + nb_a * nb_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
a_tmp))) -
|
|
0.075F) +
|
|
0.15F * cos_phi1;
|
|
lb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
i_out = (mb_a * mb_a + nb_a * nb_a) + 0.0729000047F;
|
|
mb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
nb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ob_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
lb_a = 0.27F *
|
|
arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (lb_a * lb_a) -
|
|
(i_out - 0.0729000047F) *
|
|
(i_out - 0.0729000047F)) +
|
|
0.291600019F * (mb_a * mb_a)) -
|
|
b_a_tmp) /
|
|
((((nb_a * nb_a + ob_a * ob_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
a_tmp))) +
|
|
0.15F * sin_phi1;
|
|
mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
nb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ob_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
i_out = (nb_a * nb_a + ob_a * ob_a) + 0.0729000047F;
|
|
nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ob_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
pb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
mb_a = (0.27F * arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (mb_a * mb_a) -
|
|
(i_out - 0.0729000047F) *
|
|
(i_out - 0.0729000047F)) +
|
|
0.291600019F * (nb_a * nb_a)) -
|
|
b_a_tmp) /
|
|
((((ob_a * ob_a + pb_a * pb_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
a_tmp))) -
|
|
0.075F) +
|
|
0.15F * cos_phi1;
|
|
nb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ob_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
pb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
i_out = (ob_a * ob_a + pb_a * pb_a) + 0.0729000047F;
|
|
ob_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
pb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
qb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
rb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
sb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
tb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ub_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
vb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
j_out = (ub_a * ub_a + vb_a * vb_a) + 0.0729000047F;
|
|
ub_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
vb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
wb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
xb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
yb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ac_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
k_out = (yb_a * yb_a + ac_a * ac_a) + 0.0729000047F;
|
|
yb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ac_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
bc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ac_a =
|
|
(((ac_a * ac_a + bc_a * bc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
bc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
cc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
dc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
l_out = (cc_a * cc_a + dc_a * dc_a) + 0.0729000047F;
|
|
cc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
bc_a = sqrtf((0.291600019F * (bc_a * bc_a) -
|
|
(l_out - 0.0729000047F) * (l_out - 0.0729000047F)) +
|
|
0.291600019F * (cc_a * cc_a)) -
|
|
b_a_tmp;
|
|
cc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
dc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
cc_a =
|
|
(((cc_a * cc_a + dc_a * dc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
dc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ec_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
fc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
l_out = (ec_a * ec_a + fc_a * fc_a) + 0.0729000047F;
|
|
ec_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
fc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
gc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
hc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ic_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
jc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
m_out = (ic_a * ic_a + jc_a * jc_a) + 0.0729000047F;
|
|
ic_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
jc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
kc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
lc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
mc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
nc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
n_out = (mc_a * mc_a + nc_a * nc_a) + 0.0729000047F;
|
|
mc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
nc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
oc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
pc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
qc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
rc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
sc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
tc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
o_out = (sc_a * sc_a + tc_a * tc_a) + 0.0729000047F;
|
|
sc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
tc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
uc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
vc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
wc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
xc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
p_out = (wc_a * wc_a + xc_a * xc_a) + 0.0729000047F;
|
|
wc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
xc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
yc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
xc_a =
|
|
(((xc_a * xc_a + yc_a * yc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
yc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ad_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
bd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
q_out = (ad_a * ad_a + bd_a * bd_a) + 0.0729000047F;
|
|
ad_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
yc_a = sqrtf((0.291600019F * (yc_a * yc_a) -
|
|
(q_out - 0.0729000047F) * (q_out - 0.0729000047F)) +
|
|
0.291600019F * (ad_a * ad_a)) -
|
|
b_a_tmp;
|
|
ad_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
bd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ad_a =
|
|
(((ad_a * ad_a + bd_a * bd_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
bd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
cd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
dd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
q_out = (cd_a * cd_a + dd_a * dd_a) + 0.0729000047F;
|
|
cd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
dd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ed_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
bd_a = (0.27F * arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (bd_a * bd_a) -
|
|
(q_out - 0.0729000047F) *
|
|
(q_out - 0.0729000047F)) +
|
|
0.291600019F * (cd_a * cd_a)) -
|
|
b_a_tmp) /
|
|
((((dd_a * dd_a + ed_a * ed_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
a_tmp))) -
|
|
0.075F) +
|
|
0.15F * cos_phi1;
|
|
cd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
dd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ed_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
q_out = (dd_a * dd_a + ed_a * ed_a) + 0.0729000047F;
|
|
dd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ed_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
fd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
cd_a = 0.27F *
|
|
arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (cd_a * cd_a) -
|
|
(q_out - 0.0729000047F) *
|
|
(q_out - 0.0729000047F)) +
|
|
0.291600019F * (dd_a * dd_a)) -
|
|
b_a_tmp) /
|
|
((((ed_a * ed_a + fd_a * fd_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
a_tmp))) +
|
|
0.15F * sin_phi1;
|
|
dd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ed_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
fd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
q_out = (ed_a * ed_a + fd_a * fd_a) + 0.0729000047F;
|
|
ed_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
fd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
gd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
dd_a = (0.27F * arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (dd_a * dd_a) -
|
|
(q_out - 0.0729000047F) *
|
|
(q_out - 0.0729000047F)) +
|
|
0.291600019F * (ed_a * ed_a)) -
|
|
b_a_tmp) /
|
|
((((fd_a * fd_a + gd_a * gd_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
a_tmp))) -
|
|
0.075F) +
|
|
0.15F * cos_phi1;
|
|
return d_phi1 *
|
|
((d_d_phi0_tmp_tmp +
|
|
0.54F *
|
|
arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F *
|
|
(d_phi0_tmp_tmp * d_phi0_tmp_tmp) -
|
|
(out - 0.0729000047F) *
|
|
(out - 0.0729000047F)) +
|
|
0.291600019F * (e_d_phi0_tmp_tmp *
|
|
e_d_phi0_tmp_tmp)) -
|
|
e_d_phi0_tmp) /
|
|
((((c_d_phi0_tmp_tmp * c_d_phi0_tmp_tmp + a * a) +
|
|
0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp))) *
|
|
((((i_d_phi0_tmp - h_d_phi0_tmp * (((b_a * b_a + c_a * c_a) +
|
|
0.0729000047F) -
|
|
0.0729000047F)) +
|
|
j_d_phi0_tmp) /
|
|
(2.0F * sqrtf((0.291600019F * (d_a * d_a) -
|
|
(b_out - 0.0729000047F) *
|
|
(b_out - 0.0729000047F)) +
|
|
0.291600019F * (e_a * e_a))) -
|
|
d_phi0_tmp) /
|
|
((((f_a * f_a + g_a * g_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp) -
|
|
(sqrtf((0.291600019F * (h_a * h_a) -
|
|
(c_out - 0.0729000047F) * (c_out - 0.0729000047F)) +
|
|
0.291600019F * (i_a * i_a)) -
|
|
e_d_phi0_tmp) *
|
|
f_d_phi0_tmp / (j_a * j_a)) /
|
|
(k_a * k_a / (l_a * l_a) + 1.0F)) /
|
|
((0.27F *
|
|
arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (m_a * m_a) -
|
|
(d_out - 0.0729000047F) *
|
|
(d_out - 0.0729000047F)) +
|
|
0.291600019F * (n_a * n_a)) -
|
|
e_d_phi0_tmp) /
|
|
((((o_a * o_a + p_a * p_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp))) -
|
|
0.075F) +
|
|
d_d_phi0_tmp_tmp) +
|
|
(0.27F * arm_sin_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (q_a * q_a) -
|
|
(e_out - 0.0729000047F) *
|
|
(e_out - 0.0729000047F)) +
|
|
0.291600019F * (r_a * r_a)) -
|
|
e_d_phi0_tmp) /
|
|
((((s_a * s_a + t_a * t_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp))) +
|
|
b_d_phi0_tmp_tmp) *
|
|
(b_d_phi0_tmp_tmp +
|
|
0.54F *
|
|
arm_sin_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (u_a * u_a) -
|
|
(f_out - 0.0729000047F) *
|
|
(f_out - 0.0729000047F)) +
|
|
0.291600019F * (v_a * v_a)) -
|
|
e_d_phi0_tmp) /
|
|
((((w_a * w_a + x_a * x_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp))) *
|
|
((((i_d_phi0_tmp -
|
|
h_d_phi0_tmp *
|
|
(((y_a * y_a + ab_a * ab_a) + 0.0729000047F) -
|
|
0.0729000047F)) +
|
|
j_d_phi0_tmp) /
|
|
(2.0F * sqrtf((0.291600019F * (bb_a * bb_a) -
|
|
(g_out - 0.0729000047F) *
|
|
(g_out - 0.0729000047F)) +
|
|
0.291600019F * (cb_a * cb_a))) -
|
|
d_phi0_tmp) /
|
|
((((db_a * db_a + eb_a * eb_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp) -
|
|
(sqrtf((0.291600019F * (fb_a * fb_a) -
|
|
(h_out - 0.0729000047F) *
|
|
(h_out - 0.0729000047F)) +
|
|
0.291600019F * (gb_a * gb_a)) -
|
|
e_d_phi0_tmp) *
|
|
f_d_phi0_tmp / (hb_a * hb_a)) /
|
|
(ib_a * ib_a / (jb_a * jb_a) + 1.0F)) /
|
|
(kb_a * kb_a)) /
|
|
(lb_a * lb_a / (mb_a * mb_a) + 1.0F) -
|
|
d_phi4 *
|
|
(0.54F *
|
|
arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (nb_a * nb_a) -
|
|
(i_out - 0.0729000047F) *
|
|
(i_out - 0.0729000047F)) +
|
|
0.291600019F * (ob_a * ob_a)) -
|
|
e_d_phi0_tmp) /
|
|
((((pb_a * pb_a + qb_a * qb_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp))) *
|
|
((((b_d_phi0_tmp -
|
|
k_d_phi0_tmp *
|
|
(((rb_a * rb_a + sb_a * sb_a) + 0.0729000047F) -
|
|
0.0729000047F)) +
|
|
c_d_phi0_tmp) /
|
|
(2.0F * sqrtf((0.291600019F * (tb_a * tb_a) -
|
|
(j_out - 0.0729000047F) *
|
|
(j_out - 0.0729000047F)) +
|
|
0.291600019F * (ub_a * ub_a))) -
|
|
l_d_phi0_tmp) /
|
|
((((vb_a * vb_a + wb_a * wb_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp) -
|
|
(sqrtf((0.291600019F * (xb_a * xb_a) -
|
|
(k_out - 0.0729000047F) * (k_out - 0.0729000047F)) +
|
|
0.291600019F * (yb_a * yb_a)) -
|
|
e_d_phi0_tmp) *
|
|
g_d_phi0_tmp / (ac_a * ac_a)) /
|
|
((bc_a * bc_a / (cc_a * cc_a) + 1.0F) *
|
|
((0.27F * arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (dc_a * dc_a) -
|
|
(l_out - 0.0729000047F) *
|
|
(l_out - 0.0729000047F)) +
|
|
0.291600019F * (ec_a * ec_a)) -
|
|
e_d_phi0_tmp) /
|
|
((((fc_a * fc_a + gc_a * gc_a) +
|
|
0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp))) -
|
|
0.075F) +
|
|
d_d_phi0_tmp_tmp)) +
|
|
0.54F *
|
|
arm_sin_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (hc_a * hc_a) -
|
|
(m_out - 0.0729000047F) *
|
|
(m_out - 0.0729000047F)) +
|
|
0.291600019F * (ic_a * ic_a)) -
|
|
e_d_phi0_tmp) /
|
|
((((jc_a * jc_a + kc_a * kc_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp))) *
|
|
(0.27F *
|
|
arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (lc_a * lc_a) -
|
|
(n_out - 0.0729000047F) *
|
|
(n_out - 0.0729000047F)) +
|
|
0.291600019F * (mc_a * mc_a)) -
|
|
e_d_phi0_tmp) /
|
|
((((nc_a * nc_a + oc_a * oc_a) +
|
|
0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp))) +
|
|
b_d_phi0_tmp_tmp) *
|
|
((((b_d_phi0_tmp -
|
|
k_d_phi0_tmp *
|
|
(((pc_a * pc_a + qc_a * qc_a) + 0.0729000047F) -
|
|
0.0729000047F)) +
|
|
c_d_phi0_tmp) /
|
|
(2.0F * sqrtf((0.291600019F * (rc_a * rc_a) -
|
|
(o_out - 0.0729000047F) *
|
|
(o_out - 0.0729000047F)) +
|
|
0.291600019F * (sc_a * sc_a))) -
|
|
l_d_phi0_tmp) /
|
|
((((tc_a * tc_a + uc_a * uc_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_phi0_tmp) -
|
|
(sqrtf((0.291600019F * (vc_a * vc_a) -
|
|
(p_out - 0.0729000047F) * (p_out - 0.0729000047F)) +
|
|
0.291600019F * (wc_a * wc_a)) -
|
|
e_d_phi0_tmp) *
|
|
g_d_phi0_tmp / (xc_a * xc_a)) /
|
|
((yc_a * yc_a / (ad_a * ad_a) + 1.0F) * (bd_a * bd_a))) /
|
|
(cd_a * cd_a / (dd_a * dd_a) + 1.0F);
|
|
}
|
|
|
|
/* Function Definitions */
|
|
/*
|
|
* Arguments : float phi_1
|
|
* float phi_4
|
|
* float d_phi1
|
|
* float d_phi4
|
|
* Return Type : float
|
|
*/
|
|
float get_dL0(float phi_1, float phi_4, float d_phi1, float d_phi4)
|
|
{
|
|
//空间换时间 提前算好三角函数
|
|
|
|
float sin_phi1 = arm_sin_f32(phi_1);
|
|
float sin_phi4 = arm_sin_f32(phi_4);
|
|
float cos_phi1 = arm_cos_f32(phi_1);
|
|
float cos_phi4 = arm_cos_f32(phi_4);
|
|
|
|
|
|
float a;
|
|
float a_tmp;
|
|
float ab_a;
|
|
float ac_a;
|
|
float ad_a;
|
|
float b_a;
|
|
float b_a_tmp;
|
|
float b_d_L0_tmp;
|
|
float b_d_L0_tmp_tmp;
|
|
float b_d_L0_tmp_tmp_tmp;
|
|
float b_out;
|
|
float bb_a;
|
|
float bc_a;
|
|
float bd_a;
|
|
float c_a;
|
|
float c_d_L0_tmp;
|
|
float c_d_L0_tmp_tmp;
|
|
float c_out;
|
|
float cb_a;
|
|
float cc_a;
|
|
float cd_a;
|
|
float d_L0_tmp;
|
|
float d_L0_tmp_tmp;
|
|
float d_L0_tmp_tmp_tmp;
|
|
float d_a;
|
|
float d_d_L0_tmp;
|
|
float d_out;
|
|
float db_a;
|
|
float dc_a;
|
|
float dd_a;
|
|
float e_a;
|
|
float e_d_L0_tmp;
|
|
float e_out;
|
|
float eb_a;
|
|
float ec_a;
|
|
float ed_a;
|
|
float f_a;
|
|
float f_d_L0_tmp;
|
|
float f_out;
|
|
float fb_a;
|
|
float fc_a;
|
|
float g_a;
|
|
float g_d_L0_tmp;
|
|
float g_out;
|
|
float gb_a;
|
|
float gc_a;
|
|
float h_a;
|
|
float h_d_L0_tmp;
|
|
float h_out;
|
|
float hb_a;
|
|
float hc_a;
|
|
float i_a;
|
|
float i_d_L0_tmp;
|
|
float i_out;
|
|
float ib_a;
|
|
float ic_a;
|
|
float j_a;
|
|
float j_d_L0_tmp;
|
|
float j_out;
|
|
float jb_a;
|
|
float jc_a;
|
|
float k_a;
|
|
float k_d_L0_tmp;
|
|
float k_out;
|
|
float kb_a;
|
|
float kc_a;
|
|
float l_a;
|
|
float l_d_L0_tmp;
|
|
float l_out;
|
|
float lb_a;
|
|
float lc_a;
|
|
float m_a;
|
|
float m_out;
|
|
float mb_a;
|
|
float mc_a;
|
|
float n_a;
|
|
float n_out;
|
|
float nb_a;
|
|
float nc_a;
|
|
float o_a;
|
|
float o_out;
|
|
float ob_a;
|
|
float oc_a;
|
|
float out;
|
|
float p_a;
|
|
float p_out;
|
|
float pb_a;
|
|
float pc_a;
|
|
float q_a;
|
|
float q_out;
|
|
float qb_a;
|
|
float qc_a;
|
|
float r_a;
|
|
float rb_a;
|
|
float rc_a;
|
|
float s_a;
|
|
float sb_a;
|
|
float sc_a;
|
|
float t_a;
|
|
float tb_a;
|
|
float tc_a;
|
|
float u_a;
|
|
float ub_a;
|
|
float uc_a;
|
|
float v_a;
|
|
float vb_a;
|
|
float vc_a;
|
|
float w_a;
|
|
float wb_a;
|
|
float wc_a;
|
|
float x_a;
|
|
float xb_a;
|
|
float xc_a;
|
|
float y_a;
|
|
float yb_a;
|
|
float yc_a;
|
|
d_L0_tmp = cos_phi4;
|
|
d_L0_tmp_tmp = sin_phi4;
|
|
d_L0_tmp_tmp_tmp = sin_phi1;
|
|
b_d_L0_tmp_tmp = 0.15F * d_L0_tmp_tmp_tmp;
|
|
b_d_L0_tmp = b_d_L0_tmp_tmp - 0.15F * d_L0_tmp_tmp;
|
|
b_d_L0_tmp_tmp_tmp = cos_phi1;
|
|
c_d_L0_tmp_tmp = 0.15F * b_d_L0_tmp_tmp_tmp;
|
|
c_d_L0_tmp = (0.15F - c_d_L0_tmp_tmp) + 0.15F * d_L0_tmp;
|
|
d_d_L0_tmp = 0.54F * c_d_L0_tmp;
|
|
e_d_L0_tmp = 0.54F * b_d_L0_tmp;
|
|
f_d_L0_tmp = 0.3F * d_L0_tmp * b_d_L0_tmp;
|
|
g_d_L0_tmp = 0.3F * d_L0_tmp_tmp * c_d_L0_tmp;
|
|
h_d_L0_tmp = 2.0F * (f_d_L0_tmp + g_d_L0_tmp);
|
|
i_d_L0_tmp = 0.0874800086F * d_L0_tmp * b_d_L0_tmp;
|
|
j_d_L0_tmp = 0.0874800086F * d_L0_tmp_tmp * c_d_L0_tmp;
|
|
d_L0_tmp *= 0.0810000077F;
|
|
f_d_L0_tmp = (0.0810000077F * d_L0_tmp_tmp + f_d_L0_tmp) + g_d_L0_tmp;
|
|
g_d_L0_tmp = 0.3F * b_d_L0_tmp_tmp_tmp * b_d_L0_tmp;
|
|
d_L0_tmp_tmp = 0.3F * d_L0_tmp_tmp_tmp * c_d_L0_tmp;
|
|
k_d_L0_tmp = 2.0F * (g_d_L0_tmp + d_L0_tmp_tmp);
|
|
b_d_L0_tmp *= 0.0874800086F * b_d_L0_tmp_tmp_tmp;
|
|
c_d_L0_tmp *= 0.0874800086F * d_L0_tmp_tmp_tmp;
|
|
l_d_L0_tmp = 0.0810000077F * b_d_L0_tmp_tmp_tmp;
|
|
g_d_L0_tmp = (0.0810000077F * d_L0_tmp_tmp_tmp + g_d_L0_tmp) + d_L0_tmp_tmp;
|
|
d_L0_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
b_d_L0_tmp_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
d_L0_tmp_tmp_tmp = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
out = (b_d_L0_tmp_tmp_tmp * b_d_L0_tmp_tmp_tmp +
|
|
d_L0_tmp_tmp_tmp * d_L0_tmp_tmp_tmp) +
|
|
0.0729000047F;
|
|
b_d_L0_tmp_tmp_tmp = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
d_L0_tmp_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
b_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
c_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
d_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
e_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
f_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
b_out = (e_a * e_a + f_a * f_a) + 0.0729000047F;
|
|
e_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
f_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
g_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
h_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
i_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
j_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
c_out = (i_a * i_a + j_a * j_a) + 0.0729000047F;
|
|
i_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
j_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
k_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
a_tmp = 0.54F * ((0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4);
|
|
j_a = (((j_a * j_a + k_a * k_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
k_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
l_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
m_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
d_out = (l_a * l_a + m_a * m_a) + 0.0729000047F;
|
|
l_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
m_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
n_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
o_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
p_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
q_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
e_out = (p_a * p_a + q_a * q_a) + 0.0729000047F;
|
|
p_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
b_a_tmp = 0.54F * (0.15F * sin_phi1 - 0.15F * sin_phi4);
|
|
o_a = sqrtf((0.291600019F * (o_a * o_a) -
|
|
(e_out - 0.0729000047F) * (e_out - 0.0729000047F)) +
|
|
0.291600019F * (p_a * p_a)) -
|
|
b_a_tmp;
|
|
p_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
q_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
p_a = (((p_a * p_a + q_a * q_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
q_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
r_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
s_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
e_out = (r_a * r_a + s_a * s_a) + 0.0729000047F;
|
|
r_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
s_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
t_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
u_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
v_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
w_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
f_out = (v_a * v_a + w_a * w_a) + 0.0729000047F;
|
|
v_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
w_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
x_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
y_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ab_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
bb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
cb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
db_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
g_out = (cb_a * cb_a + db_a * db_a) + 0.0729000047F;
|
|
cb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
db_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
eb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
fb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
gb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
hb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
h_out = (gb_a * gb_a + hb_a * hb_a) + 0.0729000047F;
|
|
gb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
hb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ib_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
hb_a =
|
|
(((hb_a * hb_a + ib_a * ib_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
ib_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
jb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
kb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
i_out = (jb_a * jb_a + kb_a * kb_a) + 0.0729000047F;
|
|
jb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ib_a = sqrtf((0.291600019F * (ib_a * ib_a) -
|
|
(i_out - 0.0729000047F) * (i_out - 0.0729000047F)) +
|
|
0.291600019F * (jb_a * jb_a)) -
|
|
b_a_tmp;
|
|
jb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
kb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
jb_a =
|
|
(((jb_a * jb_a + kb_a * kb_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
kb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
lb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
mb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
i_out = (lb_a * lb_a + mb_a * mb_a) + 0.0729000047F;
|
|
lb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
kb_a = (0.27F * arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (kb_a * kb_a) -
|
|
(i_out - 0.0729000047F) *
|
|
(i_out - 0.0729000047F)) +
|
|
0.291600019F * (lb_a * lb_a)) -
|
|
b_a_tmp) /
|
|
((((mb_a * mb_a + nb_a * nb_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
a_tmp))) -
|
|
0.075F) +
|
|
0.15F * cos_phi1;
|
|
lb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
i_out = (mb_a * mb_a + nb_a * nb_a) + 0.0729000047F;
|
|
mb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
nb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ob_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
lb_a = 0.27F *
|
|
arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (lb_a * lb_a) -
|
|
(i_out - 0.0729000047F) *
|
|
(i_out - 0.0729000047F)) +
|
|
0.291600019F * (mb_a * mb_a)) -
|
|
b_a_tmp) /
|
|
((((nb_a * nb_a + ob_a * ob_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
a_tmp))) +
|
|
0.15F * sin_phi1;
|
|
mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
nb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ob_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
i_out = (nb_a * nb_a + ob_a * ob_a) + 0.0729000047F;
|
|
nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ob_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
pb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
qb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
rb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
sb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
tb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ub_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
j_out = (tb_a * tb_a + ub_a * ub_a) + 0.0729000047F;
|
|
tb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ub_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
vb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
wb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
xb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
yb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
k_out = (xb_a * xb_a + yb_a * yb_a) + 0.0729000047F;
|
|
xb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
yb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ac_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
yb_a =
|
|
(((yb_a * yb_a + ac_a * ac_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
ac_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
bc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
cc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
l_out = (bc_a * bc_a + cc_a * cc_a) + 0.0729000047F;
|
|
bc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ac_a = sqrtf((0.291600019F * (ac_a * ac_a) -
|
|
(l_out - 0.0729000047F) * (l_out - 0.0729000047F)) +
|
|
0.291600019F * (bc_a * bc_a)) -
|
|
b_a_tmp;
|
|
bc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
cc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
bc_a =
|
|
(((bc_a * bc_a + cc_a * cc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
cc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
dc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ec_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
l_out = (dc_a * dc_a + ec_a * ec_a) + 0.0729000047F;
|
|
dc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ec_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
fc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
gc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
hc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ic_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
m_out = (hc_a * hc_a + ic_a * ic_a) + 0.0729000047F;
|
|
hc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ic_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
jc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
kc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
lc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
mc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
n_out = (lc_a * lc_a + mc_a * mc_a) + 0.0729000047F;
|
|
lc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
mc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
nc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
oc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
pc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
qc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
rc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
sc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
o_out = (rc_a * rc_a + sc_a * sc_a) + 0.0729000047F;
|
|
rc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
sc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
tc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
uc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
vc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
wc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
p_out = (vc_a * vc_a + wc_a * wc_a) + 0.0729000047F;
|
|
vc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
wc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
xc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
wc_a =
|
|
(((wc_a * wc_a + xc_a * xc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
xc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
yc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ad_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
q_out = (yc_a * yc_a + ad_a * ad_a) + 0.0729000047F;
|
|
yc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
xc_a = sqrtf((0.291600019F * (xc_a * xc_a) -
|
|
(q_out - 0.0729000047F) * (q_out - 0.0729000047F)) +
|
|
0.291600019F * (yc_a * yc_a)) -
|
|
b_a_tmp;
|
|
yc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ad_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
yc_a =
|
|
(((yc_a * yc_a + ad_a * ad_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
|
|
ad_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
bd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
cd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
q_out = (bd_a * bd_a + cd_a * cd_a) + 0.0729000047F;
|
|
bd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
cd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
dd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
ad_a = (0.27F * arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (ad_a * ad_a) -
|
|
(q_out - 0.0729000047F) *
|
|
(q_out - 0.0729000047F)) +
|
|
0.291600019F * (bd_a * bd_a)) -
|
|
b_a_tmp) /
|
|
((((cd_a * cd_a + dd_a * dd_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
a_tmp))) -
|
|
0.075F) +
|
|
0.15F * cos_phi1;
|
|
bd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
cd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
dd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
q_out = (cd_a * cd_a + dd_a * dd_a) + 0.0729000047F;
|
|
cd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
dd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4;
|
|
ed_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4;
|
|
bd_a = 0.27F *
|
|
arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (bd_a * bd_a) -
|
|
(q_out - 0.0729000047F) *
|
|
(q_out - 0.0729000047F)) +
|
|
0.291600019F * (cd_a * cd_a)) -
|
|
b_a_tmp) /
|
|
((((dd_a * dd_a + ed_a * ed_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
a_tmp))) +
|
|
0.15F * sin_phi1;
|
|
return d_phi4 *
|
|
(1.08F *
|
|
arm_sin_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F *
|
|
(d_L0_tmp_tmp * d_L0_tmp_tmp) -
|
|
(out - 0.0729000047F) *
|
|
(out - 0.0729000047F)) +
|
|
0.291600019F * (b_d_L0_tmp_tmp_tmp *
|
|
b_d_L0_tmp_tmp_tmp)) -
|
|
e_d_L0_tmp) /
|
|
((((d_L0_tmp_tmp_tmp * d_L0_tmp_tmp_tmp + a * a) +
|
|
0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp))) *
|
|
((((i_d_L0_tmp -
|
|
h_d_L0_tmp * (((b_a * b_a + c_a * c_a) + 0.0729000047F) -
|
|
0.0729000047F)) +
|
|
j_d_L0_tmp) /
|
|
(2.0F * sqrtf((0.291600019F * (d_a * d_a) -
|
|
(b_out - 0.0729000047F) *
|
|
(b_out - 0.0729000047F)) +
|
|
0.291600019F * (e_a * e_a))) -
|
|
d_L0_tmp) /
|
|
((((f_a * f_a + g_a * g_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp) -
|
|
(sqrtf((0.291600019F * (h_a * h_a) -
|
|
(c_out - 0.0729000047F) * (c_out - 0.0729000047F)) +
|
|
0.291600019F * (i_a * i_a)) -
|
|
e_d_L0_tmp) *
|
|
f_d_L0_tmp / (j_a * j_a)) *
|
|
((0.27F *
|
|
arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (k_a * k_a) -
|
|
(d_out - 0.0729000047F) *
|
|
(d_out - 0.0729000047F)) +
|
|
0.291600019F * (l_a * l_a)) -
|
|
e_d_L0_tmp) /
|
|
((((m_a * m_a + n_a * n_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp))) -
|
|
0.075F) +
|
|
c_d_L0_tmp_tmp) /
|
|
(o_a * o_a / (p_a * p_a) + 1.0F) -
|
|
1.08F *
|
|
arm_cos_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (q_a * q_a) -
|
|
(e_out - 0.0729000047F) *
|
|
(e_out - 0.0729000047F)) +
|
|
0.291600019F * (r_a * r_a)) -
|
|
e_d_L0_tmp) /
|
|
((((s_a * s_a + t_a * t_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp))) *
|
|
(0.27F *
|
|
arm_sin_f32(2.0F *
|
|
atanf((sqrtf((0.291600019F * (u_a * u_a) -
|
|
(f_out - 0.0729000047F) *
|
|
(f_out - 0.0729000047F)) +
|
|
0.291600019F * (v_a * v_a)) -
|
|
e_d_L0_tmp) /
|
|
((((w_a * w_a + x_a * x_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp))) +
|
|
b_d_L0_tmp_tmp) *
|
|
((((i_d_L0_tmp - h_d_L0_tmp * (((y_a * y_a + ab_a * ab_a) +
|
|
0.0729000047F) -
|
|
0.0729000047F)) +
|
|
j_d_L0_tmp) /
|
|
(2.0F * sqrtf((0.291600019F * (bb_a * bb_a) -
|
|
(g_out - 0.0729000047F) *
|
|
(g_out - 0.0729000047F)) +
|
|
0.291600019F * (cb_a * cb_a))) -
|
|
d_L0_tmp) /
|
|
((((db_a * db_a + eb_a * eb_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp) -
|
|
(sqrtf((0.291600019F * (fb_a * fb_a) -
|
|
(h_out - 0.0729000047F) * (h_out - 0.0729000047F)) +
|
|
0.291600019F * (gb_a * gb_a)) -
|
|
e_d_L0_tmp) *
|
|
f_d_L0_tmp / (hb_a * hb_a)) /
|
|
(ib_a * ib_a / (jb_a * jb_a) + 1.0F)) /
|
|
(2.0F * sqrtf(kb_a * kb_a + lb_a * lb_a)) -
|
|
d_phi1 *
|
|
(2.0F *
|
|
(b_d_L0_tmp_tmp +
|
|
0.54F *
|
|
arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (mb_a * mb_a) -
|
|
(i_out - 0.0729000047F) *
|
|
(i_out - 0.0729000047F)) +
|
|
0.291600019F * (nb_a * nb_a)) -
|
|
e_d_L0_tmp) /
|
|
((((ob_a * ob_a + pb_a * pb_a) +
|
|
0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp))) *
|
|
((((b_d_L0_tmp -
|
|
k_d_L0_tmp *
|
|
(((qb_a * qb_a + rb_a * rb_a) + 0.0729000047F) -
|
|
0.0729000047F)) +
|
|
c_d_L0_tmp) /
|
|
(2.0F * sqrtf((0.291600019F * (sb_a * sb_a) -
|
|
(j_out - 0.0729000047F) *
|
|
(j_out - 0.0729000047F)) +
|
|
0.291600019F * (tb_a * tb_a))) -
|
|
l_d_L0_tmp) /
|
|
((((ub_a * ub_a + vb_a * vb_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp) -
|
|
(sqrtf((0.291600019F * (wb_a * wb_a) -
|
|
(k_out - 0.0729000047F) *
|
|
(k_out - 0.0729000047F)) +
|
|
0.291600019F * (xb_a * xb_a)) -
|
|
e_d_L0_tmp) *
|
|
g_d_L0_tmp / (yb_a * yb_a)) /
|
|
(ac_a * ac_a / (bc_a * bc_a) + 1.0F)) *
|
|
((0.27F *
|
|
arm_cos_f32(2.0F * atanf((sqrtf((0.291600019F * (cc_a * cc_a) -
|
|
(l_out - 0.0729000047F) *
|
|
(l_out - 0.0729000047F)) +
|
|
0.291600019F * (dc_a * dc_a)) -
|
|
e_d_L0_tmp) /
|
|
((((ec_a * ec_a + fc_a * fc_a) +
|
|
0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp))) -
|
|
0.075F) +
|
|
c_d_L0_tmp_tmp) -
|
|
2.0F *
|
|
(0.27F *
|
|
arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (gc_a * gc_a) -
|
|
(m_out - 0.0729000047F) *
|
|
(m_out - 0.0729000047F)) +
|
|
0.291600019F * (hc_a * hc_a)) -
|
|
e_d_L0_tmp) /
|
|
((((ic_a * ic_a + jc_a * jc_a) +
|
|
0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp))) +
|
|
b_d_L0_tmp_tmp) *
|
|
(c_d_L0_tmp_tmp +
|
|
0.54F *
|
|
arm_cos_f32(2.0F * atanf((sqrtf((0.291600019F * (kc_a * kc_a) -
|
|
(n_out - 0.0729000047F) *
|
|
(n_out - 0.0729000047F)) +
|
|
0.291600019F * (lc_a * lc_a)) -
|
|
e_d_L0_tmp) /
|
|
((((mc_a * mc_a + nc_a * nc_a) +
|
|
0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp))) *
|
|
((((b_d_L0_tmp -
|
|
k_d_L0_tmp *
|
|
(((oc_a * oc_a + pc_a * pc_a) + 0.0729000047F) -
|
|
0.0729000047F)) +
|
|
c_d_L0_tmp) /
|
|
(2.0F * sqrtf((0.291600019F * (qc_a * qc_a) -
|
|
(o_out - 0.0729000047F) *
|
|
(o_out - 0.0729000047F)) +
|
|
0.291600019F * (rc_a * rc_a))) -
|
|
l_d_L0_tmp) /
|
|
((((sc_a * sc_a + tc_a * tc_a) + 0.0729000047F) -
|
|
0.0729000047F) +
|
|
d_d_L0_tmp) -
|
|
(sqrtf((0.291600019F * (uc_a * uc_a) -
|
|
(p_out - 0.0729000047F) *
|
|
(p_out - 0.0729000047F)) +
|
|
0.291600019F * (vc_a * vc_a)) -
|
|
e_d_L0_tmp) *
|
|
g_d_L0_tmp / (wc_a * wc_a)) /
|
|
(xc_a * xc_a / (yc_a * yc_a) + 1.0F))) /
|
|
(2.0F * sqrtf(ad_a * ad_a + bd_a * bd_a));
|
|
}
|
|
|
|
|
|
/*
|
|
* File trailer for get_dphi0.c
|
|
*
|
|
* [EOF]
|
|
*/
|