Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 20:04:09 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

quaternion stabilization for transitioning rotorcrafts

Browse Source 
* use int_quat_transition stabilization subsystem type now
* same behaviour for transitioning rotorcrafts as before
* this still should be cleaned up further!
This commit is contained in:
Felix Ruess
2012-03-03 17:26:55 +01:00
parent 18cbd1ac64
commit fe9fcb35ce
7 changed files with 26 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/subsystems/rotorcraft/stabilization_int_quat_transition.makefile
+7
View File
@@ -0,0 +1,7 @@
ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
ap.CFLAGS += -DUSE_SETPOINTS_WITH_TRANSITIONS
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/quat_setpoint_int.c
conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile
+1 -6
View File
@@ -1,6 +1 @@
ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/quat_setpoint_int.c
$(error The stabilization quaternion subsystem has been changed, for normal rotorcrafts please use <subsystem name="stabilization" type="int_quat"/>, for transitioning rotorcrafts (quadshot) use <subsystem name="stabilization" type="int_quat_transition"/>)
sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c
+2 -16
View File
@@ -55,21 +55,7 @@ static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initia
QUAT_COPY(stab_att_sp_quat, pitch_rotated_quat2);
}
static void update_sp_quat_from_eulers(void) {
struct Int32RMat sp_rmat;
#ifdef STICKS_RMAT312
INT32_RMAT_OF_EULERS_312(sp_rmat, stab_att_sp_euler);
#else
INT32_RMAT_OF_EULERS_321(sp_rmat, stab_att_sp_euler);
#endif
INT32_QUAT_OF_RMAT(stab_att_sp_quat, sp_rmat);
INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
}
// FIXME: function parameter sp not used
void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight) {
void stabilization_attitude_read_rc_absolute(bool_t in_flight) {
// FIXME: wtf???
#ifdef AIRPLANE_STICKS
@@ -108,7 +94,7 @@ void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_fl
INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat);
}
void stabilization_attitude_sp_enter()
void stabilization_attitude_sp_enter(void)
{
// reset setpoint to "hover"
reset_sp_quat(0., 0., &ahrs.ltp_to_body_quat);
sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.h
+1 -1
View File
@@ -13,6 +13,6 @@
void stabilization_attitude_sp_enter(void);
void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight);
void stabilization_attitude_read_rc_absolute(bool_t in_flight);
#endif
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+3
View File
@@ -25,7 +25,10 @@
*/
#include "firmwares/rotorcraft/stabilization.h"
#if USE_SETPOINTS_WITH_TRANSITIONS
#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h"
#endif
#include <stdio.h>
#include "math/pprz_algebra_float.h"
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+12 -1
View File
@@ -32,6 +32,10 @@
#include "stabilization_attitude_ref_int.h"
#if USE_SETPOINTS_WITH_TRANSITIONS
#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h"
#endif
#define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC)
#define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC)
#define REF_ACCEL_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_RDOT, REF_ACCEL_FRAC)
@@ -110,8 +114,15 @@ void stabilization_attitude_ref_init(void) {
void stabilization_attitude_ref_enter()
{
reset_psi_ref_from_body();
#if USE_SETPOINTS_WITH_TRANSITIONS
stabilization_attitude_sp_enter();
memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat));
#else
update_quat_from_eulers(&stab_att_ref_quat, &stab_att_ref_euler);
//memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat));
#endif
/* set reference rate and acceleration to zero */
memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates));
memset(&stab_att_ref_rate, 0, sizeof(struct Int32Rates));
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
-1
View File
@@ -22,7 +22,6 @@
#define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H
#include "firmwares/rotorcraft/stabilization.h"
//#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h"
#include "subsystems/radio_control.h"
#include "math/pprz_algebra_float.h"