diff --git a/bindings/cffirmware.i b/bindings/cffirmware.i index 62ca332c46..1626ab82fe 100755 --- a/bindings/cffirmware.i +++ b/bindings/cffirmware.i @@ -133,13 +133,21 @@ void collisionAvoidanceUpdateSetpointWrap( free(workspace); } -void state_set_neighbor_position(state_t *state, int idx, float x, float y, float z) +void state_set_neighbor_position(state_t *state, int idx, uint8_t id, float x, float y, float z) { - state->position_neighbors[idx].x = x; - state->position_neighbors[idx].y = y; - state->position_neighbors[idx].z = z; + state->neighbors[idx].id = id; + state->neighbors[idx].pos.x = x; + state->neighbors[idx].pos.y = y; + state->neighbors[idx].pos.z = z; } +void controller_lee_payload_set_attachement(controllerLeePayload_t* self, int idx, uint8_t id, float x, float y, float z) +{ + self->attachement_points[idx].id = id; + self->attachement_points[idx].point.x = x; + self->attachement_points[idx].point.y = y; + self->attachement_points[idx].point.z = z; +} %} %pythoncode %{ diff --git a/bindings/setup.py b/bindings/setup.py index c9bfa1479f..e64e26d4c8 100644 --- a/bindings/setup.py +++ b/bindings/setup.py @@ -43,6 +43,7 @@ "src/lib/osqp/src/osqp/util.c", "src/lib/osqp/src/osqp/workspace_2uav_2hp.c", "src/lib/osqp/src/osqp/workspace_3uav_2hp.c", + "src/lib/osqp/src/osqp/workspace_3uav_2hp_rig.c", ] cffirmware = Extension( diff --git a/src/hal/interface/usec_time.h b/src/hal/interface/usec_time.h index 8a48ea28ff..6c8160e934 100644 --- a/src/hal/interface/usec_time.h +++ b/src/hal/interface/usec_time.h @@ -32,6 +32,11 @@ */ void initUsecTimer(void); +/** + * Reset the microsecond-resolution timer to 0. + */ +void usecTimerReset(void); + /** * Get microsecond-resolution timestamp. */ diff --git a/src/hal/src/usec_time.c b/src/hal/src/usec_time.c index 6971d308ea..59d302880f 100644 --- a/src/hal/src/usec_time.c +++ b/src/hal/src/usec_time.c @@ -71,6 +71,16 @@ void initUsecTimer(void) isInit = true; } +void usecTimerReset(void) +{ + IF_DEBUG_ASSERT(isInit); + + const uint32_t zero = 0; + __atomic_store(&usecTimerHighCount, &zero, __ATOMIC_SEQ_CST); + + TIM7->CNT = 0; +} + uint64_t usecTimestamp(void) { IF_DEBUG_ASSERT(isInit); diff --git a/src/lib/Kbuild b/src/lib/Kbuild index 2c709986dc..2105f45815 100644 --- a/src/lib/Kbuild +++ b/src/lib/Kbuild @@ -58,3 +58,4 @@ obj-y += osqp/src/osqp/scaling.o obj-y += osqp/src/osqp/util.o obj-y += osqp/src/osqp/workspace_2uav_2hp.o obj-y += osqp/src/osqp/workspace_3uav_2hp.o +obj-y += osqp/src/osqp/workspace_3uav_2hp_rig.o diff --git a/src/lib/osqp/src/osqp/workspace_2uav_2hp.c b/src/lib/osqp/src/osqp/workspace_2uav_2hp.c index 19fa99d00f..7f2677ea0c 100644 --- a/src/lib/osqp/src/osqp/workspace_2uav_2hp.c +++ b/src/lib/osqp/src/osqp/workspace_2uav_2hp.c @@ -8,7 +8,7 @@ #include "qdldl_interface.h" // Define data structure -c_int Pdata_i[6] = { +static c_int Pdata_i[6] = { 0, 1, 2, @@ -16,7 +16,7 @@ c_int Pdata_i[6] = { 4, 5, }; -c_int Pdata_p[7] = { +static c_int Pdata_p[7] = { 0, 1, 2, @@ -25,7 +25,7 @@ c_int Pdata_p[7] = { 5, 6, }; -c_float Pdata_x[6] = { +static c_float Pdata_x[6] = { (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, @@ -33,8 +33,8 @@ c_float Pdata_x[6] = { (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, }; -csc Pdata = {6, 6, 6, Pdata_p, Pdata_i, Pdata_x, -1}; -c_int Adata_i[12] = { +static csc Pdata = {6, 6, 6, Pdata_p, Pdata_i, Pdata_x, -1}; +static c_int Adata_i[12] = { 0, 3, 1, @@ -48,7 +48,7 @@ c_int Adata_i[12] = { 2, 4, }; -c_int Adata_p[7] = { +static c_int Adata_p[7] = { 0, 2, 4, @@ -57,7 +57,7 @@ c_int Adata_p[7] = { 10, 12, }; -c_float Adata_x[12] = { +static c_float Adata_x[12] = { (c_float)1.00000000000000000000, (c_float)0.17594719965875199597, (c_float)1.00000000000000000000, @@ -71,8 +71,8 @@ c_float Adata_x[12] = { (c_float)1.00000000000000000000, (c_float)0.36318629375584099428, }; -csc Adata = {12, 5, 6, Adata_p, Adata_i, Adata_x, -1}; -c_float qdata[6] = { +static csc Adata = {12, 5, 6, Adata_p, Adata_i, Adata_x, -1}; +static c_float qdata[6] = { (c_float)0.00000000000000000000, (c_float)0.00000000000000000000, (c_float)0.00000000000000000000, @@ -80,27 +80,27 @@ c_float qdata[6] = { (c_float)0.00000000000000000000, (c_float)0.00000000000000000000, }; -c_float ldata[5] = { +static c_float ldata[5] = { (c_float)0.00000000000000000000, (c_float)0.00000000000000000000, (c_float)0.09810000000000000664, (c_float)-9077669334311812014674932662272.00000000000000000000, (c_float)-9077669334311812014674932662272.00000000000000000000, }; -c_float udata[5] = { +static c_float udata[5] = { (c_float)0.00000000000000000000, (c_float)0.00000000000000000000, (c_float)0.09810000000000000664, (c_float)0.00000000000000000000, (c_float)0.00000000000000000000, }; -OSQPData data = {6, 5, &Pdata, &Adata, qdata, ldata, udata}; +static OSQPData data = {6, 5, &Pdata, &Adata, qdata, ldata, udata}; // Define settings structure -OSQPSettings settings = {(c_float)0.10000000000000000555, (c_float)0.00000100000000000000, 10, 1, 0, (c_float)5.00000000000000000000, 4000, (c_float)0.00100000000000000002, (c_float)0.00100000000000000002, (c_float)0.00010000000000000000, (c_float)0.00010000000000000000, (c_float)1.00000000000000000000, (enum linsys_solver_type) LINSYS_SOLVER, 0, 25, 1, }; - +static OSQPSettings settings = {(c_float)0.10000000000000000555, (c_float)0.00000100000000000000, 10, 1, 0, (c_float)5.00000000000000000000, 4000, (c_float)0.00100000000000000002, (c_float)0.00100000000000000002, (c_float)0.00010000000000000000, (c_float)0.00010000000000000000, (c_float)1.00000000000000000000, (enum linsys_solver_type) LINSYS_SOLVER, 0, 25, 1, }; + // Define scaling structure -c_float Dscaling[6] = { +static c_float Dscaling[6] = { (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, @@ -108,7 +108,7 @@ c_float Dscaling[6] = { (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, }; -c_float Dinvscaling[6] = { +static c_float Dinvscaling[6] = { (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, @@ -116,24 +116,24 @@ c_float Dinvscaling[6] = { (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, }; -c_float Escaling[5] = { +static c_float Escaling[5] = { (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, (c_float)9.07766933431181222147, (c_float)9.07766933431181222147, }; -c_float Einvscaling[5] = { +static c_float Einvscaling[5] = { (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, (c_float)1.00000000000000000000, (c_float)0.11016043470763975387, (c_float)0.11016043470763975387, }; -OSQPScaling scaling = {(c_float)1.00000000000000000000, Dscaling, Escaling, (c_float)1.00000000000000000000, Dinvscaling, Einvscaling}; +static OSQPScaling scaling = {(c_float)1.00000000000000000000, Dscaling, Escaling, (c_float)1.00000000000000000000, Dinvscaling, Einvscaling}; // Define linsys_solver structure -c_int linsys_solver_L_i[19] = { +static c_int linsys_solver_L_i[19] = { 1, 2, 2, @@ -154,7 +154,7 @@ c_int linsys_solver_L_i[19] = { 10, 10, }; -c_int linsys_solver_L_p[12] = { +static c_int linsys_solver_L_p[12] = { 0, 2, 4, @@ -168,7 +168,7 @@ c_int linsys_solver_L_p[12] = { 19, 19, }; -c_float linsys_solver_L_x[19] = { +static c_float linsys_solver_L_x[19] = { (c_float)-100.00000000000000000000, (c_float)-100.00000000000000000000, (c_float)0.99009900009802975784, @@ -189,8 +189,8 @@ c_float linsys_solver_L_x[19] = { (c_float)0.99999900000100006014, (c_float)-0.01730430965950979436, }; -csc linsys_solver_L = {19, 11, 11, linsys_solver_L_p, linsys_solver_L_i, linsys_solver_L_x, -1}; -c_float linsys_solver_Dinv[11] = { +static csc linsys_solver_L = {19, 11, 11, linsys_solver_L_p, linsys_solver_L_i, linsys_solver_L_x, -1}; +static c_float linsys_solver_Dinv[11] = { (c_float)-100.00000000000000000000, (c_float)0.00990099000098029758, (c_float)0.50248706217718097644, @@ -203,7 +203,7 @@ c_float linsys_solver_Dinv[11] = { (c_float)-0.09451674820102912156, (c_float)-0.49902472692344140848, }; -c_int linsys_solver_P[11] = { +static c_int linsys_solver_P[11] = { 8, 5, 2, @@ -216,16 +216,16 @@ c_int linsys_solver_P[11] = { 10, 6, }; -c_float linsys_solver_bp[11]; -c_float linsys_solver_sol[11]; -c_float linsys_solver_rho_inv_vec[5] = { +static c_float linsys_solver_bp[11]; +static c_float linsys_solver_sol[11]; +static c_float linsys_solver_rho_inv_vec[5] = { (c_float)0.01000000000000000021, (c_float)0.01000000000000000021, (c_float)0.01000000000000000021, (c_float)10.00000000000000000000, (c_float)10.00000000000000000000, }; -c_int linsys_solver_Pdiag_idx[6] = { +static c_int linsys_solver_Pdiag_idx[6] = { 0, 1, 2, @@ -233,7 +233,7 @@ c_int linsys_solver_Pdiag_idx[6] = { 4, 5, }; -c_int linsys_solver_KKT_i[23] = { +static c_int linsys_solver_KKT_i[23] = { 0, 1, 0, @@ -258,7 +258,7 @@ c_int linsys_solver_KKT_i[23] = { 8, 10, }; -c_int linsys_solver_KKT_p[12] = { +static c_int linsys_solver_KKT_p[12] = { 0, 1, 3, @@ -272,7 +272,7 @@ c_int linsys_solver_KKT_p[12] = { 20, 23, }; -c_float linsys_solver_KKT_x[23] = { +static c_float linsys_solver_KKT_x[23] = { (c_float)-0.01000000000000000021, (c_float)1.00000099999999991773, (c_float)1.00000000000000000000, @@ -297,8 +297,8 @@ c_float linsys_solver_KKT_x[23] = { (c_float)1.00000000000000000000, (c_float)-0.01000000000000000021, }; -csc linsys_solver_KKT = {23, 11, 11, linsys_solver_KKT_p, linsys_solver_KKT_i, linsys_solver_KKT_x, -1}; -c_int linsys_solver_PtoKKT[6] = { +static csc linsys_solver_KKT = {23, 11, 11, linsys_solver_KKT_p, linsys_solver_KKT_i, linsys_solver_KKT_x, -1}; +static c_int linsys_solver_PtoKKT[6] = { 13, 8, 3, @@ -306,7 +306,7 @@ c_int linsys_solver_PtoKKT[6] = { 6, 1, }; -c_int linsys_solver_AtoKKT[12] = { +static c_int linsys_solver_AtoKKT[12] = { 20, 14, 9, @@ -320,14 +320,14 @@ c_int linsys_solver_AtoKKT[12] = { 2, 18, }; -c_int linsys_solver_rhotoKKT[5] = { +static c_int linsys_solver_rhotoKKT[5] = { 22, 5, 0, 12, 19, }; -QDLDL_float linsys_solver_D[11] = { +static QDLDL_float linsys_solver_D[11] = { 0, 101, 1, @@ -340,7 +340,7 @@ QDLDL_float linsys_solver_D[11] = { -10, -2, }; -QDLDL_int linsys_solver_etree[11] = { +static QDLDL_int linsys_solver_etree[11] = { 1, 2, 6, @@ -353,7 +353,7 @@ QDLDL_int linsys_solver_etree[11] = { 10, -1, }; -QDLDL_int linsys_solver_Lnz[11] = { +static QDLDL_int linsys_solver_Lnz[11] = { 2, 2, 2, @@ -366,59 +366,59 @@ QDLDL_int linsys_solver_Lnz[11] = { 1, 0, }; -QDLDL_int linsys_solver_iwork[33]; -QDLDL_bool linsys_solver_bwork[11]; -QDLDL_float linsys_solver_fwork[11]; -qdldl_solver linsys_solver = {QDLDL_SOLVER, &solve_linsys_qdldl, &update_linsys_solver_matrices_qdldl, &update_linsys_solver_rho_vec_qdldl, &linsys_solver_L, linsys_solver_Dinv, linsys_solver_P, linsys_solver_bp, linsys_solver_sol, linsys_solver_rho_inv_vec, (c_float)0.00000100000000000000, 6, 5, linsys_solver_Pdiag_idx, 6, &linsys_solver_KKT, linsys_solver_PtoKKT, linsys_solver_AtoKKT, linsys_solver_rhotoKKT, linsys_solver_D, linsys_solver_etree, linsys_solver_Lnz, linsys_solver_iwork, linsys_solver_bwork, linsys_solver_fwork, }; +static QDLDL_int linsys_solver_iwork[33]; +static QDLDL_bool linsys_solver_bwork[11]; +static QDLDL_float linsys_solver_fwork[11]; +static qdldl_solver linsys_solver = {QDLDL_SOLVER, &solve_linsys_qdldl, &update_linsys_solver_matrices_qdldl, &update_linsys_solver_rho_vec_qdldl, &linsys_solver_L, linsys_solver_Dinv, linsys_solver_P, linsys_solver_bp, linsys_solver_sol, linsys_solver_rho_inv_vec, (c_float)0.00000100000000000000, 6, 5, linsys_solver_Pdiag_idx, 6, &linsys_solver_KKT, linsys_solver_PtoKKT, linsys_solver_AtoKKT, linsys_solver_rhotoKKT, linsys_solver_D, linsys_solver_etree, linsys_solver_Lnz, linsys_solver_iwork, linsys_solver_bwork, linsys_solver_fwork, }; // Define solution -c_float xsolution[6]; -c_float ysolution[5]; +static c_float xsolution[6]; +static c_float ysolution[5]; -OSQPSolution solution = {xsolution, ysolution}; +static OSQPSolution solution = {xsolution, ysolution}; // Define info -OSQPInfo info = {0, "Unsolved", OSQP_UNSOLVED, 0.0, 0.0, 0.0}; +static OSQPInfo info = {0, "Unsolved", OSQP_UNSOLVED, 0.0, 0.0, 0.0}; // Define workspace -c_float work_rho_vec[5] = { +static c_float work_rho_vec[5] = { (c_float)100.00000000000000000000, (c_float)100.00000000000000000000, (c_float)100.00000000000000000000, (c_float)0.10000000000000000555, (c_float)0.10000000000000000555, }; -c_float work_rho_inv_vec[5] = { +static c_float work_rho_inv_vec[5] = { (c_float)0.01000000000000000021, (c_float)0.01000000000000000021, (c_float)0.01000000000000000021, (c_float)10.00000000000000000000, (c_float)10.00000000000000000000, }; -c_int work_constr_type[5] = { +static c_int work_constr_type[5] = { 1, 1, 1, 0, 0, }; -c_float work_x[6]; -c_float work_y[5]; -c_float work_z[5]; -c_float work_xz_tilde[11]; -c_float work_x_prev[6]; -c_float work_z_prev[5]; -c_float work_Ax[5]; -c_float work_Px[6]; -c_float work_Aty[6]; -c_float work_delta_y[5]; -c_float work_Atdelta_y[6]; -c_float work_delta_x[6]; -c_float work_Pdelta_x[6]; -c_float work_Adelta_x[5]; -c_float work_D_temp[6]; -c_float work_D_temp_A[6]; -c_float work_E_temp[5]; +static c_float work_x[6]; +static c_float work_y[5]; +static c_float work_z[5]; +static c_float work_xz_tilde[11]; +static c_float work_x_prev[6]; +static c_float work_z_prev[5]; +static c_float work_Ax[5]; +static c_float work_Px[6]; +static c_float work_Aty[6]; +static c_float work_delta_y[5]; +static c_float work_Atdelta_y[6]; +static c_float work_delta_x[6]; +static c_float work_Pdelta_x[6]; +static c_float work_Adelta_x[5]; +static c_float work_D_temp[6]; +static c_float work_D_temp_A[6]; +static c_float work_E_temp[5]; OSQPWorkspace workspace_2uav_2hp = { &data, (LinSysSolver *)&linsys_solver, diff --git a/src/lib/osqp/src/osqp/workspace_3uav_2hp_rig.c b/src/lib/osqp/src/osqp/workspace_3uav_2hp_rig.c new file mode 100644 index 0000000000..6457a9e2b0 --- /dev/null +++ b/src/lib/osqp/src/osqp/workspace_3uav_2hp_rig.c @@ -0,0 +1,903 @@ +/* + * This file was autogenerated by OSQP-Python on December 13, 2022 at 17:11:30. + * + * This file contains the workspace variables needed by OSQP. + */ + +#include "types.h" +#include "qdldl_interface.h" + +// Define data structure +static c_int Pdata_i[9] = { +0, +1, +2, +3, +4, +5, +6, +7, +8, +}; +static c_int Pdata_p[10] = { +0, +1, +2, +3, +4, +5, +6, +7, +8, +9, +}; +static c_float Pdata_x[9] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +}; +static csc Pdata = {9, 9, 9, Pdata_p, Pdata_i, Pdata_x, -1}; +static c_int Adata_i[45] = { +0, +4, +5, +6, +7, +1, +3, +5, +6, +7, +2, +3, +4, +6, +7, +0, +4, +5, +8, +9, +1, +3, +5, +8, +9, +2, +3, +4, +8, +9, +0, +4, +5, +10, +11, +1, +3, +5, +10, +11, +2, +3, +4, +10, +11, +}; +static c_int Adata_p[10] = { +0, +5, +10, +15, +20, +25, +30, +35, +40, +45, +}; +static c_float Adata_x[45] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)-0.86315699236625065272, +(c_float)-0.67165039371486134900, +(c_float)0.67165039371486134900, +(c_float)1.00000000000000000000, +(c_float)-1.00000000000000000000, +(c_float)-0.99688554047933186641, +(c_float)-0.99857967253689183806, +(c_float)-0.99857967253689183806, +(c_float)1.00000000000000000000, +(c_float)0.03549999999999999684, +(c_float)0.04100000000000000172, +(c_float)0.28531810247355715582, +(c_float)0.28531810247355715582, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)0.86315699236625065272, +(c_float)0.48872035724121370981, +(c_float)0.99875938391451224430, +(c_float)1.00000000000000000000, +(c_float)-1.00000000000000000000, +(c_float)-0.99688554047933186641, +(c_float)0.99865696633594747134, +(c_float)-0.06851479255760506426, +(c_float)1.00000000000000000000, +(c_float)-0.03549999999999999684, +(c_float)0.04100000000000000172, +(c_float)0.26360255619471800737, +(c_float)0.23734224931382066903, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)0.86315699236625065272, +(c_float)-0.48872035724121370981, +(c_float)-0.99875938391451224430, +(c_float)1.00000000000000000000, +(c_float)-1.00000000000000000000, +(c_float)0.99688554047933186641, +(c_float)0.99865696633594747134, +(c_float)-0.06851479255760506426, +(c_float)1.00000000000000000000, +(c_float)-0.03549999999999999684, +(c_float)-0.04100000000000000172, +(c_float)0.26360255619471800737, +(c_float)0.23734224931382066903, +}; +static csc Adata = {45, 12, 9, Adata_p, Adata_i, Adata_x, -1}; +static c_float qdata[9] = { +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +}; +static c_float ldata[12] = { +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.09810000000000000664, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)-4280320274162503911916204720128.00000000000000000000, +(c_float)-4280320274162503911916204720128.00000000000000000000, +(c_float)-3954471159789210055637569896448.00000000000000000000, +(c_float)-3560632406275809218105662504960.00000000000000000000, +(c_float)-3954471159789210055637569896448.00000000000000000000, +(c_float)-3560632406275809218105662504960.00000000000000000000, +}; +static c_float udata[12] = { +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.09810000000000000664, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +(c_float)0.00000000000000000000, +}; +static OSQPData data = {9, 12, &Pdata, &Adata, qdata, ldata, udata}; + +// Define settings structure +static OSQPSettings settings = {(c_float)0.10000000000000000555, (c_float)0.00000100000000000000, 10, 1, 0, (c_float)5.00000000000000000000, 4000, (c_float)0.00100000000000000002, (c_float)0.00100000000000000002, (c_float)0.00010000000000000000, (c_float)0.00010000000000000000, (c_float)1.00000000000000000000, (enum linsys_solver_type) LINSYS_SOLVER, 0, 25, 1, }; + +// Define scaling structure +static c_float Dscaling[9] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +}; +static c_float Dinvscaling[9] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +}; +static c_float Escaling[12] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)24.31428147510565551670, +(c_float)4.28032027416250393514, +(c_float)4.28032027416250393514, +(c_float)3.95447115978920971102, +(c_float)3.56063240627580901432, +(c_float)3.95447115978920971102, +(c_float)3.56063240627580901432, +}; +static c_float Einvscaling[12] = { +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)1.00000000000000000000, +(c_float)0.04112809177700179480, +(c_float)0.23362737738022698597, +(c_float)0.23362737738022698597, +(c_float)0.25287831408872996075, +(c_float)0.28084898576933842174, +(c_float)0.25287831408872996075, +(c_float)0.28084898576933842174, +}; +static OSQPScaling scaling = {(c_float)1.00000000000000000000, Dscaling, Escaling, (c_float)1.00000000000000000000, Dinvscaling, Einvscaling}; + +// Define linsys_solver structure +static c_int linsys_solver_L_i[81] = { +16, +17, +18, +14, +15, +16, +14, +15, +16, +6, +12, +15, +6, +11, +17, +6, +11, +17, +11, +12, +13, +15, +17, +19, +10, +11, +14, +10, +12, +18, +10, +12, +18, +11, +12, +13, +14, +18, +20, +12, +13, +14, +15, +17, +18, +19, +20, +13, +14, +15, +17, +18, +19, +20, +14, +15, +17, +18, +19, +20, +15, +16, +17, +18, +19, +20, +16, +17, +18, +19, +20, +17, +18, +19, +20, +18, +19, +20, +19, +20, +20, +}; +static c_int linsys_solver_L_p[22] = { +0, +3, +6, +9, +12, +15, +18, +24, +27, +30, +33, +39, +47, +54, +60, +66, +71, +75, +78, +80, +81, +81, +}; +static c_float linsys_solver_L_x[81] = { +(c_float)-100.00000000000000000000, +(c_float)-100.00000000000000000000, +(c_float)-100.00000000000000000000, +(c_float)-0.06716503937148614323, +(c_float)0.09985796725368918658, +(c_float)-0.02853181024735571697, +(c_float)0.06716503937148614323, +(c_float)0.09985796725368918658, +(c_float)-0.02853181024735571697, +(c_float)-100.00000000000000000000, +(c_float)-100.00000000000000000000, +(c_float)-100.00000000000000000000, +(c_float)0.00685147925576050712, +(c_float)-0.09987593839145122998, +(c_float)-0.02373422493138206690, +(c_float)-0.09986569663359474991, +(c_float)-0.04887203572412137514, +(c_float)-0.02636025561947180282, +(c_float)0.00041506761513669731, +(c_float)0.98911770719007396924, +(c_float)-0.00986037140129854446, +(c_float)0.98911770719007396924, +(c_float)0.00024429928845018438, +(c_float)-0.00989117707190073983, +(c_float)-100.00000000000000000000, +(c_float)-100.00000000000000000000, +(c_float)-100.00000000000000000000, +(c_float)0.09987593839145122998, +(c_float)0.00685147925576050712, +(c_float)-0.02373422493138206690, +(c_float)0.04887203572412137514, +(c_float)-0.09986569663359474991, +(c_float)-0.02636025561947180282, +(c_float)0.98888847539490809524, +(c_float)-0.00041497142163632685, +(c_float)0.00853566002207515988, +(c_float)0.98888847539490809524, +(c_float)-0.00036181029691249162, +(c_float)0.00988888475394908081, +(c_float)-0.00000430439725147143, +(c_float)0.00447685998234533412, +(c_float)0.49721047457085043586, +(c_float)-0.01857314574154261475, +(c_float)0.01636735566636506739, +(c_float)0.01619002574588571455, +(c_float)0.00018573145741542613, +(c_float)0.00497210474570851206, +(c_float)0.90626409271444197113, +(c_float)0.01896438585028150520, +(c_float)0.49726841797122262046, +(c_float)-0.01116323185099337909, +(c_float)0.01127925604239783490, +(c_float)-0.00497268417971220757, +(c_float)0.00018964385850281506, +(c_float)0.96420666965012713501, +(c_float)0.54635291043567513203, +(c_float)-0.01217767628870684155, +(c_float)0.01217767628870683981, +(c_float)0.00000000000000002662, +(c_float)0.00491145003297285918, +(c_float)0.28740259700613973726, +(c_float)0.00000000000000000000, +(c_float)-0.01170525096795015270, +(c_float)0.01170525096795018566, +(c_float)0.00000000000000001391, +(c_float)0.00425171422397459687, +(c_float)-0.02829114626119421869, +(c_float)-0.00620164689803155179, +(c_float)-0.00531927005721477986, +(c_float)-0.00271239379727942652, +(c_float)0.00040185225114232559, +(c_float)0.98995172121417274180, +(c_float)0.98995221896466079059, +(c_float)0.00034990403634478683, +(c_float)0.00040610832566050054, +(c_float)0.49832568008213612432, +(c_float)-0.03491396906711961151, +(c_float)0.00013607495779250346, +(c_float)-0.02330003629118962416, +(c_float)-0.05384987721800952448, +(c_float)0.08189683374639393498, +}; +static csc linsys_solver_L = {81, 21, 21, linsys_solver_L_p, linsys_solver_L_i, linsys_solver_L_x, -1}; +static c_float linsys_solver_Dinv[21] = { +(c_float)-100.00000000000000000000, +(c_float)-0.10000000000000000555, +(c_float)-0.10000000000000000555, +(c_float)-100.00000000000000000000, +(c_float)-0.10000000000000000555, +(c_float)-0.10000000000000000555, +(c_float)0.00989117707190073983, +(c_float)-100.00000000000000000000, +(c_float)-0.10000000000000000555, +(c_float)-0.10000000000000000555, +(c_float)0.00988888475394908081, +(c_float)0.44747277465686602360, +(c_float)0.45695195699697782610, +(c_float)-0.54805982056171986816, +(c_float)0.29900280335042545010, +(c_float)0.49648770725256646807, +(c_float)0.00989955219575222069, +(c_float)0.49587974982888599618, +(c_float)0.65973714328137689211, +(c_float)-43.00395987504521144729, +(c_float)-41.29311243624459137891, +}; +static c_int linsys_solver_P[21] = { +11, +16, +15, +10, +18, +17, +4, +9, +20, +19, +6, +3, +7, +14, +0, +1, +2, +5, +8, +12, +13, +}; +static c_float linsys_solver_bp[21]; +static c_float linsys_solver_sol[21]; +static c_float linsys_solver_rho_inv_vec[12] = { +(c_float)0.01000000000000000021, +(c_float)0.01000000000000000021, +(c_float)0.01000000000000000021, +(c_float)0.01000000000000000021, +(c_float)0.01000000000000000021, +(c_float)0.01000000000000000021, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +}; +static c_int linsys_solver_Pdiag_idx[9] = { +0, +1, +2, +3, +4, +5, +6, +7, +8, +}; +static c_int linsys_solver_KKT_i[66] = { +0, +1, +2, +3, +4, +5, +6, +3, +5, +4, +7, +8, +9, +10, +7, +9, +8, +11, +7, +5, +4, +12, +3, +9, +8, +11, +6, +10, +12, +13, +14, +7, +13, +2, +1, +15, +3, +13, +2, +1, +16, +0, +2, +1, +17, +0, +5, +4, +18, +0, +9, +8, +15, +16, +6, +17, +12, +18, +19, +14, +16, +11, +17, +10, +18, +20, +}; +static c_int linsys_solver_KKT_p[22] = { +0, +1, +2, +3, +4, +5, +6, +10, +11, +12, +13, +17, +21, +25, +30, +35, +40, +44, +48, +52, +59, +66, +}; +static c_float linsys_solver_KKT_x[66] = { +(c_float)-0.01000000000000000021, +(c_float)-10.00000000000000000000, +(c_float)-10.00000000000000000000, +(c_float)-0.01000000000000000021, +(c_float)-10.00000000000000000000, +(c_float)-10.00000000000000000000, +(c_float)1.00000099999999991773, +(c_float)1.00000000000000000000, +(c_float)0.99865696633594747134, +(c_float)-0.06851479255760506426, +(c_float)-0.01000000000000000021, +(c_float)-10.00000000000000000000, +(c_float)-10.00000000000000000000, +(c_float)1.00000099999999991773, +(c_float)1.00000000000000000000, +(c_float)-0.48872035724121370981, +(c_float)-0.99875938391451224430, +(c_float)1.00000099999999991773, +(c_float)1.00000000000000000000, +(c_float)0.48872035724121370981, +(c_float)0.99875938391451224430, +(c_float)1.00000099999999991773, +(c_float)1.00000000000000000000, +(c_float)0.99865696633594747134, +(c_float)-0.06851479255760506426, +(c_float)0.86315699236625065272, +(c_float)-0.99688554047933186641, +(c_float)0.86315699236625065272, +(c_float)0.99688554047933186641, +(c_float)-0.01000000000000000021, +(c_float)1.00000099999999991773, +(c_float)1.00000000000000000000, +(c_float)-0.86315699236625065272, +(c_float)-0.67165039371486134900, +(c_float)0.67165039371486134900, +(c_float)1.00000099999999991773, +(c_float)1.00000000000000000000, +(c_float)-0.99688554047933186641, +(c_float)-0.99857967253689183806, +(c_float)-0.99857967253689183806, +(c_float)1.00000099999999991773, +(c_float)1.00000000000000000000, +(c_float)0.28531810247355715582, +(c_float)0.28531810247355715582, +(c_float)1.00000099999999991773, +(c_float)1.00000000000000000000, +(c_float)0.26360255619471800737, +(c_float)0.23734224931382066903, +(c_float)1.00000099999999991773, +(c_float)1.00000000000000000000, +(c_float)0.26360255619471800737, +(c_float)0.23734224931382066903, +(c_float)-1.00000000000000000000, +(c_float)0.03549999999999999684, +(c_float)-1.00000000000000000000, +(c_float)-0.03549999999999999684, +(c_float)-1.00000000000000000000, +(c_float)-0.03549999999999999684, +(c_float)-0.01000000000000000021, +(c_float)1.00000000000000000000, +(c_float)0.04100000000000000172, +(c_float)1.00000000000000000000, +(c_float)0.04100000000000000172, +(c_float)1.00000000000000000000, +(c_float)-0.04100000000000000172, +(c_float)-0.01000000000000000021, +}; +static csc linsys_solver_KKT = {66, 21, 21, linsys_solver_KKT_p, linsys_solver_KKT_i, linsys_solver_KKT_x, -1}; +static c_int linsys_solver_PtoKKT[9] = { +30, +35, +40, +17, +6, +44, +13, +21, +48, +}; +static c_int linsys_solver_AtoKKT[45] = { +31, +59, +32, +33, +34, +36, +52, +37, +38, +39, +41, +53, +60, +42, +43, +18, +61, +25, +19, +20, +7, +54, +26, +8, +9, +45, +55, +62, +46, +47, +14, +63, +27, +15, +16, +22, +56, +28, +23, +24, +49, +57, +64, +50, +51, +}; +static c_int linsys_solver_rhotoKKT[12] = { +10, +3, +0, +58, +65, +29, +2, +1, +5, +4, +12, +11, +}; +static QDLDL_float linsys_solver_D[21] = { +0, +-10, +-10, +0, +-10, +-10, +101, +0, +-10, +-10, +101, +2, +2, +-1, +3, +2, +101, +2, +1, +0, +0, +}; +static QDLDL_int linsys_solver_etree[21] = { +16, +14, +14, +6, +6, +6, +11, +10, +10, +10, +11, +12, +13, +14, +15, +16, +17, +18, +19, +20, +-1, +}; +static QDLDL_int linsys_solver_Lnz[21] = { +3, +3, +3, +3, +3, +3, +6, +3, +3, +3, +6, +8, +7, +6, +6, +5, +4, +3, +2, +1, +0, +}; +static QDLDL_int linsys_solver_iwork[63]; +static QDLDL_bool linsys_solver_bwork[21]; +static QDLDL_float linsys_solver_fwork[21]; +static qdldl_solver linsys_solver = {QDLDL_SOLVER, &solve_linsys_qdldl, &update_linsys_solver_matrices_qdldl, &update_linsys_solver_rho_vec_qdldl, &linsys_solver_L, linsys_solver_Dinv, linsys_solver_P, linsys_solver_bp, linsys_solver_sol, linsys_solver_rho_inv_vec, (c_float)0.00000100000000000000, 9, 12, linsys_solver_Pdiag_idx, 9, &linsys_solver_KKT, linsys_solver_PtoKKT, linsys_solver_AtoKKT, linsys_solver_rhotoKKT, linsys_solver_D, linsys_solver_etree, linsys_solver_Lnz, linsys_solver_iwork, linsys_solver_bwork, linsys_solver_fwork, }; + +// Define solution +static c_float xsolution[9]; +static c_float ysolution[12]; + +static OSQPSolution solution = {xsolution, ysolution}; + +// Define info +static OSQPInfo info = {0, "Unsolved", OSQP_UNSOLVED, 0.0, 0.0, 0.0}; + +// Define workspace +static c_float work_rho_vec[12] = { +(c_float)100.00000000000000000000, +(c_float)100.00000000000000000000, +(c_float)100.00000000000000000000, +(c_float)100.00000000000000000000, +(c_float)100.00000000000000000000, +(c_float)100.00000000000000000000, +(c_float)0.10000000000000000555, +(c_float)0.10000000000000000555, +(c_float)0.10000000000000000555, +(c_float)0.10000000000000000555, +(c_float)0.10000000000000000555, +(c_float)0.10000000000000000555, +}; +static c_float work_rho_inv_vec[12] = { +(c_float)0.01000000000000000021, +(c_float)0.01000000000000000021, +(c_float)0.01000000000000000021, +(c_float)0.01000000000000000021, +(c_float)0.01000000000000000021, +(c_float)0.01000000000000000021, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +(c_float)10.00000000000000000000, +}; +static c_int work_constr_type[12] = { +1, +1, +1, +1, +1, +1, +0, +0, +0, +0, +0, +0, +}; +static c_float work_x[9]; +static c_float work_y[12]; +static c_float work_z[12]; +static c_float work_xz_tilde[21]; +static c_float work_x_prev[9]; +static c_float work_z_prev[12]; +static c_float work_Ax[12]; +static c_float work_Px[9]; +static c_float work_Aty[9]; +static c_float work_delta_y[12]; +static c_float work_Atdelta_y[9]; +static c_float work_delta_x[9]; +static c_float work_Pdelta_x[9]; +static c_float work_Adelta_x[12]; +static c_float work_D_temp[9]; +static c_float work_D_temp_A[9]; +static c_float work_E_temp[12]; + +OSQPWorkspace workspace_3uav_2hp_rig = { +&data, (LinSysSolver *)&linsys_solver, +work_rho_vec, work_rho_inv_vec, +work_constr_type, +work_x, work_y, work_z, work_xz_tilde, +work_x_prev, work_z_prev, +work_Ax, work_Px, work_Aty, +work_delta_y, work_Atdelta_y, +work_delta_x, work_Pdelta_x, work_Adelta_x, +work_D_temp, work_D_temp_A, work_E_temp, +&settings, &scaling, &solution, &info}; + diff --git a/src/modules/interface/controller_lee_payload.h b/src/modules/interface/controller_lee_payload.h index 7044f21efe..6662154d1b 100644 --- a/src/modules/interface/controller_lee_payload.h +++ b/src/modules/interface/controller_lee_payload.h @@ -34,6 +34,12 @@ typedef struct controllerLeePayload_s { float thrustSI; struct vec J; // Inertia matrix (diagonal matrix); kg m^2 struct vec offset; // offset for reference + + struct { + uint8_t id; + struct vec point; + } attachement_points[3]; + //Position PID struct vec Kpos_P; float Kpos_P_limit; @@ -41,6 +47,12 @@ typedef struct controllerLeePayload_s { float Kpos_D_limit; struct vec Kpos_I; float Kpos_I_limit; + + // Payload attitude control gains + struct vec Kprot_P; + float Kprot_P_limit; + struct vec Kprot_D; + float Kprot_D_limit; struct vec i_error_pos; struct vec i_error_att; @@ -74,9 +86,18 @@ typedef struct controllerLeePayload_s { struct vec u_i; struct vec qidot_prev; struct vec acc_prev; + struct vec F_d; + struct vec M_d; // control moments of the payload + struct quat qp_des; // desired quaternion orientation for the payload + struct vec wp_des; // desired omega for the payload // desired value from the QP struct vec desVirtInp; + struct vec desVirt2_prev; + struct vec desVirt3_prev; + + float lambdaa; // regularization on how close to stay to previous value + struct vec n1; struct vec n2; struct vec n3; diff --git a/src/modules/interface/math3d.h b/src/modules/interface/math3d.h index 438b51aa84..53f3054bbd 100644 --- a/src/modules/interface/math3d.h +++ b/src/modules/interface/math3d.h @@ -492,7 +492,7 @@ static inline struct mat33 vecmult(struct vec a) { m.m[1][1] = a.y*a.y; m.m[2][1] = a.z*a.y; - m.m[0][2] = a.x*a.x; + m.m[0][2] = a.x*a.z; m.m[1][2] = a.y*a.z; m.m[2][2] = a.z*a.z; @@ -952,6 +952,24 @@ static inline struct quat qslerp(struct quat a, struct quat b, float t) } } +// numerically estimate angular velocity from two quaternions +// see e.g., https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/ +static inline struct vec quat2omega(struct quat q0, struct quat q1, float dt) +{ + // omega = vectorpart_of(2 * qdot * q_inv) + struct quat q_dot = mkquat( + (q0.x - q1.x) / dt, + (q0.y - q1.y) / dt, + (q0.z - q1.z) / dt, + (q0.w - q1.w) / dt); + + struct quat q_inv = qinv(q1); + + struct quat r = qqmul(q_dot, q_inv); + struct vec omega = vscl(2, quatimagpart(r)); + return omega; +} + // // conversion to/from raw float and double arrays. // diff --git a/src/modules/interface/peer_localization.h b/src/modules/interface/peer_localization.h index 3804f8eb59..e0f6a5d53c 100644 --- a/src/modules/interface/peer_localization.h +++ b/src/modules/interface/peer_localization.h @@ -19,9 +19,14 @@ void peerLocalizationInit(); bool peerLocalizationTest(); +// TODO: the name of this type and functions are misleading now, +// but kept in this branch to reduce the number of merge +// conflicts typedef struct peerLocalizationOtherPosition_s { - uint8_t id; // CF id - point_t pos; // position and timestamp (millisecs) + uint8_t id; // CF id + uint32_t timestamp; // timestamp when the data was received [ms] + struct vec pos; // position + struct quat orientation; // orientation, if available (nanf's otherwise) } peerLocalizationOtherPosition_t; // Tell the peer localization system the position of another Crazyflie. @@ -29,6 +34,11 @@ typedef struct peerLocalizationOtherPosition_s { // e.g. when a motion capture measurement packet is received. bool peerLocalizationTellPosition(int id, positionMeasurement_t const *pos); +// Tell the peer localization system the pose of another Crazyflie. +// Should be called when the pose is already known with high accuracy, +// e.g. when a motion capture measurement packet is received. +bool peerLocalizationTellPose(int id, poseMeasurement_t const *pose); + // Returns true if we have a position value for the given radio ID. bool peerLocalizationIsIDActive(uint8_t id); diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 8e3cac373e..06674fb3ce 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -170,10 +170,15 @@ typedef struct state_s { acc_t acc; // Gs (but acc.z without considering gravity) // positions of the neighboring UAVs uint8_t num_neighbors; - point_t position_neighbors[MAX_NEIGHBOR_UAVS]; + struct { + uint8_t id; + point_t pos; + } neighbors[MAX_NEIGHBOR_UAVS]; // Measured state of the payload point_t payload_pos; // m (world frame) + quaternion_t payload_quat; // orientation (world frame) velocity_t payload_vel; // m/s (world frame) + Axis3f payload_omega; // rad/s (world frame) } state_t; typedef enum control_mode_e { diff --git a/src/modules/src/collision_avoidance.c b/src/modules/src/collision_avoidance.c index 60c40aa7d9..e91ccc7530 100644 --- a/src/modules/src/collision_avoidance.c +++ b/src/modules/src/collision_avoidance.c @@ -325,7 +325,7 @@ void collisionAvoidanceUpdateSetpoint( continue; } - if (doAgeFilter && (time - otherPos->pos.timestamp > params.maxPeerLocAgeMillis)) { + if (doAgeFilter && (time - otherPos->timestamp > params.maxPeerLocAgeMillis)) { continue; } diff --git a/src/modules/src/controller_lee_payload.c b/src/modules/src/controller_lee_payload.c index 0d4dffe068..7e0c9ad768 100644 --- a/src/modules/src/controller_lee_payload.c +++ b/src/modules/src/controller_lee_payload.c @@ -39,6 +39,7 @@ TODO #include "osqp.h" extern OSQPWorkspace workspace_2uav_2hp; extern OSQPWorkspace workspace_3uav_2hp; +extern OSQPWorkspace workspace_3uav_2hp_rig; #define GRAVITY_MAGNITUDE (9.81f) @@ -46,10 +47,12 @@ extern OSQPWorkspace workspace_3uav_2hp; struct QPInput { struct vec F_d; + struct vec M_d; struct vec plStPos; struct vec statePos; struct vec statePos2; struct vec statePos3; + uint8_t ids[2]; // ids for statePos2, statePos3 controllerLeePayload_t* self; }; @@ -67,9 +70,9 @@ struct QPOutput #include "static_mem.h" -#define CONTROLLER_LEE_PAYLOAD_QP_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE) +#define CONTROLLER_LEE_PAYLOAD_QP_TASK_STACKSIZE (6 * configMINIMAL_STACK_SIZE) #define CONTROLLER_LEE_PAYLOAD_QP_TASK_NAME "LEEQP" -#define CONTROLLER_LEE_PAYLOAD_QP_TASK_PRI 1 +#define CONTROLLER_LEE_PAYLOAD_QP_TASK_PRI 0 STATIC_MEM_TASK_ALLOC(controllerLeePayloadQPTask, CONTROLLER_LEE_PAYLOAD_QP_TASK_STACKSIZE); static void controllerLeePayloadQPTask(void * prm); @@ -80,6 +83,8 @@ STATIC_MEM_QUEUE_ALLOC(queueQPInput, 1, sizeof(struct QPInput)); static QueueHandle_t queueQPOutput; STATIC_MEM_QUEUE_ALLOC(queueQPOutput, 1, sizeof(struct QPOutput)); +static uint32_t qp_runtime_us = 0; + #endif // static inline struct vec computePlaneNormal(struct vec rpy, float yaw) { @@ -188,6 +193,13 @@ static controllerLeePayload_t g_self = { .Kpos_I ={10, 10, 10}, .Kpos_I_limit = 0, + // Payload attitude gains + + .Kprot_P = {0.01, 0.01, 0.01}, + .Kprot_P_limit = 100, + .Kprot_D = {0.005, 0.005, 0.005}, + .Kprot_D_limit = 100, + // Cables PD .K_q = {25, 25, 25}, .K_w = {24, 24, 24}, @@ -196,9 +208,13 @@ static controllerLeePayload_t g_self = { .KR = {0.008, 0.008, 0.01}, .Komega = {0.0013, 0.0013, 0.002}, .KI = {0.02, 0.02, 0.05}, - // -----------------------FOR QP----------------------------// - // 0 for UAV 1 and, 1 for UAV 2 + // desired orientation and omega for payload + .qp_des = {0, 0, 0, 1}, + .wp_des = {0, 0, 0}, + .radius = 0.15, + + .lambdaa = 0.0, }; // static inline struct vec vclampscl(struct vec value, float min, float max) { @@ -211,6 +227,7 @@ static controllerLeePayload_t g_self = { static void runQP(const struct QPInput *input, struct QPOutput* output) { struct vec F_d = input->F_d; + struct vec M_d = input->M_d; struct vec statePos = input->statePos; struct vec plStPos = input->plStPos; struct vec statePos2 = input->statePos2; @@ -220,7 +237,50 @@ static void runQP(const struct QPInput *input, struct QPOutput* output) float l3 = vmag(vsub(plStPos, statePos3)); float radius = input->self->radius; - struct vec desVirtInp; + // att points + struct vec attPoint = input->self->attachement_points[0].point; + struct vec attPoint2 = input->self->attachement_points[1].point; + struct vec attPoint3 = input->self->attachement_points[2].point; + + struct vec desVirt_prev = input->self->desVirtInp; + struct vec desVirt2_prev = input->self->desVirt2_prev; + struct vec desVirt3_prev = input->self->desVirt3_prev; + + // find the corresponding attachement points + for (uint8_t i = 0; i < 3; ++i) { + if (input->self->attachement_points[i].id == input->ids[0]) { + attPoint2 = input->self->attachement_points[i].point; + } else if (input->self->attachement_points[i].id == input->ids[1]) { + attPoint3 = input->self->attachement_points[i].point; + } else { + attPoint = input->self->attachement_points[i].point; + } + } + // printf("\nattPoint: %f %f %f\n", attPoint.x, attPoint.y, attPoint.z); + // printf("StatePos: %f %f %f\n", statePos.x, statePos.y, statePos.z); + // printf("attPoint2: %f %f %f\n", attPoint2.x, attPoint2.y, attPoint2.z); + // printf("statePos2: %f %f %f\n", statePos2.x, statePos2.y, statePos2.z); + // printf("attPoint3: %f %f %f\n", attPoint3.x, attPoint3.y, attPoint3.z); + // printf("statePos3: %f %f %f\n", statePos3.x, statePos3.y, statePos3.z); + // DEBUG_PRINT("s %f %f %f\n", (double)statePos.x, (double)statePos.y, (double)statePos.z); + // DEBUG_PRINT("ap %f %f %f\n", (double)attPoint.x, (double)attPoint.y, (double)attPoint.z); + + // DEBUG_PRINT("s2 %f %f %f\n", (double)statePos2.x, (double)statePos2.y, (double)statePos2.z); + // DEBUG_PRINT("ap2 %f %f %f\n", (double)attPoint2.x, (double)attPoint2.y, (double)attPoint2.z); + + // DEBUG_PRINT("s3 %f %f %f\n", (double)statePos3.x, (double)statePos3.y, (double)statePos3.z); + // DEBUG_PRINT("ap3 %f %f %f\n", (double)attPoint3.x, (double)attPoint3.y, (double)attPoint3.z); + + + + // printf("\n%f %f %f\n", attPoint.x, attPoint.y, attPoint.z); + // printf("%f %f %f\n", attPoint2.x, attPoint2.y, attPoint2.z); + // printf("%f %f %f\n", attPoint3.x, attPoint3.y, attPoint3.z); + // printf("\n%f %f %f\n", statePos.x, statePos.y, statePos.z); + // printf("%f %f %f\n", statePos2.x, statePos2.y, statePos2.z); + // printf("%f %f %f\n", statePos3.x, statePos3.y, statePos3.z); + + struct vec desVirtInp = input->self->desVirtInp; //------------------------------------------QP------------------------------// // The QP will be added here for the desired virtual input (mu_des) @@ -239,8 +299,44 @@ static void runQP(const struct QPInput *input, struct QPOutput* output) // c_float l_new[6] = {F_d.x, F_d.y, F_d.z, -INFINITY, -INFINITY,}; // c_float u_new[6] = {F_d.x, F_d.y, F_d.z, 0, 0,}; - OSQPWorkspace* workspace = &workspace_3uav_2hp; + // OSQPWorkspace* workspace = &workspace_3uav_2hp; + // workspace->settings->warm_start = 1; + + // struct vec n1 = computePlaneNormal(statePos, statePos2, plStPos, radius, l1, l2); + // struct vec n2 = computePlaneNormal(statePos, statePos3, plStPos, radius, l1, l3); + // struct vec n3 = computePlaneNormal(statePos2, statePos, plStPos, radius, l2, l1); + // struct vec n4 = computePlaneNormal(statePos2, statePos3, plStPos, radius, l2, l3); + // struct vec n5 = computePlaneNormal(statePos3, statePos, plStPos, radius, l3, l1); + // struct vec n6 = computePlaneNormal(statePos3, statePos2, plStPos, radius, l3, l2); + // // printf("%f\n",(double)workspace->data->A->nzmax); + // c_float Ax_new[27] = {1, n1.x, n2.x, 1, n1.y, n2.y, 1, n1.z, n2.z, 1, n3.x, n4.x, 1, n3.y, n4.y, 1, n3.z, n4.z, 1, n5.x, n6.x, 1, n5.y, n6.y, 1, n5.z, n6.z, }; + // // c_int Ax_new_idx[27] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26}; + // c_int Ax_new_n = 27; + // c_float l_new[9] = {F_d.x, F_d.y, F_d.z, -INFINITY, -INFINITY, -INFINITY, -INFINITY, -INFINITY, -INFINITY,}; + // c_float u_new[9] = {F_d.x, F_d.y, F_d.z, 0, 0, 0, 0, 0, 0}; + + OSQPWorkspace* workspace = &workspace_3uav_2hp_rig; workspace->settings->warm_start = 1; + // settings = {'eps_abs': 1.0e-5, 'eps_rel' : 1.0e-05, + // 'eps_prim_inf' : 1.0e-04, 'eps_dual_inf' : 1.0e-04,'rho' : 1.00e-01, + // 'sigma' : 1.00e-06, 'alpha' : 1.60 ,'max_iter': 1000, + // 'verbose': False, 'linsys_solver': 'qdldl', 'check_termination': 25, 'polish': True} + + c_float x_warm[9] = { desVirt_prev.x, desVirt_prev.y, desVirt_prev.z, + desVirt2_prev.x, desVirt2_prev.y, desVirt2_prev.z, + desVirt3_prev.x, desVirt3_prev.y, desVirt3_prev.z}; + osqp_warm_start_x(workspace, x_warm); + + // workspace->settings->eps_abs = 0.00001f; + // workspace->settings->eps_rel = 0.00001f; + // workspace->settings->eps_prim_inf = 0.0001f; + // workspace->settings->eps_dual_inf = 0.0001f; + // workspace->settings->rho = 0.1f; + // workspace->settings->sigma = 0.000001f; + // workspace->settings->alpha = 1.6f; + // workspace->settings->max_iter = 1000; + // workspace->settings->check_termination = 25; + struct vec n1 = computePlaneNormal(statePos, statePos2, plStPos, radius, l1, l2); struct vec n2 = computePlaneNormal(statePos, statePos3, plStPos, radius, l1, l3); @@ -248,26 +344,71 @@ static void runQP(const struct QPInput *input, struct QPOutput* output) struct vec n4 = computePlaneNormal(statePos2, statePos3, plStPos, radius, l2, l3); struct vec n5 = computePlaneNormal(statePos3, statePos, plStPos, radius, l3, l1); struct vec n6 = computePlaneNormal(statePos3, statePos2, plStPos, radius, l3, l2); - // printf("%f\n",(double)workspace->data->A->nzmax); - c_float Ax_new[27] = {1, n1.x, n2.x, 1, n1.y, n2.y, 1, n1.z, n2.z, 1, n3.x, n4.x, 1, n3.y, n4.y, 1, n3.z, n4.z, 1, n5.x, n6.x, 1, n5.y, n6.y, 1, n5.z, n6.z, }; - // c_int Ax_new_idx[27] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26}; - c_int Ax_new_n = 27; - c_float l_new[9] = {F_d.x, F_d.y, F_d.z, -INFINITY, -INFINITY, -INFINITY, -INFINITY, -INFINITY, -INFINITY,}; - c_float u_new[9] = {F_d.x, F_d.y, F_d.z, 0, 0, 0, 0, 0, 0}; + c_float Ax_new[45] = { + 1, attPoint.z, -attPoint.y, n1.x, n2.x, 1, -attPoint.z, attPoint.x, n1.y, n2.y, 1, attPoint.y, -attPoint.x, n1.z, n2.z, + 1, attPoint2.z, -attPoint2.y, n3.x, n4.x, 1, -attPoint2.z, attPoint2.x, n3.y, n4.y, 1, attPoint2.y, -attPoint2.x, n3.z, n4.z, + 1, attPoint3.z, -attPoint3.y, n5.x, n6.x, 1, -attPoint3.z, attPoint3.x, n5.y, n6.y, 1, attPoint3.y, -attPoint3.x, n5.z, n6.z, + }; + c_int Ax_new_n = 45; + c_float l_new[12] = {F_d.x, F_d.y, F_d.z, M_d.x, M_d.y, M_d.z, -INFINITY, -INFINITY, -INFINITY, -INFINITY, -INFINITY, -INFINITY,}; + c_float u_new[12] = {F_d.x, F_d.y, F_d.z, M_d.x, M_d.y, M_d.z, 0, 0, 0, 0, 0, 0}; + + // P = np.eye(9) [can't be changed online] + // x^2 + lambda (x^2 - 2xx_d + x_d^2) + // => J = (1+lambda) x^2 - 2 * lambda x_d x + // => = x^2 - 2 * lambda / (1+lambda) x_d x + // => q = -2 * lambda / (1+lambda) * x_d + + const float factor = - 2.0f * input->self->lambdaa / (1.0f + input->self->lambdaa); + + c_float q_new[9] = {factor * desVirt_prev.x, factor * desVirt_prev.y, factor * desVirt_prev.z, + factor * desVirt2_prev.x, factor * desVirt2_prev.y, factor * desVirt2_prev.z, + factor * desVirt3_prev.x, factor * desVirt3_prev.y, factor * desVirt3_prev.z}; osqp_update_A(workspace, Ax_new, OSQP_NULL, Ax_new_n); + + // c_int j, i, row_start, row_stop; + // c_int k = 0; + + // for (j = 0; j < workspace->data->A->n; j++) { + // row_start = workspace->data->A->p[j]; + // row_stop = workspace->data->A->p[j + 1]; + + // if (row_start == row_stop) continue; + // else { + // for (i = row_start; i < row_stop; i++) { + // printf("\t[%3u,%3u] = %.3g\n", (int)workspace->data->A->i[i], (int)j, workspace->data->A->x[k++]); + // } + // } + // } + + + osqp_update_lin_cost(workspace, q_new); // osqp_update_P(workspace, Px_new, Px_new_idx, Px_new_n); osqp_update_lower_bound(workspace, l_new); osqp_update_upper_bound(workspace, u_new); osqp_solve(workspace); - + + + // printf("\nmu_des= %f %f %f %f %f %f %f %f %f\n", + // (workspace)->solution->x[0], (workspace)->solution->x[1], (workspace)->solution->x[2], + // (workspace)->solution->x[3], (workspace)->solution->x[4], (workspace)->solution->x[5], + // (workspace)->solution->x[6], (workspace)->solution->x[7], (workspace)->solution->x[8]); if (workspace->info->status_val == OSQP_SOLVED) { desVirtInp.x = (workspace)->solution->x[0]; desVirtInp.y = (workspace)->solution->x[1]; desVirtInp.z = (workspace)->solution->x[2]; + + // input->self->desVirtInp = desVirtInp; + input->self->desVirt2_prev.x = (workspace)->solution->x[3]; + input->self->desVirt2_prev.y = (workspace)->solution->x[4]; + input->self->desVirt2_prev.z = (workspace)->solution->x[5]; + input->self->desVirt3_prev.x = (workspace)->solution->x[6]; + input->self->desVirt3_prev.y = (workspace)->solution->x[7]; + input->self->desVirt3_prev.z = (workspace)->solution->x[8]; } else { #ifdef CRAZYFLIE_FW DEBUG_PRINT("QP: %s\n", workspace->info->status); @@ -307,17 +448,20 @@ static void runQP(const struct QPInput *input, struct QPOutput* output) } #ifdef CRAZYFLIE_FW -static struct vec computeDesiredVirtualInput(controllerLeePayload_t* self, const state_t *state, struct vec F_d) +static struct vec computeDesiredVirtualInput(controllerLeePayload_t* self, const state_t *state, struct vec F_d, struct vec M_d) { struct QPInput qpinput; struct QPOutput qpoutput; // push the latest change to the QP qpinput.F_d = F_d; + qpinput.M_d = M_d; qpinput.plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); qpinput.statePos = mkvec(state->position.x, state->position.y, state->position.z); - qpinput.statePos2 = mkvec(state->position_neighbors[0].x, state->position_neighbors[0].y, state->position_neighbors[0].z); - qpinput.statePos3 = mkvec(state->position_neighbors[1].x, state->position_neighbors[1].y, state->position_neighbors[1].z); + qpinput.statePos2 = mkvec(state->neighbors[0].pos.x, state->neighbors[0].pos.y, state->neighbors[0].pos.z); + qpinput.statePos3 = mkvec(state->neighbors[1].pos.x, state->neighbors[1].pos.y, state->neighbors[1].pos.z); + qpinput.ids[0] = state->neighbors[0].id; + qpinput.ids[1] = state->neighbors[1].id; qpinput.self = self; xQueueOverwrite(queueQPInput, &qpinput); @@ -336,28 +480,34 @@ void controllerLeePayloadQPTask(void * prm) xQueueReceive(queueQPInput, &qpinput, portMAX_DELAY); // solve the QP + uint64_t start = usecTimestamp(); runQP(&qpinput, &qpoutput); - + uint64_t end = usecTimestamp(); + qp_runtime_us = end - start; // store the result xQueueOverwrite(queueQPOutput, &qpoutput); } } #else -static struct vec computeDesiredVirtualInput(controllerLeePayload_t* self, const state_t *state, struct vec F_d) +static struct vec computeDesiredVirtualInput(controllerLeePayload_t* self, const state_t *state, struct vec F_d, struct vec M_d) { struct QPInput qpinput; struct QPOutput qpoutput; // push the latest change to the QP qpinput.F_d = F_d; + qpinput.M_d = M_d; qpinput.plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); qpinput.statePos = mkvec(state->position.x, state->position.y, state->position.z); - qpinput.statePos2 = mkvec(state->position_neighbors[0].x, state->position_neighbors[0].y, state->position_neighbors[0].z); - qpinput.statePos3 = mkvec(state->position_neighbors[1].x, state->position_neighbors[1].y, state->position_neighbors[1].z); + qpinput.statePos2 = mkvec(state->neighbors[0].pos.x, state->neighbors[0].pos.y, state->neighbors[0].pos.z); + qpinput.statePos3 = mkvec(state->neighbors[1].pos.x, state->neighbors[1].pos.y, state->neighbors[1].pos.z); + qpinput.ids[0] = state->neighbors[0].id; + qpinput.ids[1] = state->neighbors[1].id; qpinput.self = self; // solve the QP runQP(&qpinput, &qpoutput); + return qpoutput.desVirtInp; } @@ -437,6 +587,9 @@ void controllerLeePayload(controllerLeePayload_t* self, control_t *control, setp struct vec plStPos = mkvec(state->payload_pos.x, state->payload_pos.y, state->payload_pos.z); struct vec plStVel = mkvec(state->payload_vel.x, state->payload_vel.y, state->payload_vel.z); + // rotational states of the payload + struct quat plquat = mkquat(state->payload_quat.x, state->payload_quat.y, state->payload_quat.z, state->payload_quat.w); + struct vec plomega = mkvec(state->payload_omega.x, state->payload_omega.y, state->payload_omega.z); float l = vmag(vsub(plStPos, statePos)); // errors @@ -444,16 +597,38 @@ void controllerLeePayload(controllerLeePayload_t* self, control_t *control, setp struct vec plvel_e = vclampnorm(vsub(plVel_d, plStVel), self->Kpos_D_limit); self->i_error_pos = vclampnorm(vadd(self->i_error_pos, vscl(dt, plpos_e)), self->Kpos_I_limit); + // payload orientation errors + // payload quat to R + struct mat33 Rp = quat2rotmat(plquat); + // define desired payload Rp_des = eye(3) (i.e., qp_des = [0,0,0,1] (x,y,z,w)) + self->qp_des = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w); + struct mat33 Rp_des = quat2rotmat(self->qp_des); + // define orientation error + // eRp = msub(mmul(mtranspose(self->R_des), self->R), mmul(mtranspose(self->R), self->R_des)); + struct mat33 eRMp = msub(mmul(mtranspose(Rp_des), Rp), mmul(mtranspose(Rp), Rp_des)); + struct vec eRp = vscl(0.5f, mkvec(eRMp.m[2][1], eRMp.m[0][2], eRMp.m[1][0])); + + self->wp_des = mkvec(setpoint->attitudeRate.roll, setpoint->attitudeRate.pitch, setpoint->attitudeRate.yaw); + struct vec omega_pr = mvmul(mmul(mtranspose(Rp), Rp_des), self->wp_des); + struct vec omega_perror = vsub(plomega, omega_pr); + self->plp_error = plpos_e; self->plv_error = plvel_e; - struct vec F_d =vscl(self->mp ,vadd4( + self->F_d =vscl(self->mp ,vadd4( plAcc_d, veltmul(self->Kpos_P, plpos_e), veltmul(self->Kpos_D, plvel_e), veltmul(self->Kpos_I, self->i_error_pos))); - self->desVirtInp = computeDesiredVirtualInput(self, state, F_d); + self->M_d = vadd( + vneg(veltmul(self->Kprot_P, eRp)), + vneg(veltmul(self->Kprot_D, omega_perror)) + ); + struct vec F_dP = mvmul(mtranspose(Rp),self->F_d); + // computed desired generalized forces in rigid payload case for equation 23 is Pmu_des = [Rp.T@F_d, M_d] + // if a point mass for the payload is considered then: Pmu_des = F_d + self->desVirtInp = computeDesiredVirtualInput(self, state, F_dP, self->M_d); //directional unit vector qi and angular velocity wi pointing from UAV to payload struct vec qi = vnormalize(vsub(plStPos, statePos)); @@ -474,7 +649,7 @@ void controllerLeePayload(controllerLeePayload_t* self, control_t *control, setp struct mat33 skewqi = mcrossmat(qi); struct mat33 skewqi2 = mmul(skewqi,skewqi); - struct vec qdidot = vzero(); //vdiv(vsub(qdi, self->qdi_prev), dt); + struct vec qdidot = vzero(); //vdiv(vsub(qdi, self->qdi_prev), dt); self->qdi_prev = qdi; struct vec wdi = vcross(qdi, qdidot); struct vec ew = vadd(wi, mvmul(skewqi2, wdi)); @@ -664,6 +839,17 @@ PARAM_ADD(PARAM_FLOAT, Kpos_Iy, &g_self.Kpos_I.y) PARAM_ADD(PARAM_FLOAT, Kpos_Iz, &g_self.Kpos_I.z) PARAM_ADD(PARAM_FLOAT, Kpos_I_limit, &g_self.Kpos_I_limit) +// Attitude Payload P +PARAM_ADD(PARAM_FLOAT, Kprot_Px, &g_self.Kprot_P.x) +PARAM_ADD(PARAM_FLOAT, Kprot_Py, &g_self.Kprot_P.y) +PARAM_ADD(PARAM_FLOAT, Kprot_Pz, &g_self.Kprot_P.z) +PARAM_ADD(PARAM_FLOAT, Kprot_P_limit, &g_self.Kprot_P_limit) +// Attitude Payload D +PARAM_ADD(PARAM_FLOAT, Kprot_Dx, &g_self.Kprot_D.x) +PARAM_ADD(PARAM_FLOAT, Kprot_Dy, &g_self.Kprot_D.y) +PARAM_ADD(PARAM_FLOAT, Kprot_Dz, &g_self.Kprot_D.z) +PARAM_ADD(PARAM_FLOAT, Kprot_D_limit, &g_self.Kprot_D_limit) + // Attitude P PARAM_ADD(PARAM_FLOAT, KRx, &g_self.KR.x) PARAM_ADD(PARAM_FLOAT, KRy, &g_self.KR.y) @@ -694,6 +880,27 @@ PARAM_ADD(PARAM_FLOAT, massP, &g_self.mp) PARAM_ADD(PARAM_FLOAT, radius, &g_self.radius) +// QP tuning + +PARAM_ADD(PARAM_FLOAT, lambda, &g_self.lambdaa) + +// Attachement points rigid body payload +PARAM_ADD(PARAM_UINT8, ap0id, &g_self.attachement_points[0].id) +PARAM_ADD(PARAM_FLOAT, ap0x, &g_self.attachement_points[0].point.x) +PARAM_ADD(PARAM_FLOAT, ap0y, &g_self.attachement_points[0].point.y) +PARAM_ADD(PARAM_FLOAT, ap0z, &g_self.attachement_points[0].point.z) + +PARAM_ADD(PARAM_UINT8, ap1id, &g_self.attachement_points[1].id) +PARAM_ADD(PARAM_FLOAT, ap1x, &g_self.attachement_points[1].point.x) +PARAM_ADD(PARAM_FLOAT, ap1y, &g_self.attachement_points[1].point.y) +PARAM_ADD(PARAM_FLOAT, ap1z, &g_self.attachement_points[1].point.z) + +PARAM_ADD(PARAM_UINT8, ap2id, &g_self.attachement_points[2].id) +PARAM_ADD(PARAM_FLOAT, ap2x, &g_self.attachement_points[2].point.x) +PARAM_ADD(PARAM_FLOAT, ap2y, &g_self.attachement_points[2].point.y) +PARAM_ADD(PARAM_FLOAT, ap2z, &g_self.attachement_points[2].point.z) + + PARAM_GROUP_STOP(ctrlLeeP) @@ -733,12 +940,37 @@ LOG_ADD(LOG_FLOAT, n1x, &g_self.n1.x) LOG_ADD(LOG_FLOAT, n1y, &g_self.n1.y) LOG_ADD(LOG_FLOAT, n1z, &g_self.n1.z) +LOG_ADD(LOG_FLOAT, n2x, &g_self.n2.x) +LOG_ADD(LOG_FLOAT, n2y, &g_self.n2.y) +LOG_ADD(LOG_FLOAT, n2z, &g_self.n2.z) + +LOG_ADD(LOG_FLOAT, n3x, &g_self.n3.x) +LOG_ADD(LOG_FLOAT, n3y, &g_self.n3.y) +LOG_ADD(LOG_FLOAT, n3z, &g_self.n3.z) + +LOG_ADD(LOG_FLOAT, n4x, &g_self.n4.x) +LOG_ADD(LOG_FLOAT, n4y, &g_self.n4.y) +LOG_ADD(LOG_FLOAT, n4z, &g_self.n4.z) + +LOG_ADD(LOG_FLOAT, n5x, &g_self.n5.x) +LOG_ADD(LOG_FLOAT, n5y, &g_self.n5.y) +LOG_ADD(LOG_FLOAT, n5z, &g_self.n5.z) + +LOG_ADD(LOG_FLOAT, n6x, &g_self.n6.x) +LOG_ADD(LOG_FLOAT, n6y, &g_self.n6.y) +LOG_ADD(LOG_FLOAT, n6z, &g_self.n6.z) + +// computed desired payload force +LOG_ADD(LOG_FLOAT, Fdx, &g_self.F_d.x) +LOG_ADD(LOG_FLOAT, Fdy, &g_self.F_d.y) +LOG_ADD(LOG_FLOAT, Fdz, &g_self.F_d.z) + // computed virtual input LOG_ADD(LOG_FLOAT, desVirtInpx, &g_self.desVirtInp.x) LOG_ADD(LOG_FLOAT, desVirtInpy, &g_self.desVirtInp.y) LOG_ADD(LOG_FLOAT, desVirtInpz, &g_self.desVirtInp.z) -// LOG_ADD(LOG_UINT32, ticks, &ticks) +LOG_ADD(LOG_UINT32, profQP, &qp_runtime_us) LOG_GROUP_STOP(ctrlLeeP) diff --git a/src/modules/src/crtp_localization_service.c b/src/modules/src/crtp_localization_service.c index 585170d163..2ba43043ee 100644 --- a/src/modules/src/crtp_localization_service.c +++ b/src/modules/src/crtp_localization_service.c @@ -115,6 +115,7 @@ static bool enableLighthouseAngleStream = false; static float extPosStdDev = 0.01; static float extQuatStdDev = 4.5e-3; static bool isInit = false; +static bool isTimeSynced = false; static uint8_t my_id; static uint16_t tickOfLastPacket; // tick when last packet was received @@ -161,7 +162,29 @@ static void updateLogFromExtPos() ext_pose.z = ext_pos.z; } +/* This function is called from all localization events that use + broadcasts (i.e., pos, pos_packed, pose, pose_packed). The first + time such a broadcast is received, the usecTimer is reset to 0. + Since broadcasts are received simultanously at all Crazyflies, + this should reset the local on-board timer of all Crazyflies + at almost the same exact time. + + TODO: In the future, it would be better to have a dedicated CRTP + functionality for this, rather than "piggybacking" on the + localization service for the motion capture. +*/ + +static void syncTimeIfNeeded() +{ + if (!isTimeSynced) { + usecTimerReset(); + isTimeSynced = true; + } +} + static void extPositionHandler(CRTPPacket* pk) { + syncTimeIfNeeded(); + const struct CrtpExtPosition* data = (const struct CrtpExtPosition*)pk->data; ext_pos.x = data->x; @@ -176,6 +199,8 @@ static void extPositionHandler(CRTPPacket* pk) { } static void extPoseHandler(const CRTPPacket* pk) { + syncTimeIfNeeded(); + const struct CrtpExtPose* data = (const struct CrtpExtPose*)&pk->data[1]; ext_pose.x = data->x; @@ -193,24 +218,28 @@ static void extPoseHandler(const CRTPPacket* pk) { } static void extPosePackedHandler(const CRTPPacket* pk) { + syncTimeIfNeeded(); + uint8_t numItems = (pk->size - 1) / sizeof(extPosePackedItem); for (uint8_t i = 0; i < numItems; ++i) { const extPosePackedItem* item = (const extPosePackedItem*)&pk->data[1 + i * sizeof(extPosePackedItem)]; + + ext_pose.x = item->x / 1000.0f; + ext_pose.y = item->y / 1000.0f; + ext_pose.z = item->z / 1000.0f; + quatdecompress(item->quat, (float *)&ext_pose.quat.q0); + ext_pose.stdDevPos = extPosStdDev; + ext_pose.stdDevQuat = extQuatStdDev; + if (item->id == my_id) { - ext_pose.x = item->x / 1000.0f; - ext_pose.y = item->y / 1000.0f; - ext_pose.z = item->z / 1000.0f; - quatdecompress(item->quat, (float *)&ext_pose.quat.q0); - ext_pose.stdDevPos = extPosStdDev; - ext_pose.stdDevQuat = extQuatStdDev; estimatorEnqueuePose(&ext_pose); tickOfLastPacket = xTaskGetTickCount(); } else { - ext_pos.x = item->x / 1000.0f; - ext_pos.y = item->y / 1000.0f; - ext_pos.z = item->z / 1000.0f; - ext_pos.stdDev = extPosStdDev; - peerLocalizationTellPosition(item->id, &ext_pos); + // ext_pos.x = item->x / 1000.0f; + // ext_pos.y = item->y / 1000.0f; + // ext_pos.z = item->z / 1000.0f; + // ext_pos.stdDev = extPosStdDev; + peerLocalizationTellPose(item->id, &ext_pose); } } } @@ -306,6 +335,8 @@ static void genericLocHandle(CRTPPacket* pk) static void extPositionPackedHandler(CRTPPacket* pk) { + syncTimeIfNeeded(); + uint8_t numItems = pk->size / sizeof(extPositionPackedItem); for (uint8_t i = 0; i < numItems; ++i) { const extPositionPackedItem* item = (const extPositionPackedItem*)&pk->data[i * sizeof(extPositionPackedItem)]; diff --git a/src/modules/src/peer_localization.c b/src/modules/src/peer_localization.c index a3ea8d66aa..745672e2ce 100644 --- a/src/modules/src/peer_localization.c +++ b/src/modules/src/peer_localization.c @@ -27,7 +27,30 @@ bool peerLocalizationTellPosition(int cfid, positionMeasurement_t const *pos) other_positions[i].pos.x = pos->x; other_positions[i].pos.y = pos->y; other_positions[i].pos.z = pos->z; - other_positions[i].pos.timestamp = xTaskGetTickCount(); + other_positions[i].orientation.x = nanf(""); + other_positions[i].orientation.y = nanf(""); + other_positions[i].orientation.z = nanf(""); + other_positions[i].orientation.w = nanf(""); + other_positions[i].timestamp = xTaskGetTickCount(); + return true; + } + } + return false; +} + +bool peerLocalizationTellPose(int cfid, poseMeasurement_t const *pose) +{ + for (uint8_t i = 0; i < PEER_LOCALIZATION_MAX_NEIGHBORS; ++i) { + if (other_positions[i].id == 0 || other_positions[i].id == cfid) { + other_positions[i].id = cfid; + other_positions[i].pos.x = pose->pos[0]; + other_positions[i].pos.y = pose->pos[1]; + other_positions[i].pos.z = pose->pos[2]; + other_positions[i].orientation.x = pose->quat.x; + other_positions[i].orientation.y = pose->quat.y; + other_positions[i].orientation.z = pose->quat.z; + other_positions[i].orientation.w = pose->quat.w; + other_positions[i].timestamp = xTaskGetTickCount(); return true; } } diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 9cace10b1a..d0ff51eb08 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -130,7 +130,9 @@ static struct { // for payloads static float payload_alpha = 0.9; // between 0...1; 1: no filter static point_t payload_pos_last; // m (world frame) +static quaternion_t payload_quat_last; static velocity_t payload_vel_last; // m/s (world frame) +static Axis3f payload_omega_last; STATIC_MEM_TASK_ALLOC(stabilizerTask, STABILIZER_TASK_STACKSIZE); @@ -312,13 +314,14 @@ static void stabilizerTask(void* param) // handle the payload // if we got a new state - if (payload_pos_last.timestamp < other->pos.timestamp) { + if (payload_pos_last.timestamp < other->timestamp) { struct vec vel_filtered = vzero(); + struct vec omega_filtered = vzero(); // in the beginning, estimate the velocity to be zero, otherwise use // numeric estimation with filter if (payload_pos_last.timestamp != 0) { // estimate the velocity numerically - const float dt = (other->pos.timestamp - payload_pos_last.timestamp) / 1000.0f; //s + const float dt = (other->timestamp - payload_pos_last.timestamp) / 1000.0f; //s struct vec pos = mkvec(other->pos.x, other->pos.y, other->pos.z); struct vec last_pos = mkvec(payload_pos_last.x, payload_pos_last.y, payload_pos_last.z); struct vec vel = vdiv(vsub(pos, last_pos), dt); @@ -326,32 +329,56 @@ static void stabilizerTask(void* param) // apply a simple complementary filter struct vec vel_old = mkvec(payload_vel_last.x, payload_vel_last.y, payload_vel_last.z); vel_filtered = vadd(vscl(1.0f - payload_alpha, vel_old), vscl(payload_alpha, vel)); + + // estimate omega numerically + struct quat q = mkquat(other->orientation.x, other->orientation.y, other->orientation.z, other->orientation.w); + struct quat last_q = mkquat(payload_quat_last.x, payload_quat_last.y, payload_quat_last.z, payload_quat_last.w); + struct vec omega = quat2omega(last_q, q, dt); + struct vec omega_old = mkvec(payload_omega_last.x, payload_omega_last.y, payload_omega_last.z); + omega_filtered = vadd(vscl(1.0f - payload_alpha, omega_old), vscl(payload_alpha, omega)); } // update the position state.payload_pos.x = other->pos.x; state.payload_pos.y = other->pos.y; state.payload_pos.z = other->pos.z; - state.payload_pos.timestamp = other->pos.timestamp; + state.payload_pos.timestamp = other->timestamp; + + // update the orientation + state.payload_quat.x = other->orientation.x; + state.payload_quat.y = other->orientation.y; + state.payload_quat.z = other->orientation.z; + state.payload_quat.w = other->orientation.w; + state.payload_quat.timestamp = other->timestamp; // update the velocity state.payload_vel.x = vel_filtered.x; state.payload_vel.y = vel_filtered.y; state.payload_vel.z = vel_filtered.z; - state.payload_vel.timestamp = other->pos.timestamp; + state.payload_vel.timestamp = other->timestamp; + + // update the angular velocity + state.payload_omega.x = omega_filtered.x; + state.payload_omega.y = omega_filtered.y; + state.payload_omega.z = omega_filtered.z; // update state payload_pos_last = state.payload_pos; + payload_quat_last = state.payload_quat; payload_vel_last = state.payload_vel; + payload_omega_last = state.payload_omega; } else { state.payload_pos = payload_pos_last; + state.payload_quat = payload_quat_last; state.payload_vel = payload_vel_last; + state.payload_omega = payload_omega_last; } } else if (num_neighbors < MAX_NEIGHBOR_UAVS) { // handle regular team members - state.position_neighbors[num_neighbors].x = other->pos.x; - state.position_neighbors[num_neighbors].y = other->pos.y; - state.position_neighbors[num_neighbors].z = other->pos.z; + state.neighbors[num_neighbors].id = other->id; + state.neighbors[num_neighbors].pos.x = other->pos.x; + state.neighbors[num_neighbors].pos.y = other->pos.y; + state.neighbors[num_neighbors].pos.z = other->pos.z; ++num_neighbors; } } @@ -825,6 +852,73 @@ LOG_ADD_CORE(LOG_FLOAT, qz, &state.attitudeQuaternion.z) * @brief Attitude as a quaternion, w */ LOG_ADD_CORE(LOG_FLOAT, qw, &state.attitudeQuaternion.w) + + +/** + * @brief The position of the payload in the global reference frame, X [m] + */ +LOG_ADD(LOG_FLOAT, px, &state.payload_pos.x) + +/** + * @brief The position of the payload in the global reference frame, Y [m] + */ +LOG_ADD(LOG_FLOAT, py, &state.payload_pos.y) + +/** + * @brief The position of the payload in the global reference frame, Z [m] + */ +LOG_ADD(LOG_FLOAT, pz, &state.payload_pos.z) + +/** + * @brief The orientation of the payload in the global reference frame + */ +LOG_ADD(LOG_FLOAT, pqx, &state.payload_quat.x) + +/** + * @brief The orientation of the payload in the global reference frame + */ +LOG_ADD(LOG_FLOAT, pqy, &state.payload_quat.y) + +/** + * @brief The orientation of the payload in the global reference frame + */ +LOG_ADD(LOG_FLOAT, pqz, &state.payload_quat.z) + +/** + * @brief The orientation of the payload in the global reference frame + */ +LOG_ADD(LOG_FLOAT, pqw, &state.payload_quat.w) + +/** + * @brief The velocity of the payload in the global reference frame, X [m/s] + */ +LOG_ADD(LOG_FLOAT, pvx, &state.payload_vel.x) + +/** + * @brief The velocity of the payload in the global reference frame, Y [m/s] + */ +LOG_ADD(LOG_FLOAT, pvy, &state.payload_vel.y) + +/** + * @brief The velocity of the payload in the global reference frame, Z [m/s] + */ +LOG_ADD(LOG_FLOAT, pvz, &state.payload_vel.z) + +/** + * @brief Angular velocity of the payload, X [rad/s] + */ +LOG_ADD(LOG_FLOAT, pwx, &state.payload_omega.x) + +/** + * @brief Angular velocity of the payload, Y [rad/s] + */ +LOG_ADD(LOG_FLOAT, pwy, &state.payload_omega.y) + +/** + * @brief Angular velocity of the payload, Z [rad/s] + */ +LOG_ADD(LOG_FLOAT, pwz, &state.payload_omega.z) + LOG_GROUP_STOP(stateEstimate) /**