diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile index d79598d8ebf..448148fedc1 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile @@ -3,6 +3,7 @@ STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_float.h\" STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c +STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_transformations.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c ap.CFLAGS += $(STAB_ATT_CFLAGS) diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile index 317172085ae..50e15da7b11 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile @@ -3,6 +3,7 @@ STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_quat_int.h\" STAB_ATT_SRCS = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c +STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_transformations.c STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c ap.CFLAGS += $(STAB_ATT_CFLAGS) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index a5764fa8590..00fa397b66c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -25,6 +25,7 @@ #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h" #include #include "math/pprz_algebra_float.h" @@ -145,72 +146,7 @@ void stabilization_attitude_set_failsafe_setpoint(void) { void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd); - /* orientation vector describing simultaneous rotation of roll/pitch */ - struct FloatVect3 ov; - ov.x = stab_att_sp_euler.phi; - ov.y = stab_att_sp_euler.theta; - ov.z = 0.0; - /* quaternion from that orientation vector */ - struct FloatQuat q_rp; - FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); - - /// @todo optimize yaw angle calculation - - /* - * Instead of using the psi setpoint angle to rotate around the body z-axis, - * calculate the real angle needed to align the projection of the body x-axis - * onto the horizontal plane with the psi setpoint. - * - * angle between two vectors a and b: - * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) - * where n is the thrust vector (i.e. both a and b lie in that plane) - */ - const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; - const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; - struct FloatVect3 a; - FLOAT_QUAT_VMULT(a, q_rp, xaxis); - - // desired heading vect in earth x-y plane - struct FloatVect3 psi_vect; - psi_vect.x = cosf(stab_att_sp_euler.psi); - psi_vect.y = sinf(stab_att_sp_euler.psi); - psi_vect.z = 0.0; - // normal is the direction of the thrust vector - struct FloatVect3 normal; - FLOAT_QUAT_VMULT(normal, q_rp, zaxis); - - // projection of desired heading onto body x-y plane - // b = v - dot(v,n)*n - float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); - struct FloatVect3 dotn; - FLOAT_VECT3_SMUL(dotn, normal, dot); - - // b = v - dot(v,n)*n - struct FloatVect3 b; - FLOAT_VECT3_DIFF(b, psi_vect, dotn); - dot = FLOAT_VECT3_DOT_PRODUCT(a, b); - struct FloatVect3 cross; - VECT3_CROSS_PRODUCT(cross, a, b); - // norm of the cross product - float nc = FLOAT_VECT3_NORM(cross); - // angle = atan2(norm(cross(a,b)), dot(a,b)) - float yaw2 = atan2(nc, dot) / 2.0; - - // negative angle if needed - // sign(dot(cross(a,b), n) - float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); - if (dot_cross_ab < 0) { - yaw2 = -yaw2; - } - - /* quaternion with yaw command */ - struct FloatQuat q_yaw; - QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); - - /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ - FLOAT_QUAT_COMP(stab_att_sp_quat, q_yaw, q_rp); - FLOAT_QUAT_NORMALIZE(stab_att_sp_quat); - FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat); + quat_from_rpy_cmd_f(&stab_att_sp_quat, &stab_att_sp_euler); } #ifndef GAIN_PRESCALER_FF diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index f709f9f5c04..7ec25903d9e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -25,6 +25,7 @@ #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h" #include #include "math/pprz_algebra_float.h" @@ -96,81 +97,7 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { // copy euler setpoint for debugging memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers)); - /// @todo calc sp_quat in fixed-point - - /* orientation vector describing simultaneous rotation of roll/pitch */ - struct FloatVect3 ov; - ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi); - ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta); - ov.z = 0.0; - /* quaternion from that orientation vector */ - struct FloatQuat q_rp; - FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); - - const float psi_sp = ANGLE_FLOAT_OF_BFP(sp_cmd->psi); - - /// @todo optimize yaw angle calculation - - /* - * Instead of using the psi setpoint angle to rotate around the body z-axis, - * calculate the real angle needed to align the projection of the body x-axis - * onto the horizontal plane with the psi setpoint. - * - * angle between two vectors a and b: - * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) - * where n is the thrust vector (i.e. both a and b lie in that plane) - */ - const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; - const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; - struct FloatVect3 a; - FLOAT_QUAT_VMULT(a, q_rp, xaxis); - - // desired heading vect in earth x-y plane - struct FloatVect3 psi_vect; - psi_vect.x = cosf(psi_sp); - psi_vect.y = sinf(psi_sp); - psi_vect.z = 0.0; - // normal is the direction of the thrust vector - struct FloatVect3 normal; - FLOAT_QUAT_VMULT(normal, q_rp, zaxis); - - // projection of desired heading onto body x-y plane - // b = v - dot(v,n)*n - float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); - struct FloatVect3 dotn; - FLOAT_VECT3_SMUL(dotn, normal, dot); - - // b = v - dot(v,n)*n - struct FloatVect3 b; - FLOAT_VECT3_DIFF(b, psi_vect, dotn); - - dot = FLOAT_VECT3_DOT_PRODUCT(a, b); - struct FloatVect3 cross; - VECT3_CROSS_PRODUCT(cross, a, b); - // norm of the cross product - float nc = FLOAT_VECT3_NORM(cross); - // angle = atan2(norm(cross(a,b)), dot(a,b)) - float yaw2 = atan2(nc, dot) / 2.0; - - // negative angle if needed - // sign(dot(cross(a,b), n) - float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); - if (dot_cross_ab < 0) { - yaw2 = -yaw2; - } - - /* quaternion with yaw command */ - struct FloatQuat q_yaw; - QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); - - /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ - struct FloatQuat q_sp; - FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp); - FLOAT_QUAT_NORMALIZE(q_sp); - FLOAT_QUAT_WRAP_SHORTEST(q_sp); - - /* convert to fixed point */ - QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp); + quat_from_rpy_cmd_i(&stab_att_sp_quat, &stab_att_sp_euler); } #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c new file mode 100644 index 00000000000..457e66370df --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c @@ -0,0 +1,107 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file stabilization_attitude_quat_transformations.c + * Quaternion transformation functions. + */ + +#include "stabilization_attitude_quat_transformations.h" + +void quat_from_rpy_cmd_i(struct Int32Quat* quat, struct Int32Eulers* cmd) { + // use float conversion for now... + struct FloatEulers cmd_f; + EULERS_FLOAT_OF_BFP(cmd_f, *cmd); + + struct FloatQuat quat_f; + quat_from_rpy_cmd_f(&quat_f, &cmd_f); + + // convert back to fixed point + QUAT_BFP_OF_REAL(*quat, quat_f); +} + +void quat_from_rpy_cmd_f(struct FloatQuat* quat, struct FloatEulers* cmd) { + /* orientation vector describing simultaneous rotation of roll/pitch */ + struct FloatVect3 ov; + ov.x = cmd->phi; + ov.y = cmd->theta; + ov.z = 0.0; + /* quaternion from that orientation vector */ + struct FloatQuat q_rp; + FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); + + /// @todo optimize yaw angle calculation + + /* + * Instead of using the psi setpoint angle to rotate around the body z-axis, + * calculate the real angle needed to align the projection of the body x-axis + * onto the horizontal plane with the psi setpoint. + * + * angle between two vectors a and b: + * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) + * where n is the thrust vector (i.e. both a and b lie in that plane) + */ + const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; + const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; + struct FloatVect3 a; + FLOAT_QUAT_VMULT(a, q_rp, xaxis); + + // desired heading vect in earth x-y plane + struct FloatVect3 psi_vect; + psi_vect.x = cosf(cmd->psi); + psi_vect.y = sinf(cmd->psi); + psi_vect.z = 0.0; + // normal is the direction of the thrust vector + struct FloatVect3 normal; + FLOAT_QUAT_VMULT(normal, q_rp, zaxis); + + // projection of desired heading onto body x-y plane + // b = v - dot(v,n)*n + float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); + struct FloatVect3 dotn; + FLOAT_VECT3_SMUL(dotn, normal, dot); + + // b = v - dot(v,n)*n + struct FloatVect3 b; + FLOAT_VECT3_DIFF(b, psi_vect, dotn); + dot = FLOAT_VECT3_DOT_PRODUCT(a, b); + struct FloatVect3 cross; + VECT3_CROSS_PRODUCT(cross, a, b); + // norm of the cross product + float nc = FLOAT_VECT3_NORM(cross); + // angle = atan2(norm(cross(a,b)), dot(a,b)) + float yaw2 = atan2(nc, dot) / 2.0; + + // negative angle if needed + // sign(dot(cross(a,b), n) + float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); + if (dot_cross_ab < 0) { + yaw2 = -yaw2; + } + + /* quaternion with yaw command */ + struct FloatQuat q_yaw; + QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); + + /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ + FLOAT_QUAT_COMP(*quat, q_yaw, q_rp); + FLOAT_QUAT_NORMALIZE(*quat); + FLOAT_QUAT_WRAP_SHORTEST(*quat); +} diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h new file mode 100644 index 00000000000..a6f4f574d87 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file stabilization_attitude_quat_transformations.h + * Quaternion transformation functions. + */ + +#ifndef STABILIZATION_ATTITUDE_QUAT_TRANSFORMATIONS_H +#define STABILIZATION_ATTITUDE_QUAT_TRANSFORMATIONS_H + +#include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_int.h" + +extern void quat_from_rpy_cmd_i(struct Int32Quat* quat, struct Int32Eulers* cmd); +extern void quat_from_rpy_cmd_f(struct FloatQuat* quat, struct FloatEulers* cmd); + +#endif diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index da50880f154..abafcb4b7a7 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -29,6 +29,7 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/stabilization.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h" #include "state.h" #include "stabilization_attitude_ref_float.h" @@ -66,13 +67,6 @@ static inline void reset_psi_ref_from_body(void) { stab_att_ref_accel.r = 0; } -static inline void update_ref_quat_from_eulers(void) { - struct FloatRMat ref_rmat; - FLOAT_RMAT_OF_EULERS(ref_rmat, stab_att_ref_euler); - FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); - FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat); -} - void stabilization_attitude_ref_init(void) { FLOAT_EULERS_ZERO(stab_att_sp_euler); @@ -95,7 +89,7 @@ void stabilization_attitude_ref_schedule(uint8_t idx) { void stabilization_attitude_ref_enter(void) { reset_psi_ref_from_body(); - update_ref_quat_from_eulers(); + quat_from_rpy_cmd_f(&stab_att_ref_quat, &stab_att_ref_euler); } /* diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index b0202847ddb..1c33970b085 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -29,6 +29,7 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/stabilization.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h" #include "stabilization_attitude_ref_int.h" @@ -103,11 +104,10 @@ void stabilization_attitude_ref_enter(void) { reset_psi_ref_from_body(); - /* convert reference attitude with REF_ANGLE_FRAC to eulers with normal INT32_ANGLE_FRAC */ - struct Int32Eulers ref_eul; - INT32_EULERS_RSHIFT(ref_eul, stab_att_ref_euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); - INT32_QUAT_OF_EULERS(stab_att_ref_quat, ref_eul); - INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat); + /* convert reference attitude command with REF_ANGLE_FRAC to eulers with normal INT32_ANGLE_FRAC */ + struct Int32Eulers ref_cmd; + INT32_EULERS_RSHIFT(ref_cmd, stab_att_ref_euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); + quat_from_rpy_cmd_i(&stab_att_ref_quat, &ref_cmd); /* set reference rate and acceleration to zero */ memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates));