[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6015] moved booz stabilization to firmwares/rotorcr
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6015] moved booz stabilization to firmwares/rotorcraft, still need to rename and adjust makefiles |
|
Date: |
Tue, 28 Sep 2010 15:47:28 +0000 |
Revision: 6015
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6015
Author: flixr
Date: 2010-09-28 15:47:28 +0000 (Tue, 28 Sep 2010)
Log Message:
-----------
moved booz stabilization to firmwares/rotorcraft, still need to rename and
adjust makefiles
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/booz/booz_stabilization.c
paparazzi3/trunk/sw/airborne/booz/booz_stabilization.h
paparazzi3/trunk/sw/airborne/booz/stabilization/
Deleted: paparazzi3/trunk/sw/airborne/booz/booz_stabilization.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_stabilization.c 2010-09-28
14:48:59 UTC (rev 6014)
+++ paparazzi3/trunk/sw/airborne/booz/booz_stabilization.c 2010-09-28
15:47:28 UTC (rev 6015)
@@ -1,36 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
- *
- * 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.
- */
-
-#include "booz_stabilization.h"
-
-int32_t booz_stabilization_cmd[COMMANDS_NB];
-
-void booz_stabilization_init(void) {
-#ifndef BOOZ_STABILIZATION_SKIP_RATE
- booz_stabilization_rate_init();
-#endif
- booz_stabilization_attitude_init();
-}
-
-
-
Deleted: paparazzi3/trunk/sw/airborne/booz/booz_stabilization.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_stabilization.h 2010-09-28
14:48:59 UTC (rev 6014)
+++ paparazzi3/trunk/sw/airborne/booz/booz_stabilization.h 2010-09-28
15:47:28 UTC (rev 6015)
@@ -1,38 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
- *
- * 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.
- */
-
-#ifndef BOOZ_STABILIZATION_H
-#define BOOZ_STABILIZATION_H
-
-#include "std.h"
-
-#include "airframe.h"
-
-#include "stabilization/booz_stabilization_rate.h"
-#include "stabilization/booz_stabilization_attitude.h"
-
-extern void booz_stabilization_init(void);
-
-extern int32_t booz_stabilization_cmd[COMMANDS_NB];
-
-#endif /* BOOZ_STABILIZATION_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,47 @@
+
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef BOOZ_STABILIZATION_ATTITUDE_H
+#define BOOZ_STABILIZATION_ATTITUDE_H
+
+
+#include STABILISATION_ATTITUDE_H
+extern void booz_stabilization_attitude_init(void);
+extern void booz_stabilization_attitude_read_rc(bool_t in_flight);
+extern void booz_stabilization_attitude_read_beta_vane(float beta);
+extern void booz_stabilization_attitude_read_alpha_vane(float alpha);
+extern void booz_stabilization_attitude_enter(void);
+extern void booz_stabilization_attitude_run(bool_t in_flight);
+
+#include "stabilization/booz_stabilization_attitude_ref.h"
+#include STABILISATION_ATTITUDE_REF_H
+extern void booz_stabilization_attitude_ref_init(void);
+extern void booz_stabilization_attitude_ref_update(void);
+
+#define booz_stabilization_attitude_SetKiPhi(_val) { \
+ booz_stabilization_gains.i.x = _val; \
+ booz_stabilization_att_sum_err.phi = 0; \
+ }
+
+#endif /* BOOZ2_STABILIZATION_ATTITUDE_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,148 @@
+/*
+ * $Id: booz_stabilization_attitude_euler.c 3795 2009-07-24 23:43:02Z poine $
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "booz_stabilization.h"
+
+#include "math/pprz_algebra_float.h"
+#include <firmwares/rotorcraft/ahrs.h>
+#include "booz_radio_control.h"
+
+#include "airframe.h"
+
+
+struct FloatAttitudeGains booz_stabilization_gains;
+struct FloatEulers booz_stabilization_att_sum_err;
+
+float booz_stabilization_att_fb_cmd[COMMANDS_NB];
+float booz_stabilization_att_ff_cmd[COMMANDS_NB];
+
+
+void booz_stabilization_attitude_init(void) {
+
+ booz_stabilization_attitude_ref_init();
+
+ VECT3_ASSIGN(booz_stabilization_gains.p,
+ BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN);
+
+ VECT3_ASSIGN(booz_stabilization_gains.d,
+ BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN);
+
+ VECT3_ASSIGN(booz_stabilization_gains.i,
+ BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
+
+ VECT3_ASSIGN(booz_stabilization_gains.dd,
+ BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
+
+ FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
+
+}
+
+
+void booz_stabilization_attitude_read_rc(bool_t in_flight) {
+
+ BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stab_att_sp_euler, in_flight);
+
+}
+
+
+void booz_stabilization_attitude_enter(void) {
+
+ BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stab_att_sp_euler );
+ FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
+
+}
+
+
+#define MAX_SUM_ERR RadOfDeg(56000)
+
+void booz_stabilization_attitude_run(bool_t in_flight) {
+
+ booz_stabilization_attitude_ref_update();
+
+ /* Compute feedforward */
+ booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
+ booz_stabilization_gains.dd.x * booz_stab_att_ref_accel.p / 32.;
+ booz_stabilization_att_ff_cmd[COMMAND_PITCH] =
+ booz_stabilization_gains.dd.y * booz_stab_att_ref_accel.q / 32.;
+ booz_stabilization_att_ff_cmd[COMMAND_YAW] =
+ booz_stabilization_gains.dd.z * booz_stab_att_ref_accel.r / 32.;
+
+ /* Compute feedback */
+ /* attitude error */
+ struct FloatEulers att_float;
+ EULERS_FLOAT_OF_BFP(att_float, ahrs.ltp_to_body_euler);
+ struct FloatEulers att_err;
+ EULERS_DIFF(att_err, att_float, booz_stab_att_ref_euler);
+ FLOAT_ANGLE_NORMALIZE(att_err.psi);
+
+ if (in_flight) {
+ /* update integrator */
+ EULERS_ADD(booz_stabilization_att_sum_err, att_err);
+ EULERS_BOUND_CUBE(booz_stabilization_att_sum_err, -MAX_SUM_ERR,
MAX_SUM_ERR);
+ }
+ else {
+ FLOAT_EULERS_ZERO(booz_stabilization_att_sum_err);
+ }
+
+ /* rate error */
+ struct FloatRates rate_float;
+ RATES_FLOAT_OF_BFP(rate_float, ahrs.body_rate);
+ struct FloatRates rate_err;
+ RATES_DIFF(rate_err, rate_float, booz_stab_att_ref_rate);
+
+ /* PID */
+
+ booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
+ booz_stabilization_gains.p.x * att_err.phi +
+ booz_stabilization_gains.d.x * rate_err.p +
+ booz_stabilization_gains.i.x * booz_stabilization_att_sum_err.phi / 1024.;
+
+ booz_stabilization_att_fb_cmd[COMMAND_PITCH] =
+ booz_stabilization_gains.p.y * att_err.theta +
+ booz_stabilization_gains.d.y * rate_err.q +
+ booz_stabilization_gains.i.y * booz_stabilization_att_sum_err.theta /
1024.;
+
+ booz_stabilization_att_fb_cmd[COMMAND_YAW] =
+ booz_stabilization_gains.p.z * att_err.psi +
+ booz_stabilization_gains.d.z * rate_err.r +
+ booz_stabilization_gains.i.z * booz_stabilization_att_sum_err.psi / 1024.;
+
+
+ booz_stabilization_cmd[COMMAND_ROLL] =
+
(booz_stabilization_att_fb_cmd[COMMAND_ROLL]+booz_stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
+ booz_stabilization_cmd[COMMAND_PITCH] =
+
(booz_stabilization_att_fb_cmd[COMMAND_PITCH]+booz_stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
+ booz_stabilization_cmd[COMMAND_YAW] =
+
(booz_stabilization_att_fb_cmd[COMMAND_YAW]+booz_stabilization_att_ff_cmd[COMMAND_YAW])/16.;
+
+}
+
+
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,159 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "booz_stabilization.h"
+#include <firmwares/rotorcraft/ahrs.h>
+#include "booz_radio_control.h"
+
+
+
+#include "airframe.h"
+
+struct Int32AttitudeGains booz_stabilization_gains;
+
+struct Int32Eulers booz_stabilization_att_sum_err;
+
+int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB];
+int32_t booz_stabilization_att_ff_cmd[COMMANDS_NB];
+
+void booz_stabilization_attitude_init(void) {
+
+ booz_stabilization_attitude_ref_init();
+
+
+ VECT3_ASSIGN(booz_stabilization_gains.p,
+ BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN);
+
+ VECT3_ASSIGN(booz_stabilization_gains.d,
+ BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN);
+
+ VECT3_ASSIGN(booz_stabilization_gains.i,
+ BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
+
+ VECT3_ASSIGN(booz_stabilization_gains.dd,
+ BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
+ BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
+
+
+ INT_EULERS_ZERO( booz_stabilization_att_sum_err );
+
+}
+
+
+void booz_stabilization_attitude_read_rc(bool_t in_flight) {
+
+ BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stab_att_sp_euler, in_flight);
+
+}
+
+
+void booz_stabilization_attitude_enter(void) {
+
+ BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stab_att_sp_euler );
+ INT_EULERS_ZERO( booz_stabilization_att_sum_err );
+
+}
+
+
+#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
+#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
+
+#define MAX_SUM_ERR 4000000
+
+void booz_stabilization_attitude_run(bool_t in_flight) {
+
+
+ /* update reference */
+ booz_stabilization_attitude_ref_update();
+
+ /* compute feedforward command */
+ booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
+ OFFSET_AND_ROUND(booz_stabilization_gains.dd.x *
booz_stab_att_ref_accel.p, 5);
+ booz_stabilization_att_ff_cmd[COMMAND_PITCH] =
+ OFFSET_AND_ROUND(booz_stabilization_gains.dd.y *
booz_stab_att_ref_accel.q, 5);
+ booz_stabilization_att_ff_cmd[COMMAND_YAW] =
+ OFFSET_AND_ROUND(booz_stabilization_gains.dd.z *
booz_stab_att_ref_accel.r, 5);
+
+ /* compute feedback command */
+ /* attitude error */
+ const struct Int32Eulers att_ref_scaled = {
+ OFFSET_AND_ROUND(booz_stab_att_ref_euler.phi, (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC)),
+ OFFSET_AND_ROUND(booz_stab_att_ref_euler.theta, (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC)),
+ OFFSET_AND_ROUND(booz_stab_att_ref_euler.psi, (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC)) };
+ struct Int32Eulers att_err;
+ EULERS_DIFF(att_err, ahrs.ltp_to_body_euler, att_ref_scaled);
+ INT32_ANGLE_NORMALIZE(att_err.psi);
+
+ if (in_flight) {
+ /* update integrator */
+ EULERS_ADD(booz_stabilization_att_sum_err, att_err);
+ EULERS_BOUND_CUBE(booz_stabilization_att_sum_err, -MAX_SUM_ERR,
MAX_SUM_ERR);
+ }
+ else {
+ INT_EULERS_ZERO(booz_stabilization_att_sum_err);
+ }
+
+ /* rate error */
+ const struct Int32Rates rate_ref_scaled = {
+ OFFSET_AND_ROUND(booz_stab_att_ref_rate.p, (REF_RATE_FRAC -
INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(booz_stab_att_ref_rate.q, (REF_RATE_FRAC -
INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(booz_stab_att_ref_rate.r, (REF_RATE_FRAC -
INT32_RATE_FRAC)) };
+ struct Int32Rates rate_err;
+ RATES_DIFF(rate_err, ahrs.body_rate, rate_ref_scaled);
+
+ /* PID */
+ booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
+ booz_stabilization_gains.p.x * att_err.phi +
+ booz_stabilization_gains.d.x * rate_err.p +
+ OFFSET_AND_ROUND2((booz_stabilization_gains.i.x *
booz_stabilization_att_sum_err.phi), 10);
+
+ booz_stabilization_att_fb_cmd[COMMAND_PITCH] =
+ booz_stabilization_gains.p.y * att_err.theta +
+ booz_stabilization_gains.d.y * rate_err.q +
+ OFFSET_AND_ROUND2((booz_stabilization_gains.i.y *
booz_stabilization_att_sum_err.theta), 10);
+
+ booz_stabilization_att_fb_cmd[COMMAND_YAW] =
+ booz_stabilization_gains.p.z * att_err.psi +
+ booz_stabilization_gains.d.z * rate_err.r +
+ OFFSET_AND_ROUND2((booz_stabilization_gains.i.z *
booz_stabilization_att_sum_err.psi), 10);
+
+ /* sum feedforward and feedback */
+ booz_stabilization_cmd[COMMAND_ROLL] =
+
OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_ROLL]+booz_stabilization_att_ff_cmd[COMMAND_ROLL]),
16);
+
+ booz_stabilization_cmd[COMMAND_PITCH] =
+
OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_PITCH]+booz_stabilization_att_ff_cmd[COMMAND_PITCH]),
16);
+
+ booz_stabilization_cmd[COMMAND_YAW] =
+
OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_YAW]+booz_stabilization_att_ff_cmd[COMMAND_YAW]),
16);
+
+}
+
+
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_float.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,52 @@
+/*
+ * $Id: booz_stabilization_attitude.h 3794 2009-07-24 22:01:51Z poine $
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef BOOZ_STABILIZATION_ATTITUDE_FLOAT_H
+#define BOOZ_STABILIZATION_ATTITUDE_FLOAT_H
+
+#include "math/pprz_algebra_float.h"
+
+#include "airframe.h"
+
+struct FloatAttitudeGains {
+ struct FloatVect3 p;
+ struct FloatVect3 d;
+ struct FloatVect3 dd;
+ struct FloatVect3 rates_d;
+ struct FloatVect3 i;
+ struct FloatVect3 surface_p;
+ struct FloatVect3 surface_d;
+ struct FloatVect3 surface_dd;
+ struct FloatVect3 surface_i;
+};
+
+extern struct FloatAttitudeGains booz_stabilization_gains[];
+extern struct FloatEulers booz_stabilization_att_sum_err_eulers;
+
+extern float booz_stabilization_att_fb_cmd[COMMANDS_NB];
+extern float booz_stabilization_att_ff_cmd[COMMANDS_NB];
+
+void booz_stabilization_attitude_gain_schedule(uint8_t idx);
+
+#endif /* BOOZ_STABILIZATION_ATTITUDE_FLOAT_H */
+
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_int.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,45 @@
+/*
+ * $Id: booz_stabilization_attitude.h 3794 2009-07-24 22:01:51Z poine $
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef BOOZ_STABILIZATION_ATTITUDE_INT_H
+#define BOOZ_STABILIZATION_ATTITUDE_INT_H
+
+#include "math/pprz_algebra_int.h"
+
+#include "airframe.h"
+
+struct Int32AttitudeGains {
+ struct Int32Vect3 p;
+ struct Int32Vect3 d;
+ struct Int32Vect3 dd;
+ struct Int32Vect3 i;
+};
+
+extern struct Int32AttitudeGains booz_stabilization_gains;
+extern struct Int32Eulers booz_stabilization_att_sum_err;
+
+extern int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB];
+extern int32_t booz_stabilization_att_ff_cmd[COMMANDS_NB];
+
+#endif /* BOOZ_STABILIZATION_ATTITUDE_INT_H */
+
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,219 @@
+/*
+ * $Id: booz_stabilization_attitude_quat_float.c 3787 2009-07-24 15:33:54Z
poine $
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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 booz_stabilization_attitude_quat_float.c
+ * \brief Booz quaternion attitude stabilization
+ */
+
+#include "booz_stabilization.h"
+
+#include <stdio.h>
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_int.h"
+#include <firmwares/rotorcraft/ahrs.h>
+#include "airframe.h"
+
+struct FloatAttitudeGains
booz_stabilization_gains[BOOZ_STABILIZATION_ATTITUDE_GAIN_NB];
+
+struct FloatQuat booz_stabilization_att_sum_err_quat;
+struct FloatEulers booz_stabilization_att_sum_err_eulers;
+
+float booz_stabilization_att_fb_cmd[COMMANDS_NB];
+float booz_stabilization_att_ff_cmd[COMMANDS_NB];
+
+static int gain_idx = BOOZ_STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT;
+
+static const float phi_pgain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN;
+static const float theta_pgain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN;
+static const float psi_pgain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN;
+
+static const float phi_dgain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN;
+static const float theta_dgain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN;
+static const float psi_dgain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN;
+
+static const float phi_igain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN;
+static const float theta_igain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN;
+static const float psi_igain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN;
+
+static const float phi_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN;
+static const float theta_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN;
+static const float psi_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN;
+
+static const float phi_dgain_d[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN_D;
+static const float theta_dgain_d[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN_D;
+static const float psi_dgain_d[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN_D;
+
+static const float phi_pgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE;
+static const float theta_pgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE;
+static const float psi_pgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE;
+
+static const float phi_dgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE;
+static const float theta_dgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE;
+static const float psi_dgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE;
+
+static const float phi_igain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE;
+static const float theta_igain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE;
+static const float psi_igain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE;
+
+static const float phi_ddgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE;
+static const float theta_ddgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE;
+static const float psi_ddgain_surface[] =
BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE;
+
+#define IERROR_SCALE 1024
+
+void booz_stabilization_attitude_init(void) {
+
+ booz_stabilization_attitude_ref_init();
+
+ for (int i = 0; i < BOOZ_STABILIZATION_ATTITUDE_GAIN_NB; i++) {
+ VECT3_ASSIGN(booz_stabilization_gains[i].p, phi_pgain[i], theta_pgain[i],
psi_pgain[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].d, phi_dgain[i], theta_dgain[i],
psi_dgain[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].i, phi_igain[i], theta_igain[i],
psi_igain[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].dd, phi_ddgain[i],
theta_ddgain[i], psi_ddgain[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].rates_d, phi_dgain_d[i],
theta_dgain_d[i], psi_dgain_d[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].surface_p, phi_pgain_surface[i],
theta_pgain_surface[i], psi_pgain_surface[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].surface_d, phi_dgain_surface[i],
theta_dgain_surface[i], psi_dgain_surface[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].surface_i, phi_igain_surface[i],
theta_igain_surface[i], psi_igain_surface[i]);
+ VECT3_ASSIGN(booz_stabilization_gains[i].surface_dd,
phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
+ }
+
+ FLOAT_QUAT_ZERO( booz_stabilization_att_sum_err_quat );
+ FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err_eulers );
+}
+
+void booz_stabilization_attitude_gain_schedule(uint8_t idx)
+{
+ if (gain_idx >= BOOZ_STABILIZATION_ATTITUDE_GAIN_NB) {
+ // This could be bad -- Just say no.
+ return;
+ }
+ gain_idx = idx;
+ booz_stabilization_attitude_ref_schedule(idx);
+}
+
+void booz_stabilization_attitude_enter(void) {
+
+ booz_stabilization_attitude_ref_enter();
+
+ FLOAT_QUAT_ZERO( booz_stabilization_att_sum_err_quat );
+ FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err_eulers );
+}
+
+static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains
*gains, struct FloatRates *ref_accel)
+{
+ /* Compute feedforward based on reference acceleration */
+
+ ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x *
ref_accel->p;
+ ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y *
ref_accel->q;
+ ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z *
ref_accel->r;
+ ff_commands[COMMAND_ROLL_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.x
* ref_accel->p;
+ ff_commands[COMMAND_PITCH_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.y
* ref_accel->q;
+ ff_commands[COMMAND_YAW_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.z
* ref_accel->r;
+}
+
+static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains
*gains, struct FloatQuat *att_err,
+ struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct
FloatQuat *sum_err)
+{
+ /* PID feedback */
+ fb_commands[COMMAND_ROLL] =
+ GAIN_PRESCALER_P * -gains->p.x * att_err->qx +
+ GAIN_PRESCALER_D * gains->d.x * rate_err->p +
+ GAIN_PRESCALER_D * gains->rates_d.x * rate_err_d->p +
+ GAIN_PRESCALER_I * gains->i.x * sum_err->qx;
+
+ fb_commands[COMMAND_PITCH] =
+ GAIN_PRESCALER_P * -gains->p.y * att_err->qy +
+ GAIN_PRESCALER_D * gains->d.y * rate_err->q +
+ GAIN_PRESCALER_D * gains->rates_d.y * rate_err_d->q +
+ GAIN_PRESCALER_I * gains->i.y * sum_err->qy;
+
+ fb_commands[COMMAND_YAW] =
+ GAIN_PRESCALER_P * -gains->p.z * att_err->qz +
+ GAIN_PRESCALER_D * gains->d.z * rate_err->r +
+ GAIN_PRESCALER_D * gains->rates_d.z * rate_err_d->r +
+ GAIN_PRESCALER_I * gains->i.z * sum_err->qz;
+
+ fb_commands[COMMAND_ROLL_SURFACE] =
+ GAIN_PRESCALER_P * -gains->surface_p.x * att_err->qx +
+ GAIN_PRESCALER_D * gains->surface_d.x * rate_err->p +
+ GAIN_PRESCALER_I * gains->surface_i.x * sum_err->qx;
+
+ fb_commands[COMMAND_PITCH_SURFACE] =
+ GAIN_PRESCALER_P * -gains->surface_p.y * att_err->qy +
+ GAIN_PRESCALER_D * gains->surface_d.y * rate_err->q +
+ GAIN_PRESCALER_I * gains->surface_i.y * sum_err->qy;
+
+ fb_commands[COMMAND_YAW_SURFACE] =
+ GAIN_PRESCALER_P * -gains->surface_p.z * att_err->qz +
+ GAIN_PRESCALER_D * gains->surface_d.z * rate_err->r +
+ GAIN_PRESCALER_I * gains->surface_i.z * sum_err->qz;
+
+}
+
+void booz_stabilization_attitude_run(bool_t enable_integrator) {
+
+ /*
+ * Update reference
+ */
+ booz_stabilization_attitude_ref_update();
+
+ /*
+ * Compute errors for feedback
+ */
+
+ /* attitude error */
+ struct FloatQuat att_err;
+ FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat,
booz_stab_att_ref_quat);
+ /* wrap it in the shortest direction */
+ FLOAT_QUAT_WRAP_SHORTEST(att_err);
+
+ /* rate error */
+ struct FloatRates rate_err;
+ RATES_DIFF(rate_err, ahrs_float.body_rate, booz_stab_att_ref_rate);
+
+ /* integrated error */
+ if (enable_integrator) {
+ struct FloatQuat new_sum_err, scaled_att_err;
+ /* update accumulator */
+ scaled_att_err.qi = att_err.qi;
+ scaled_att_err.qx = att_err.qx / IERROR_SCALE;
+ scaled_att_err.qy = att_err.qy / IERROR_SCALE;
+ scaled_att_err.qz = att_err.qz / IERROR_SCALE;
+ FLOAT_QUAT_COMP_INV(new_sum_err, booz_stabilization_att_sum_err_quat,
scaled_att_err);
+ FLOAT_QUAT_NORMALISE(new_sum_err);
+ FLOAT_QUAT_COPY(booz_stabilization_att_sum_err_quat, new_sum_err);
+ FLOAT_EULERS_OF_QUAT(booz_stabilization_att_sum_err_eulers,
booz_stabilization_att_sum_err_quat);
+ } else {
+ /* reset accumulator */
+ FLOAT_QUAT_ZERO( booz_stabilization_att_sum_err_quat );
+ FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err_eulers );
+ }
+
+ attitude_run_ff(booz_stabilization_att_ff_cmd,
&booz_stabilization_gains[gain_idx], &booz_stab_att_ref_accel);
+
+ attitude_run_fb(booz_stabilization_att_fb_cmd,
&booz_stabilization_gains[gain_idx], &att_err, &rate_err,
&ahrs_float.body_rate_d, &booz_stabilization_att_sum_err_quat);
+
+ for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) {
+ booz_stabilization_cmd[i] =
booz_stabilization_att_fb_cmd[i]+booz_stabilization_att_ff_cmd[i];
+ }
+}
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,37 @@
+#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_H
+#define BOOZ_STABILIZATION_ATTITUDE_REF_H
+
+#define SATURATE_SPEED_TRIM_ACCEL() { \
+ if (booz_stab_att_ref_rate.p >= REF_RATE_MAX_P) { \
+ booz_stab_att_ref_rate.p = REF_RATE_MAX_P; \
+ if (booz_stab_att_ref_accel.p > 0) \
+ booz_stab_att_ref_accel.p = 0; \
+ } \
+ else if (booz_stab_att_ref_rate.p <= -REF_RATE_MAX_P) { \
+ booz_stab_att_ref_rate.p = -REF_RATE_MAX_P; \
+ if (booz_stab_att_ref_accel.p < 0) \
+ booz_stab_att_ref_accel.p = 0; \
+ } \
+ if (booz_stab_att_ref_rate.q >= REF_RATE_MAX_Q) { \
+ booz_stab_att_ref_rate.q = REF_RATE_MAX_Q; \
+ if (booz_stab_att_ref_accel.q > 0) \
+ booz_stab_att_ref_accel.q = 0; \
+ } \
+ else if (booz_stab_att_ref_rate.q <= -REF_RATE_MAX_Q) { \
+ booz_stab_att_ref_rate.q = -REF_RATE_MAX_Q; \
+ if (booz_stab_att_ref_accel.q < 0) \
+ booz_stab_att_ref_accel.q = 0; \
+ } \
+ if (booz_stab_att_ref_rate.r >= REF_RATE_MAX_R) { \
+ booz_stab_att_ref_rate.r = REF_RATE_MAX_R; \
+ if (booz_stab_att_ref_accel.r > 0) \
+ booz_stab_att_ref_accel.r = 0; \
+ } \
+ else if (booz_stab_att_ref_rate.r <= -REF_RATE_MAX_R) { \
+ booz_stab_att_ref_rate.r = -REF_RATE_MAX_R; \
+ if (booz_stab_att_ref_accel.r < 0) \
+ booz_stab_att_ref_accel.r = 0; \
+ } \
+ }
+
+#endif /* BOOZ_STABILIZATION_ATTITUDE_REF_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,85 @@
+#include "booz_stabilization.h"
+
+
+struct FloatEulers booz_stab_att_sp_euler;
+struct FloatEulers booz_stab_att_ref_euler;
+struct FloatRates booz_stab_att_ref_rate;
+struct FloatRates booz_stab_att_ref_accel;
+
+void booz_stabilization_attitude_ref_init(void) {
+
+ FLOAT_EULERS_ZERO(booz_stab_att_sp_euler);
+ FLOAT_EULERS_ZERO(booz_stab_att_ref_euler);
+ FLOAT_RATES_ZERO(booz_stab_att_ref_rate);
+ FLOAT_RATES_ZERO(booz_stab_att_ref_accel);
+
+}
+
+
+/*
+ * Reference
+ */
+#define DT_UPDATE (1./512.)
+
+#define REF_ACCEL_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_PDOT
+#define REF_ACCEL_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_QDOT
+#define REF_ACCEL_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_RDOT
+
+#define REF_RATE_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_P
+#define REF_RATE_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_Q
+#define REF_RATE_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_R
+
+#define OMEGA_P BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P
+#define OMEGA_Q BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q
+#define OMEGA_R BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
+
+#define ZETA_P BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P
+#define ZETA_Q BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q
+#define ZETA_R BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
+
+
+#define USE_REF 1
+
+void booz_stabilization_attitude_ref_update() {
+
+#ifdef USE_REF
+
+ /* dumb integrate reference attitude */
+ struct FloatRates delta_rate;
+ RATES_SMUL(delta_rate, booz_stab_att_ref_rate, DT_UPDATE);
+ struct FloatEulers delta_angle;
+ EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
+ EULERS_ADD(booz_stab_att_ref_euler, delta_angle );
+ FLOAT_ANGLE_NORMALIZE(booz_stab_att_ref_euler.psi);
+
+ /* integrate reference rotational speeds */
+ struct FloatRates delta_accel;
+ RATES_SMUL(delta_accel, booz_stab_att_ref_accel, DT_UPDATE);
+ RATES_ADD(booz_stab_att_ref_rate, delta_accel);
+
+ /* compute reference attitude error */
+ struct FloatEulers ref_err;
+ EULERS_DIFF(ref_err, booz_stab_att_ref_euler, booz_stab_att_sp_euler);
+ /* wrap it in the shortest direction */
+ FLOAT_ANGLE_NORMALIZE(ref_err.psi);
+
+ /* compute reference angular accelerations */
+ booz_stab_att_ref_accel.p = -2.*ZETA_P*OMEGA_P*booz_stab_att_ref_rate.p -
OMEGA_P*OMEGA_P*ref_err.phi;
+ booz_stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_P*booz_stab_att_ref_rate.q -
OMEGA_Q*OMEGA_Q*ref_err.theta;
+ booz_stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*booz_stab_att_ref_rate.r -
OMEGA_R*OMEGA_R*ref_err.psi;
+
+ /* saturate acceleration */
+ const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q,
-REF_ACCEL_MAX_R };
+ const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q,
REF_ACCEL_MAX_R }; \
+ RATES_BOUND_BOX(booz_stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
+
+ /* saturate speed and trim accel accordingly */
+ SATURATE_SPEED_TRIM_ACCEL();
+
+#else /* !USE_REF */
+ EULERS_COPY(booz_stab_att_ref_euler, booz_stabilization_att_sp);
+ FLOAT_RATES_ZERO(booz_stab_att_ref_rate);
+ FLOAT_RATES_ZERO(booz_stab_att_ref_accel);
+#endif /* USE_REF */
+
+}
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,77 @@
+/*
+ * $Id: booz_stabilization_attitude_ref_traj_euler.h 3796 2009-07-25 00:01:02Z
poine $
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H
+#define BOOZ_STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H
+
+#include "booz_radio_control.h"
+#include "math/pprz_algebra_float.h"
+
+
+#include "stabilization/booz_stabilization_attitude_ref_float.h"
+
+/*
+ * Radio Control
+ */
+#define SP_MAX_PHI BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI
+#define SP_MAX_THETA BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA
+#define SP_MAX_R BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R
+
+
+
+
+
+#define RC_UPDATE_FREQ 40.
+
+#define YAW_DEADBAND_EXCEEDED()
\
+ (radio_control.values[RADIO_CONTROL_YAW] >
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \
+ radio_control.values[RADIO_CONTROL_YAW] <
-BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
+
+#define BOOZ_STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \
+ \
+ _sp.phi = \
+ (-radio_control.values[RADIO_CONTROL_ROLL] * SP_MAX_PHI / MAX_PPRZ); \
+ _sp.theta =
\
+ ( radio_control.values[RADIO_CONTROL_PITCH] * SP_MAX_THETA / MAX_PPRZ); \
+ if (_inflight) { \
+ if (YAW_DEADBAND_EXCEEDED()) { \
+ _sp.psi += \
+ (-radio_control.values[RADIO_CONTROL_YAW] * SP_MAX_R / MAX_PPRZ /
RC_UPDATE_FREQ); \
+ FLOAT_ANGLE_NORMALIZE(_sp.psi); \
+ }
\
+ } \
+ else { /* if not flying, use current yaw as setpoint */ \
+ _sp.psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \
+ } \
+ }
+
+#define BOOZ_STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \
+ struct FloatEulers add_sp_float; \
+ EULERS_FLOAT_OF_BFP(add_sp_float, (_add_sp)); \
+ EULERS_ADD(booz_stabilization_att_sp,add_sp_float); \
+ FLOAT_ANGLE_NORMALIZE(booz_stabilization_att_sp.psi); \
+ }
+
+
+
+#endif /* BOOZ2_STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,129 @@
+/*
+ * $Id: booz_stabilization_attitude_ref_euler_int.h -1 $
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "booz_stabilization.h"
+
+struct Int32Eulers booz_stab_att_sp_euler;
+struct Int32Eulers booz_stab_att_ref_euler;
+struct Int32Rates booz_stab_att_ref_rate;
+struct Int32Rates booz_stab_att_ref_accel;
+
+void booz_stabilization_attitude_ref_init(void) {
+
+ INT_EULERS_ZERO(booz_stab_att_sp_euler);
+ INT_EULERS_ZERO(booz_stab_att_ref_euler);
+ INT_RATES_ZERO( booz_stab_att_ref_rate);
+ INT_RATES_ZERO( booz_stab_att_ref_accel);
+
+}
+
+#define F_UPDATE_RES 9
+#define F_UPDATE (1<<F_UPDATE_RES)
+
+#define REF_ACCEL_MAX_P BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_PDOT,
REF_ACCEL_FRAC)
+#define REF_ACCEL_MAX_Q BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_QDOT,
REF_ACCEL_FRAC)
+#define REF_ACCEL_MAX_R BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_RDOT,
REF_ACCEL_FRAC)
+
+#define REF_RATE_MAX_P BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_P,
REF_RATE_FRAC)
+#define REF_RATE_MAX_Q BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_Q,
REF_RATE_FRAC)
+#define REF_RATE_MAX_R BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_REF_MAX_R,
REF_RATE_FRAC)
+
+#define OMEGA_P BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P
+#define ZETA_P BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P
+#define ZETA_OMEGA_P_RES 10
+#define ZETA_OMEGA_P BFP_OF_REAL((ZETA_P*OMEGA_P), ZETA_OMEGA_P_RES)
+#define OMEGA_2_P_RES 7
+#define OMEGA_2_P BFP_OF_REAL((OMEGA_P*OMEGA_P), OMEGA_2_P_RES)
+
+#define OMEGA_Q BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q
+#define ZETA_Q BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q
+#define ZETA_OMEGA_Q_RES 10
+#define ZETA_OMEGA_Q BFP_OF_REAL((ZETA_Q*OMEGA_Q), ZETA_OMEGA_Q_RES)
+#define OMEGA_2_Q_RES 7
+#define OMEGA_2_Q BFP_OF_REAL((OMEGA_Q*OMEGA_Q), OMEGA_2_Q_RES)
+
+#define OMEGA_R BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
+#define ZETA_R BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
+#define ZETA_OMEGA_R_RES 10
+#define ZETA_OMEGA_R BFP_OF_REAL((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES)
+#define OMEGA_2_R_RES 7
+#define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES)
+
+#define USE_REF 1
+
+void booz_stabilization_attitude_ref_update() {
+
+#ifdef USE_REF
+
+ /* dumb integrate reference attitude */
+ const struct Int32Eulers d_angle = {
+ booz_stab_att_ref_rate.p >> ( F_UPDATE_RES + REF_RATE_FRAC -
REF_ANGLE_FRAC),
+ booz_stab_att_ref_rate.q >> ( F_UPDATE_RES + REF_RATE_FRAC -
REF_ANGLE_FRAC),
+ booz_stab_att_ref_rate.r >> ( F_UPDATE_RES + REF_RATE_FRAC -
REF_ANGLE_FRAC)};
+ EULERS_ADD(booz_stab_att_ref_euler, d_angle );
+ ANGLE_REF_NORMALIZE(booz_stab_att_ref_euler.psi);
+
+ /* integrate reference rotational speeds */
+ const struct Int32Rates d_rate = {
+ booz_stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC -
REF_RATE_FRAC),
+ booz_stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC -
REF_RATE_FRAC),
+ booz_stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC -
REF_RATE_FRAC)};
+ RATES_ADD(booz_stab_att_ref_rate, d_rate);
+
+ /* compute reference attitude error */
+ struct Int32Eulers ref_err;
+ EULERS_DIFF(ref_err, booz_stab_att_ref_euler, booz_stab_att_sp_euler);
+ /* wrap it in the shortest direction */
+ ANGLE_REF_NORMALIZE(ref_err.psi);
+
+ /* compute reference angular accelerations */
+ const struct Int32Rates accel_rate = {
+ ((int32_t)(-2.*ZETA_OMEGA_P) * (booz_stab_att_ref_rate.p >>
(REF_RATE_FRAC - REF_ACCEL_FRAC)))
+ >> (ZETA_OMEGA_P_RES),
+ ((int32_t)(-2.*ZETA_OMEGA_Q) * (booz_stab_att_ref_rate.q >>
(REF_RATE_FRAC - REF_ACCEL_FRAC)))
+ >> (ZETA_OMEGA_Q_RES),
+ ((int32_t)(-2.*ZETA_OMEGA_R) * (booz_stab_att_ref_rate.r >>
(REF_RATE_FRAC - REF_ACCEL_FRAC)))
+ >> (ZETA_OMEGA_R_RES) };
+
+ const struct Int32Rates accel_angle = {
+ ((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (REF_ANGLE_FRAC -
REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES),
+ ((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (REF_ANGLE_FRAC -
REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES),
+ ((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (REF_ANGLE_FRAC -
REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) };
+
+ RATES_SUM(booz_stab_att_ref_accel, accel_rate, accel_angle);
+
+ /* saturate acceleration */
+ const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q,
-REF_ACCEL_MAX_R };
+ const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q,
REF_ACCEL_MAX_R };
+ RATES_BOUND_BOX(booz_stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
+
+ /* saturate speed and trim accel accordingly */
+ SATURATE_SPEED_TRIM_ACCEL();
+
+#else /* !USE_REF */
+ EULERS_COPY(booz_stab_att_ref_euler, booz_stabilization_att_sp);
+ INT_RATES_ZERO(booz_stab_att_ref_rate);
+ INT_RATES_ZERO(booz_stab_att_ref_accel);
+#endif /* USE_REF */
+
+}
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,94 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_EULER_INT_H
+#define BOOZ_STABILIZATION_ATTITUDE_REF_EULER_INT_H
+
+#include "stabilization/booz_stabilization_attitude_ref_int.h"
+
+#include "booz_radio_control.h"
+
+
+#define REF_ACCEL_FRAC 12
+#define REF_RATE_FRAC 16
+#define REF_ANGLE_FRAC 20
+#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029,
REF_ANGLE_FRAC)
+#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029,
REF_ANGLE_FRAC)
+#define ANGLE_REF_NORMALIZE(_a) { \
+ while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI;
\
+ while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI;
\
+ }
+
+
+
+/*
+ * Radio Control
+ */
+#define SP_MAX_PHI ANGLE_BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI)
+#define SP_MAX_THETA
ANGLE_BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA)
+#define SP_MAX_R ANGLE_BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R)
+
+
+
+
+
+#define RC_UPDATE_FREQ 40
+
+#define YAW_DEADBAND_EXCEEDED()
\
+ (radio_control.values[RADIO_CONTROL_YAW] >
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \
+ radio_control.values[RADIO_CONTROL_YAW] <
-BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
+
+#define BOOZ_STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \
+ \
+ _sp.phi = \
+ ((int32_t)-radio_control.values[RADIO_CONTROL_ROLL] *
(int32_t)SP_MAX_PHI / MAX_PPRZ) \
+ << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
\
+ _sp.theta =
\
+ ((int32_t) radio_control.values[RADIO_CONTROL_PITCH] *
(int32_t)SP_MAX_THETA / MAX_PPRZ) \
+ << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
\
+ if (_inflight) { \
+ if (YAW_DEADBAND_EXCEEDED()) { \
+ _sp.psi += \
+ ((int32_t)-radio_control.values[RADIO_CONTROL_YAW] *
(int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \
+ << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
\
+ ANGLE_REF_NORMALIZE(_sp.psi); \
+ }
\
+ } \
+ else { /* if not flying, use current yaw as setpoint */ \
+ _sp.psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC)); \
+ } \
+ }
+
+#define BOOZ_STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \
+ EULERS_ADD(booz_stab_att_sp_euler,_add_sp); \
+ ANGLE_REF_NORMALIZE(booz_stab_att_sp_euler.psi); \
+}
+
+#define BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \
+ _sp.psi = ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC); \
+ booz_stab_att_ref_euler.psi = _sp.psi; \
+ booz_stab_att_ref_rate.r = 0; \
+ }
+
+
+#endif /* BOOZ2_STABILIZATION_ATTITUDE_REF_EULER_INT_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_float.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,43 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+#ifndef BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H
+#define BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H
+
+#include "airframe.h"
+
+extern struct FloatEulers booz_stab_att_sp_euler;
+extern struct FloatQuat booz_stab_att_sp_quat;
+extern struct FloatEulers booz_stab_att_ref_euler;
+extern struct FloatQuat booz_stab_att_ref_quat;
+extern struct FloatRates booz_stab_att_ref_rate;
+extern struct FloatRates booz_stab_att_ref_accel;
+
+struct FloatRefModel {
+ struct FloatRates omega;
+ struct FloatRates zeta;
+};
+
+extern struct FloatRefModel booz_stab_att_ref_model[];
+
+#endif /* BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H */
+
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_int.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,35 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2010 The Paparazzi Team
+ *
+ * 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.
+ */
+#ifndef BOOZ_STABILISATION_ATTITUDE_REF_INT_H
+#define BOOZ_STABILISATION_ATTITUDE_REF_INT_H
+
+extern struct Int32Eulers booz_stab_att_sp_vi_euler; /* vehicle interface */
+extern struct Int32Eulers booz_stab_att_sp_rc_euler; /* radio control */
+extern struct Int32Eulers booz_stab_att_sp_euler; /* sum of the above */
+extern struct Int32Quat booz_stab_att_sp_quat;
+extern struct Int32Eulers booz_stab_att_ref_euler;
+extern struct Int32Quat booz_stab_att_ref_quat;
+extern struct Int32Rates booz_stab_att_ref_rate;
+extern struct Int32Rates booz_stab_att_ref_accel;
+
+#endif /* BOOZ_STABILISATION_ATTITUDE_REF_INT_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,148 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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 booz_stabilization_attitude_ref_float.c
+ * \brief Booz attitude reference generation (quaternion float version)
+ *
+ */
+
+#include "airframe.h"
+#include "booz_stabilization.h"
+#include <firmwares/rotorcraft/ahrs.h>
+
+#include "booz_stabilization_attitude_ref_float.h"
+#include "quat_setpoint.h"
+
+#define REF_ACCEL_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_PDOT
+#define REF_ACCEL_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_QDOT
+#define REF_ACCEL_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_RDOT
+
+struct FloatEulers booz_stab_att_sp_euler;
+struct FloatQuat booz_stab_att_sp_quat;
+struct FloatEulers booz_stab_att_ref_euler;
+struct FloatQuat booz_stab_att_ref_quat;
+struct FloatRates booz_stab_att_ref_rate;
+struct FloatRates booz_stab_att_ref_accel;
+
+struct FloatRefModel
booz_stab_att_ref_model[BOOZ_STABILIZATION_ATTITUDE_GAIN_NB];
+
+static int ref_idx = BOOZ_STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT;
+
+static const float omega_p[] = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P;
+static const float zeta_p[] = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P;
+static const float omega_q[] = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q;
+static const float zeta_q[] = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q;
+static const float omega_r[] = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R;
+static const float zeta_r[] = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R;
+
+static void reset_psi_ref_from_body(void) {
+ booz_stab_att_ref_euler.psi = ahrs_float.ltp_to_body_euler.psi;
+ booz_stab_att_ref_rate.r = 0;
+ booz_stab_att_ref_accel.r = 0;
+}
+
+static void update_ref_quat_from_eulers(void) {
+ struct FloatRMat ref_rmat;
+
+#ifdef STICKS_RMAT312
+ FLOAT_RMAT_OF_EULERS_312(ref_rmat, booz_stab_att_ref_euler);
+#else
+ FLOAT_RMAT_OF_EULERS_321(ref_rmat, booz_stab_att_ref_euler);
+#endif
+ FLOAT_QUAT_OF_RMAT(booz_stab_att_ref_quat, ref_rmat);
+ FLOAT_QUAT_WRAP_SHORTEST(booz_stab_att_ref_quat);
+}
+
+void booz_stabilization_attitude_ref_init(void) {
+
+ FLOAT_EULERS_ZERO(booz_stab_att_sp_euler);
+ FLOAT_QUAT_ZERO( booz_stab_att_sp_quat);
+ FLOAT_EULERS_ZERO(booz_stab_att_ref_euler);
+ FLOAT_QUAT_ZERO( booz_stab_att_ref_quat);
+ FLOAT_RATES_ZERO( booz_stab_att_ref_rate);
+ FLOAT_RATES_ZERO( booz_stab_att_ref_accel);
+
+ for (int i = 0; i < BOOZ_STABILIZATION_ATTITUDE_GAIN_NB; i++) {
+ RATES_ASSIGN(booz_stab_att_ref_model[i].omega, omega_p[i], omega_q[i],
omega_r[i]);
+ RATES_ASSIGN(booz_stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i],
zeta_r[i]);
+ }
+
+}
+
+void booz_stabilization_attitude_ref_schedule(uint8_t idx)
+{
+ ref_idx = idx;
+}
+
+void booz_stabilization_attitude_ref_enter()
+{
+ reset_psi_ref_from_body();
+ booz_stabilization_attitude_sp_enter();
+ update_ref_quat_from_eulers();
+}
+
+/*
+ * Reference
+ */
+#ifdef BOOZ_AP_PERIODIC_PRESCALE
+#define DT_UPDATE ((float) BOOZ_AP_PERIODIC_PRESCALE / (float) PERIODIC_FREQ)
+#else
+#define DT_UPDATE (1./512.)
+#endif
+
+void booz_stabilization_attitude_ref_update() {
+
+ /* integrate reference attitude */
+ struct FloatQuat qdot;
+ FLOAT_QUAT_DERIVATIVE(qdot, booz_stab_att_ref_rate, booz_stab_att_ref_quat);
+ QUAT_SMUL(qdot, qdot, DT_UPDATE);
+ QUAT_ADD(booz_stab_att_ref_quat, qdot);
+ FLOAT_QUAT_NORMALISE(booz_stab_att_ref_quat);
+
+ /* integrate reference rotational speeds */
+ struct FloatRates delta_rate;
+ RATES_SMUL(delta_rate, booz_stab_att_ref_accel, DT_UPDATE);
+ RATES_ADD(booz_stab_att_ref_rate, delta_rate);
+
+ /* compute reference angular accelerations */
+ struct FloatQuat err;
+ /* compute reference attitude error */
+ FLOAT_QUAT_INV_COMP(err, booz_stab_att_sp_quat, booz_stab_att_ref_quat);
+ /* wrap it in the shortest direction */
+ FLOAT_QUAT_WRAP_SHORTEST(err);
+ /* propagate the 2nd order linear model */
+ booz_stab_att_ref_accel.p =
-2.*booz_stab_att_ref_model[ref_idx].zeta.p*booz_stab_att_ref_model[ref_idx].omega.p*booz_stab_att_ref_rate.p
+ -
booz_stab_att_ref_model[ref_idx].omega.p*booz_stab_att_ref_model[ref_idx].omega.p*err.qx;
+ booz_stab_att_ref_accel.q =
-2.*booz_stab_att_ref_model[ref_idx].zeta.q*booz_stab_att_ref_model[ref_idx].omega.q*booz_stab_att_ref_rate.q
+ -
booz_stab_att_ref_model[ref_idx].omega.q*booz_stab_att_ref_model[ref_idx].omega.q*err.qy;
+ booz_stab_att_ref_accel.r =
-2.*booz_stab_att_ref_model[ref_idx].zeta.r*booz_stab_att_ref_model[ref_idx].omega.r*booz_stab_att_ref_rate.r
+ -
booz_stab_att_ref_model[ref_idx].omega.r*booz_stab_att_ref_model[ref_idx].omega.r*err.qz;
+
+ /* saturate acceleration */
+ const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q,
-REF_ACCEL_MAX_R };
+ const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q,
REF_ACCEL_MAX_R };
+ RATES_BOUND_BOX(booz_stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
+
+ /* compute ref_euler */
+ FLOAT_EULERS_OF_QUAT(booz_stab_att_ref_euler, booz_stab_att_ref_quat);
+}
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,59 @@
+/*
+ * $Id: booz_stabilization_attitude_ref_traj_euler.h 3796 2009-07-25 00:01:02Z
poine $
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H
+#define BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H
+
+#include "booz_stabilization.h"
+
+#include "booz_radio_control.h"
+#include "math/pprz_algebra_float.h"
+
+#include "stabilization/booz_stabilization_attitude_ref_float.h"
+
+#define RC_UPDATE_FREQ 40.
+#define ROLL_COEF (BOOZ_STABILIZATION_ATTITUDE_SP_MAX_P / MAX_PPRZ)
+#define ROLL_COEF_H (BOOZ_STABILIZATION_ATTITUDE_SP_MAX_P_H / MAX_PPRZ)
+#define PITCH_COEF ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ)
+#define YAW_COEF (BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PSI / MAX_PPRZ)
+
+#define ROLL_COEF_RATE (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_P / MAX_PPRZ)
+#define PITCH_COEF_RATE ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_Q / MAX_PPRZ)
+#define YAW_COEF_RATE ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ)
+
+#define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE <
-VALUE))
+#define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ?
VARIABLE : 0.0)
+
+#define ROLL_DEADBAND_EXCEEDED()
\
+ (radio_control.values[RADIO_CONTROL_ROLL] >
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A || \
+ radio_control.values[RADIO_CONTROL_ROLL] <
-BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A)
+#define PITCH_DEADBAND_EXCEEDED()
\
+ (radio_control.values[RADIO_CONTROL_PITCH] >
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_E || \
+ radio_control.values[RADIO_CONTROL_PITCH] <
-BOOZ_STABILIZATION_ATTITUDE_DEADBAND_E)
+#define YAW_DEADBAND_EXCEEDED()
\
+ (radio_control.values[RADIO_CONTROL_YAW] >
BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \
+ radio_control.values[RADIO_CONTROL_YAW] <
-BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
+
+void booz_stabilization_attitude_ref_enter(void);
+void booz_stabilization_attitude_ref_schedule(uint8_t idx);
+
+#endif /* BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,198 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ * Copyright (C) 2010 Felix Ruess <address@hidden>
+ *
+ * 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.
+ */
+
+#include "booz_stabilization.h"
+
+#include <firmwares/rotorcraft/ahrs.h>
+
+#include <firmwares/rotorcraft/imu.h>
+#include "booz_radio_control.h"
+#include "airframe.h"
+
+#define F_UPDATE_RES 9
+#define REF_DOT_FRAC 11
+#define REF_FRAC 16
+
+#define MAX_SUM_ERR 4000000
+
+#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_P
+#define BOOZ_STABILIZATION_RATE_DDGAIN_P 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_Q
+#define BOOZ_STABILIZATION_RATE_DDGAIN_Q 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_R
+#define BOOZ_STABILIZATION_RATE_DDGAIN_R 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_IGAIN_P
+#define BOOZ_STABILIZATION_RATE_IGAIN_P 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_IGAIN_Q
+#define BOOZ_STABILIZATION_RATE_IGAIN_Q 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_IGAIN_R
+#define BOOZ_STABILIZATION_RATE_IGAIN_R 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_REF_TAU
+#define BOOZ_STABILIZATION_RATE_REF_TAU 4
+#endif
+
+#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
+#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
+
+struct Int32Rates booz_stabilization_rate_sp;
+struct Int32Rates booz_stabilization_rate_gain;
+struct Int32Rates booz_stabilization_rate_igain;
+struct Int32Rates booz_stabilization_rate_ddgain;
+struct Int32Rates booz_stabilization_rate_ref;
+struct Int32Rates booz_stabilization_rate_refdot;
+struct Int32Rates booz_stabilization_rate_sum_err;
+
+struct Int32Rates booz_stabilization_rate_fb_cmd;
+struct Int32Rates booz_stabilization_rate_ff_cmd;
+
+
+#ifndef BOOZ_STABILIZATION_RATE_DEADBAND_P
+#define BOOZ_STABILIZATION_RATE_DEADBAND_P 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_DEADBAND_Q
+#define BOOZ_STABILIZATION_RATE_DEADBAND_Q 0
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_DEADBAND_R
+#define BOOZ_STABILIZATION_RATE_DEADBAND_R 200
+#endif
+
+#define ROLL_RATE_DEADBAND_EXCEEDED() \
+ (radio_control.values[RADIO_CONTROL_ROLL] >
BOOZ_STABILIZATION_RATE_DEADBAND_P || \
+ radio_control.values[RADIO_CONTROL_ROLL] <
-BOOZ_STABILIZATION_RATE_DEADBAND_P)
+
+#define PITCH_RATE_DEADBAND_EXCEEDED()
\
+ (radio_control.values[RADIO_CONTROL_PITCH] >
BOOZ_STABILIZATION_RATE_DEADBAND_Q || \
+ radio_control.values[RADIO_CONTROL_PITCH] <
-BOOZ_STABILIZATION_RATE_DEADBAND_Q)
+
+#define YAW_RATE_DEADBAND_EXCEEDED() \
+ (radio_control.values[RADIO_CONTROL_YAW] >
BOOZ_STABILIZATION_RATE_DEADBAND_R || \
+ radio_control.values[RADIO_CONTROL_YAW] <
-BOOZ_STABILIZATION_RATE_DEADBAND_R)
+
+
+void booz_stabilization_rate_init(void) {
+
+ INT_RATES_ZERO(booz_stabilization_rate_sp);
+
+ RATES_ASSIGN(booz_stabilization_rate_gain,
+ BOOZ_STABILIZATION_RATE_GAIN_P,
+ BOOZ_STABILIZATION_RATE_GAIN_Q,
+ BOOZ_STABILIZATION_RATE_GAIN_R);
+ RATES_ASSIGN(booz_stabilization_rate_igain,
+ BOOZ_STABILIZATION_RATE_IGAIN_P,
+ BOOZ_STABILIZATION_RATE_IGAIN_Q,
+ BOOZ_STABILIZATION_RATE_IGAIN_R);
+ RATES_ASSIGN(booz_stabilization_rate_ddgain,
+ BOOZ_STABILIZATION_RATE_DDGAIN_P,
+ BOOZ_STABILIZATION_RATE_DDGAIN_Q,
+ BOOZ_STABILIZATION_RATE_DDGAIN_R);
+
+ INT_RATES_ZERO(booz_stabilization_rate_ref);
+ INT_RATES_ZERO(booz_stabilization_rate_refdot);
+ INT_RATES_ZERO(booz_stabilization_rate_sum_err);
+}
+
+
+void booz_stabilization_rate_read_rc( void ) {
+
+ if(ROLL_RATE_DEADBAND_EXCEEDED())
+ booz_stabilization_rate_sp.p =
(int32_t)-radio_control.values[RADIO_CONTROL_ROLL] *
BOOZ_STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
+ else
+ booz_stabilization_rate_sp.p = 0;
+
+ if(PITCH_RATE_DEADBAND_EXCEEDED())
+ booz_stabilization_rate_sp.q =
(int32_t)radio_control.values[RADIO_CONTROL_PITCH] *
BOOZ_STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
+ else
+ booz_stabilization_rate_sp.q = 0;
+
+ if(YAW_RATE_DEADBAND_EXCEEDED())
+ booz_stabilization_rate_sp.r =
(int32_t)-radio_control.values[RADIO_CONTROL_YAW] *
BOOZ_STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
+ else
+ booz_stabilization_rate_sp.r = 0;
+}
+
+void booz_stabilization_rate_enter(void) {
+ RATES_COPY(booz_stabilization_rate_ref, booz_stabilization_rate_sp);
+ INT_RATES_ZERO(booz_stabilization_rate_sum_err);
+}
+
+void booz_stabilization_rate_run(bool_t in_flight) {
+
+ /* reference */
+ struct Int32Rates _r;
+ RATES_DIFF(_r, booz_stabilization_rate_sp, booz_stabilization_rate_ref);
+ RATES_SDIV(booz_stabilization_rate_refdot, _r,
BOOZ_STABILIZATION_RATE_REF_TAU);
+ /* integrate ref */
+ const struct Int32Rates _delta_ref = {
+ booz_stabilization_rate_refdot.p >> ( F_UPDATE_RES + REF_DOT_FRAC -
REF_FRAC),
+ booz_stabilization_rate_refdot.q >> ( F_UPDATE_RES + REF_DOT_FRAC -
REF_FRAC),
+ booz_stabilization_rate_refdot.r >> ( F_UPDATE_RES + REF_DOT_FRAC -
REF_FRAC)};
+ RATES_ADD(booz_stabilization_rate_ref, _delta_ref);
+
+ /* compute feed-forward command */
+ RATES_EWMULT_RSHIFT(booz_stabilization_rate_ff_cmd,
booz_stabilization_rate_ddgain, booz_stabilization_rate_refdot, 14);
+
+
+ /* compute feed-back command */
+ /* error for feedback */
+ const struct Int32Rates _ref_scaled = {
+ OFFSET_AND_ROUND(booz_stabilization_rate_ref.p, (REF_FRAC -
INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(booz_stabilization_rate_ref.q, (REF_FRAC -
INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(booz_stabilization_rate_ref.r, (REF_FRAC -
INT32_RATE_FRAC)) };
+ struct Int32Rates _error;
+ RATES_DIFF(_error, ahrs.body_rate, _ref_scaled);
+ if (in_flight) {
+ /* update integrator */
+ RATES_ADD(booz_stabilization_rate_sum_err, _error);
+ RATES_BOUND_CUBE(booz_stabilization_rate_sum_err, -MAX_SUM_ERR,
MAX_SUM_ERR);
+ }
+ else {
+ INT_RATES_ZERO(booz_stabilization_rate_sum_err);
+ }
+
+ /* PI */
+ booz_stabilization_rate_fb_cmd.p = booz_stabilization_rate_gain.p * _error.p
+
+ OFFSET_AND_ROUND2((booz_stabilization_rate_igain.p *
booz_stabilization_rate_sum_err.p), 10);
+
+ booz_stabilization_rate_fb_cmd.q = booz_stabilization_rate_gain.q * _error.q
+
+ OFFSET_AND_ROUND2((booz_stabilization_rate_igain.q *
booz_stabilization_rate_sum_err.q), 10);
+
+ booz_stabilization_rate_fb_cmd.r = booz_stabilization_rate_gain.r * _error.r
+
+ OFFSET_AND_ROUND2((booz_stabilization_rate_igain.r *
booz_stabilization_rate_sum_err.r), 10);
+
+ booz_stabilization_rate_fb_cmd.p = booz_stabilization_rate_fb_cmd.p >> 16;
+ booz_stabilization_rate_fb_cmd.q = booz_stabilization_rate_fb_cmd.q >> 16;
+ booz_stabilization_rate_fb_cmd.r = booz_stabilization_rate_fb_cmd.r >> 16;
+
+ /* sum to final command */
+ booz_stabilization_cmd[COMMAND_ROLL] = booz_stabilization_rate_ff_cmd.p +
booz_stabilization_rate_fb_cmd.p;
+ booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_rate_ff_cmd.q +
booz_stabilization_rate_fb_cmd.q;
+ booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_rate_ff_cmd.r +
booz_stabilization_rate_fb_cmd.r;
+
+}
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h
(from rev 6014,
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,45 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef BOOZ_STABILIZATION_RATE
+#define BOOZ_STABILIZATION_RATE
+
+#include "math/pprz_algebra_int.h"
+
+extern void booz_stabilization_rate_init(void);
+extern void booz_stabilization_rate_read_rc(void);
+extern void booz_stabilization_rate_run(bool_t in_flight);
+extern void booz_stabilization_rate_enter(void);
+
+extern struct Int32Rates booz_stabilization_rate_sp;
+extern struct Int32Rates booz_stabilization_rate_gain;
+extern struct Int32Rates booz_stabilization_rate_igain;
+extern struct Int32Rates booz_stabilization_rate_ddgain;
+extern struct Int32Rates booz_stabilization_rate_ref;
+extern struct Int32Rates booz_stabilization_rate_refdot;
+extern struct Int32Rates booz_stabilization_rate_sum_err;
+
+extern struct Int32Rates booz_stabilization_rate_fb_cmd;
+extern struct Int32Rates booz_stabilization_rate_ff_cmd;
+
+#endif /* BOOZ_STABILIZATION_RATE */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization.c (from
rev 6014, paparazzi3/trunk/sw/airborne/booz/booz_stabilization.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization.c
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,36 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "booz_stabilization.h"
+
+int32_t booz_stabilization_cmd[COMMANDS_NB];
+
+void booz_stabilization_init(void) {
+#ifndef BOOZ_STABILIZATION_SKIP_RATE
+ booz_stabilization_rate_init();
+#endif
+ booz_stabilization_attitude_init();
+}
+
+
+
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization.h (from
rev 6014, paparazzi3/trunk/sw/airborne/booz/booz_stabilization.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization.h
2010-09-28 15:47:28 UTC (rev 6015)
@@ -0,0 +1,38 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef BOOZ_STABILIZATION_H
+#define BOOZ_STABILIZATION_H
+
+#include "std.h"
+
+#include "airframe.h"
+
+#include "stabilization/booz_stabilization_rate.h"
+#include "stabilization/booz_stabilization_attitude.h"
+
+extern void booz_stabilization_init(void);
+
+extern int32_t booz_stabilization_cmd[COMMANDS_NB];
+
+#endif /* BOOZ_STABILIZATION_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6015] moved booz stabilization to firmwares/rotorcraft, still need to rename and adjust makefiles,
Felix Ruess <=