[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6004] moving guidance, rename still to be done
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6004] moving guidance, rename still to be done |
|
Date: |
Tue, 28 Sep 2010 14:04:47 +0000 |
Revision: 6004
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6004
Author: flixr
Date: 2010-09-28 14:04:47 +0000 (Tue, 28 Sep 2010)
Log Message:
-----------
moving guidance, rename still to be done
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/booz/booz_guidance.h
paparazzi3/trunk/sw/airborne/booz/guidance/
Deleted: paparazzi3/trunk/sw/airborne/booz/booz_guidance.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_guidance.h 2010-09-28 14:04:33 UTC
(rev 6003)
+++ paparazzi3/trunk/sw/airborne/booz/booz_guidance.h 2010-09-28 14:04:47 UTC
(rev 6004)
@@ -1,7 +0,0 @@
-#ifndef BOOZ_GUIDANCE_H
-#define BOOZ_GUIDANCE_H
-
-#include "guidance/booz2_guidance_h.h"
-#include "guidance/booz2_guidance_v.h"
-
-#endif /* BOOZ_GUIDANCE_H */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
(from rev 6003, paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
2010-09-28 14:04:47 UTC (rev 6004)
@@ -0,0 +1,400 @@
+/*
+ * $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.
+ */
+
+#define B2_GUIDANCE_H_C
+//#define B2_GUIDANCE_H_USE_REF
+#include "booz2_guidance_h.h"
+
+#include "ahrs.h"
+#include "booz_stabilization.h"
+#include "booz_fms.h"
+#include "ins.h"
+#include "booz2_navigation.h"
+
+#include "airframe.h"
+
+uint8_t booz2_guidance_h_mode;
+
+struct Int32Vect2 booz2_guidance_h_pos_sp;
+int32_t booz2_guidance_h_psi_sp;
+struct Int32Vect2 booz2_guidance_h_pos_ref;
+struct Int32Vect2 booz2_guidance_h_speed_ref;
+struct Int32Vect2 booz2_guidance_h_accel_ref;
+
+struct Int32Vect2 booz2_guidance_h_pos_err;
+struct Int32Vect2 booz2_guidance_h_speed_err;
+struct Int32Vect2 booz2_guidance_h_pos_err_sum;
+struct Int32Vect2 booz2_guidance_h_nav_err;
+
+struct Int32Eulers booz2_guidance_h_rc_sp;
+struct Int32Vect2 booz2_guidance_h_command_earth;
+struct Int32Vect2 booz2_guidance_h_stick_earth_sp;
+struct Int32Eulers booz2_guidance_h_command_body;
+
+int32_t booz2_guidance_h_pgain;
+int32_t booz2_guidance_h_dgain;
+int32_t booz2_guidance_h_igain;
+int32_t booz2_guidance_h_ngain;
+int32_t booz2_guidance_h_again;
+
+#ifndef BOOZ2_GUIDANCE_H_NGAIN
+#define BOOZ2_GUIDANCE_H_NGAIN 0
+#endif
+
+#ifndef BOOZ2_GUIDANCE_H_AGAIN
+#define BOOZ2_GUIDANCE_H_AGAIN 0
+#endif
+
+static inline void booz2_guidance_h_hover_run(void);
+static inline void booz2_guidance_h_nav_run(bool_t in_flight);
+static inline void booz2_guidance_h_hover_enter(void);
+static inline void booz2_guidance_h_nav_enter(void);
+
+#define Booz2GuidanceHSetRef(_pos, _speed, _accel) { \
+ b2_gh_set_ref(_pos, _speed, _accel); \
+ VECT2_COPY(booz2_guidance_h_pos_ref, _pos); \
+ VECT2_COPY(booz2_guidance_h_speed_ref, _speed); \
+ VECT2_COPY(booz2_guidance_h_accel_ref, _accel); \
+ }
+
+
+void booz2_guidance_h_init(void) {
+
+ booz2_guidance_h_mode = BOOZ2_GUIDANCE_H_MODE_KILL;
+ booz2_guidance_h_psi_sp = 0;
+ INT_VECT2_ZERO(booz2_guidance_h_pos_sp);
+ INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum);
+ INT_EULERS_ZERO(booz2_guidance_h_rc_sp);
+ INT_EULERS_ZERO(booz2_guidance_h_command_body);
+ booz2_guidance_h_pgain = BOOZ2_GUIDANCE_H_PGAIN;
+ booz2_guidance_h_igain = BOOZ2_GUIDANCE_H_IGAIN;
+ booz2_guidance_h_dgain = BOOZ2_GUIDANCE_H_DGAIN;
+ booz2_guidance_h_ngain = BOOZ2_GUIDANCE_H_NGAIN;
+ booz2_guidance_h_again = BOOZ2_GUIDANCE_H_AGAIN;
+
+}
+
+
+void booz2_guidance_h_mode_changed(uint8_t new_mode) {
+ if (new_mode == booz2_guidance_h_mode)
+ return;
+
+ switch ( booz2_guidance_h_mode ) {
+ // case BOOZ2_GUIDANCE_H_MODE_RATE:
+ // booz_stabilization_rate_exit();
+ // break;
+ default:
+ break;
+ }
+
+ switch (new_mode) {
+
+ case BOOZ2_GUIDANCE_H_MODE_RATE:
+ booz_stabilization_rate_enter();
+ break;
+
+ case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
+ booz_stabilization_attitude_enter();
+ break;
+
+ case BOOZ2_GUIDANCE_H_MODE_HOVER:
+ booz2_guidance_h_hover_enter();
+ break;
+
+ case BOOZ2_GUIDANCE_H_MODE_NAV:
+ booz2_guidance_h_nav_enter();
+ break;
+ default:
+ break;
+ }
+
+ booz2_guidance_h_mode = new_mode;
+
+}
+
+
+void booz2_guidance_h_read_rc(bool_t in_flight) {
+
+ switch ( booz2_guidance_h_mode ) {
+
+ case BOOZ2_GUIDANCE_H_MODE_RATE:
+ booz_stabilization_rate_read_rc();
+ break;
+
+ case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
+ booz_stabilization_attitude_read_rc(in_flight);
+ break;
+
+ case BOOZ2_GUIDANCE_H_MODE_HOVER:
+ BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
+ break;
+
+ case BOOZ2_GUIDANCE_H_MODE_NAV:
+ if (radio_control.status == RADIO_CONTROL_OK) {
+ BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
+ booz2_guidance_h_rc_sp.psi = 0;
+ }
+ else {
+ INT_EULERS_ZERO(booz2_guidance_h_rc_sp);
+ }
+ break;
+ default:
+ break;
+ }
+
+}
+
+
+void booz2_guidance_h_run(bool_t in_flight) {
+ switch ( booz2_guidance_h_mode ) {
+
+ case BOOZ2_GUIDANCE_H_MODE_RATE:
+ booz_stabilization_rate_run(in_flight);
+ break;
+
+ case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
+ booz_stabilization_attitude_run(in_flight);
+ break;
+
+ case BOOZ2_GUIDANCE_H_MODE_HOVER:
+ booz2_guidance_h_hover_run();
+ booz_stabilization_attitude_run(in_flight);
+ break;
+
+ case BOOZ2_GUIDANCE_H_MODE_NAV:
+ {
+ if (!in_flight) booz2_guidance_h_nav_enter();
+
+ if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
+#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
+ booz_stab_att_sp_euler.phi = nav_roll << (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC);
+ booz_stab_att_sp_euler.theta = nav_pitch << (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC);
+#endif
+ }
+ else {
+ INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp,
booz2_navigation_carrot);
+#ifdef B2_GUIDANCE_H_USE_REF
+ b2_gh_update_ref_from_pos_sp(booz2_guidance_h_pos_sp);
+#endif
+#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
+ booz2_guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC));
+#endif
+ //booz2_guidance_h_hover_run();
+ booz2_guidance_h_nav_run(in_flight);
+ }
+ booz_stabilization_attitude_run(in_flight);
+ break;
+ }
+ default:
+ break;
+ }
+
+
+
+}
+
+#define MAX_POS_ERR POS_BFP_OF_REAL(16.)
+#define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
+#define MAX_POS_ERR_SUM ((int32_t)(MAX_POS_ERR)<< 12)
+
+// 15 degres
+//#define MAX_BANK (65536)
+#define MAX_BANK (98000)
+
+static inline void booz2_guidance_h_hover_run(void) {
+
+ /* compute position error */
+ VECT2_DIFF(booz2_guidance_h_pos_err, ins_ltp_pos, booz2_guidance_h_pos_sp);
+ /* saturate it */
+ VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
+
+ /* compute speed error */
+ VECT2_COPY(booz2_guidance_h_speed_err, ins_ltp_speed);
+ /* saturate it */
+ VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
+
+ /* update pos error integral */
+ VECT2_ADD(booz2_guidance_h_pos_err_sum, booz2_guidance_h_pos_err);
+ /* saturate it */
+ VECT2_STRIM(booz2_guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM);
+
+ /* run PID */
+ // cmd_earth < 15.17
+ booz2_guidance_h_command_earth.x = (booz2_guidance_h_pgain<<1) *
booz2_guidance_h_pos_err.x +
+ booz2_guidance_h_dgain *
(booz2_guidance_h_speed_err.x>>9) +
+ booz2_guidance_h_igain *
(booz2_guidance_h_pos_err_sum.x >> 12);
+ booz2_guidance_h_command_earth.y = (booz2_guidance_h_pgain<<1) *
booz2_guidance_h_pos_err.y +
+ booz2_guidance_h_dgain *(
booz2_guidance_h_speed_err.y>>9) +
+ booz2_guidance_h_igain *
(booz2_guidance_h_pos_err_sum.y >> 12);
+
+ VECT2_STRIM(booz2_guidance_h_command_earth, -MAX_BANK, MAX_BANK);
+
+ /* Rotate to body frame */
+ int32_t s_psi, c_psi;
+ PPRZ_ITRIG_SIN(s_psi, ahrs.ltp_to_body_euler.psi);
+ PPRZ_ITRIG_COS(c_psi, ahrs.ltp_to_body_euler.psi);
+
+
+ // INT32_TRIG_FRAC - 2: 100mm erreur, gain 100 -> 10000 command | 2 degres =
36000, so multiply by 4
+ booz2_guidance_h_command_body.phi =
+ ( - s_psi * booz2_guidance_h_command_earth.x + c_psi *
booz2_guidance_h_command_earth.y)
+ >> (INT32_TRIG_FRAC - 2);
+ booz2_guidance_h_command_body.theta =
+ - ( c_psi * booz2_guidance_h_command_earth.x + s_psi *
booz2_guidance_h_command_earth.y)
+ >> (INT32_TRIG_FRAC - 2);
+
+
+ booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi;
+ booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta;
+ booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp +
booz2_guidance_h_rc_sp.psi;
+#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
+ ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi);
+#endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
+
+ EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body);
+
+}
+
+// 20 degres -> 367002 (0.35 << 20)
+#define NAV_MAX_BANK BFP_OF_REAL(0.35,REF_ANGLE_FRAC)
+#define HOLD_DISTANCE POS_BFP_OF_REAL(10.)
+
+static inline void booz2_guidance_h_nav_run(bool_t in_flight) {
+
+ /* convert our reference to generic representation */
+#ifdef B2_GUIDANCE_H_USE_REF
+ INT32_VECT2_RSHIFT(booz2_guidance_h_pos_ref, b2_gh_pos_ref,
(B2_GH_POS_REF_FRAC - INT32_POS_FRAC));
+ INT32_VECT2_LSHIFT(booz2_guidance_h_speed_ref, b2_gh_speed_ref,
(INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
+ INT32_VECT2_LSHIFT(booz2_guidance_h_accel_ref, b2_gh_accel_ref,
(INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC));
+#else
+ VECT2_COPY(booz2_guidance_h_pos_ref, booz2_guidance_h_pos_sp);
+ INT_VECT2_ZERO(booz2_guidance_h_speed_ref);
+ INT_VECT2_ZERO(booz2_guidance_h_accel_ref);
+#endif
+
+ /* compute position error */
+ VECT2_DIFF(booz2_guidance_h_pos_err, ins_ltp_pos, booz2_guidance_h_pos_ref);
+ /* saturate it */
+ VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
+
+ /* compute speed error */
+ //VECT2_COPY(booz2_guidance_h_speed_err, ins_ltp_speed);
+ VECT2_DIFF(booz2_guidance_h_speed_err, ins_ltp_speed,
booz2_guidance_h_speed_ref);
+ /* saturate it */
+ VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
+
+ int32_t dist;
+ INT32_VECT2_NORM(dist, booz2_guidance_h_pos_err);
+ if ( dist < HOLD_DISTANCE ) { // Hold position
+ /* update pos error integral */
+ VECT2_ADD(booz2_guidance_h_pos_err_sum, booz2_guidance_h_pos_err);
+ /* saturate it */
+ VECT2_STRIM(booz2_guidance_h_pos_err_sum, -MAX_POS_ERR_SUM,
MAX_POS_ERR_SUM);
+ }
+ else { // Tracking algorithm, no integral
+ int32_t vect_prod = 0;
+ int32_t scal_prod = ins_ltp_speed.x * booz2_guidance_h_pos_err.x +
ins_ltp_speed.y * booz2_guidance_h_pos_err.y;
+ // compute vectorial product only if angle < pi/2 (scalar product > 0)
+ if (scal_prod >= 0) {
+ vect_prod = ((ins_ltp_speed.x * booz2_guidance_h_pos_err.y) >>
(INT32_POS_FRAC + INT32_SPEED_FRAC - 10))
+ - ((ins_ltp_speed.y * booz2_guidance_h_pos_err.x) >>
(INT32_POS_FRAC + INT32_SPEED_FRAC - 10));
+ }
+ // multiply by vector orthogonal to speed
+ VECT2_ASSIGN(booz2_guidance_h_nav_err,
+ vect_prod * (-ins_ltp_speed.y),
+ vect_prod * ins_ltp_speed.x);
+ // divide by 2 times dist ( >> 16 )
+ VECT2_SDIV(booz2_guidance_h_nav_err, booz2_guidance_h_nav_err, dist*dist);
+ // *2 ??
+ }
+ if (!in_flight) { INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); }
+
+ /* run PID */
+ booz2_guidance_h_command_earth.x =
+ booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.x << (10 -
INT32_POS_FRAC)) +
+ booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.x >>
(INT32_SPEED_FRAC - 10)) +
+ booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.x >> (12 +
INT32_POS_FRAC - 10)) +
+ booz2_guidance_h_ngain * booz2_guidance_h_nav_err.x +
+ booz2_guidance_h_again * booz2_guidance_h_accel_ref.x; /* feedforward gain
*/
+ booz2_guidance_h_command_earth.y =
+ booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.y << (10 -
INT32_POS_FRAC)) +
+ booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.y >>
(INT32_SPEED_FRAC - 10)) +
+ booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.y >> (12 +
INT32_POS_FRAC - 10)) +
+ booz2_guidance_h_ngain * booz2_guidance_h_nav_err.y +
+ booz2_guidance_h_again * booz2_guidance_h_accel_ref.y; /* feedforward gain
*/
+
+ VECT2_STRIM(booz2_guidance_h_command_earth, -NAV_MAX_BANK, NAV_MAX_BANK);
+ INT32_VECT2_RSHIFT(booz2_guidance_h_command_earth,
booz2_guidance_h_command_earth, REF_ANGLE_FRAC - 16); // Reduice to 16 for
trigo operation
+
+ /* Rotate to body frame */
+ int32_t s_psi, c_psi;
+ PPRZ_ITRIG_SIN(s_psi, ahrs.ltp_to_body_euler.psi);
+ PPRZ_ITRIG_COS(c_psi, ahrs.ltp_to_body_euler.psi);
+
+ // Restore angle ref resolution after rotation
+ booz2_guidance_h_command_body.phi =
+ ( - s_psi * booz2_guidance_h_command_earth.x + c_psi *
booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16));
+ booz2_guidance_h_command_body.theta =
+ - ( c_psi * booz2_guidance_h_command_earth.x + s_psi *
booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16));
+
+ // Add RC setpoint
+ booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi;
+ booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta;
+ booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp +
booz2_guidance_h_rc_sp.psi;
+ ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi);
+
+ // Set attitude setpoint
+ EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body);
+
+}
+
+static inline void booz2_guidance_h_hover_enter(void) {
+
+ VECT2_COPY(booz2_guidance_h_pos_sp, ins_ltp_pos);
+
+ BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz2_guidance_h_rc_sp );
+
+ INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum);
+
+}
+
+static inline void booz2_guidance_h_nav_enter(void) {
+
+ INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot);
+ struct Int32Vect2 pos,speed,zero;
+ INT_VECT2_ZERO(zero);
+ VECT2_COPY(pos, ins_ltp_pos);
+ VECT2_COPY(speed, ins_ltp_speed);
+ Booz2GuidanceHSetRef(pos, speed, zero);
+
+ struct Int32Eulers tmp_sp;
+ BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( tmp_sp );
+ booz2_guidance_h_psi_sp = tmp_sp.psi;
+#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
+ nav_heading = (booz2_guidance_h_psi_sp >> (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC));
+#endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
+ booz2_guidance_h_rc_sp.psi = 0;
+
+ INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum);
+
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
(from rev 6003, paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
2010-09-28 14:04:47 UTC (rev 6004)
@@ -0,0 +1,76 @@
+/*
+ * $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 BOOZ2_GUIDANCE_H_H
+#define BOOZ2_GUIDANCE_H_H
+
+
+#include "math/pprz_algebra_int.h"
+
+#include "booz2_guidance_h_ref.h"
+
+#define BOOZ2_GUIDANCE_H_MODE_KILL 0
+#define BOOZ2_GUIDANCE_H_MODE_RATE 1
+#define BOOZ2_GUIDANCE_H_MODE_ATTITUDE 2
+#define BOOZ2_GUIDANCE_H_MODE_HOVER 3
+#define BOOZ2_GUIDANCE_H_MODE_NAV 4
+
+
+extern uint8_t booz2_guidance_h_mode;
+
+/* horizontal setpoint in NED */
+/* Q_int32_xx_8 */
+extern struct Int32Vect2 booz2_guidance_h_pos_sp;
+extern int32_t booz2_guidance_h_psi_sp;
+extern struct Int32Vect2 booz2_guidance_h_pos_ref;
+extern struct Int32Vect2 booz2_guidance_h_speed_ref;
+extern struct Int32Vect2 booz2_guidance_h_accel_ref;
+
+extern struct Int32Vect2 booz2_guidance_h_pos_err;
+extern struct Int32Vect2 booz2_guidance_h_speed_err;
+extern struct Int32Vect2 booz2_guidance_h_pos_err_sum;
+extern struct Int32Vect2 booz2_guidance_h_nav_err;
+
+extern struct Int32Eulers booz2_guidance_h_rc_sp;
+extern struct Int32Vect2 booz2_guidance_h_command_earth;
+extern struct Int32Eulers booz2_guidance_h_command_body;
+
+extern int32_t booz2_guidance_h_pgain;
+extern int32_t booz2_guidance_h_dgain;
+extern int32_t booz2_guidance_h_igain;
+extern int32_t booz2_guidance_h_ngain;
+extern int32_t booz2_guidance_h_again;
+
+
+extern void booz2_guidance_h_init(void);
+extern void booz2_guidance_h_mode_changed(uint8_t new_mode);
+extern void booz2_guidance_h_read_rc(bool_t in_flight);
+extern void booz2_guidance_h_run(bool_t in_flight);
+
+
+#define booz2_guidance_h_SetKi(_val) { \
+ booz2_guidance_h_igain = _val; \
+ INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); \
+ }
+
+#endif /* BOOZ2_GUIDANCE_H_H */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
(from rev 6003,
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h_ref.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
2010-09-28 14:04:47 UTC (rev 6004)
@@ -0,0 +1,191 @@
+/*
+ * $Id: booz2_guidance_v_ref.h 4173 2009-09-18 11:57:21Z flixr $
+ *
+ * Copyright (C) 2008-2009 ENAC <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 BOOZ2_GUIDANCE_H_REF_H
+#define BOOZ2_GUIDANCE_H_REF_H
+
+#include "airframe.h"
+#include "inttypes.h"
+#include "math/pprz_algebra.h"
+#include "math/pprz_algebra_int.h"
+
+/* update frequency */
+#define B2_GH_FREQ_FRAC 9
+#define B2_GH_FREQ (1<<B2_GH_FREQ_FRAC)
+
+/* reference model accel in meters/sec2 (output) */
+/* Q23.8 : accuracy 0.0039 , range 8388km/s2 */
+/* int32_4_8_t */
+extern struct Int32Vect2 b2_gh_accel_ref;
+#define B2_GH_ACCEL_REF_FRAC 8
+
+/* reference model speed in meters/sec (output) */
+/* Q14.17 : accuracy 0.0000076 , range 16384m/s2 */
+extern struct Int32Vect2 b2_gh_speed_ref;
+#define B2_GH_SPEED_REF_FRAC (B2_GH_ACCEL_REF_FRAC + B2_GH_FREQ_FRAC)
+
+/* reference model position in meters (output) */
+/* Q37.26 : */
+extern struct Int64Vect2 b2_gh_pos_ref;
+#define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC)
+
+/* Saturations definition */
+#ifndef BOOZ2_GUIDANCE_H_REF_MAX_ACCEL
+#define BOOZ2_GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 )
+#endif
+#define B2_GH_MAX_ACCEL BFP_OF_REAL(BOOZ2_GUIDANCE_H_REF_MAX_ACCEL,
B2_GH_ACCEL_REF_FRAC)
+
+#ifndef BOOZ2_GUIDANCE_H_REF_MAX_SPEED
+#define BOOZ2_GUIDANCE_H_REF_MAX_SPEED ( 5. )
+#endif
+#define B2_GH_MAX_SPEED BFP_OF_REAL(BOOZ2_GUIDANCE_H_REF_MAX_SPEED,
B2_GH_SPEED_REF_FRAC)
+
+/* second order model natural frequency and damping */
+#ifndef BOOZ2_GUIDANCE_H_REF_OMEGA
+#define BOOZ2_GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
+#endif
+#ifndef BOOZ2_GUIDANCE_H_REF_ZETA
+#define BOOZ2_GUIDANCE_H_REF_ZETA 0.85
+#endif
+#define B2_GH_ZETA_OMEGA_FRAC 10
+#define B2_GH_ZETA_OMEGA
BFP_OF_REAL((BOOZ2_GUIDANCE_H_REF_ZETA*BOOZ2_GUIDANCE_H_REF_OMEGA),
B2_GH_ZETA_OMEGA_FRAC)
+#define B2_GH_OMEGA_2_FRAC 7
+#define B2_GH_OMEGA_2
BFP_OF_REAL((BOOZ2_GUIDANCE_H_REF_OMEGA*BOOZ2_GUIDANCE_H_REF_OMEGA),
B2_GH_OMEGA_2_FRAC)
+
+/* first order time constant */
+#define B2_GH_REF_THAU_F 0.5
+#define B2_GH_REF_INV_THAU_FRAC 16
+#define B2_GH_REF_INV_THAU BFP_OF_REAL((1./B2_GH_REF_THAU_F),
B2_GH_REF_INV_THAU_FRAC)
+
+#ifdef B2_GUIDANCE_H_C
+static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2
speed, struct Int32Vect2 accel);
+static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp);
+static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp);
+
+struct Int64Vect2 b2_gh_pos_ref;
+struct Int32Vect2 b2_gh_speed_ref;
+struct Int32Vect2 b2_gh_accel_ref;
+
+static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2
speed, struct Int32Vect2 accel) {
+ struct Int64Vect2 new_pos;
+ new_pos.x = ((int64_t)pos.x)<<(B2_GH_POS_REF_FRAC - INT32_POS_FRAC);
+ new_pos.y = ((int64_t)pos.y)<<(B2_GH_POS_REF_FRAC - INT32_POS_FRAC);
+ b2_gh_pos_ref = new_pos;
+ INT32_VECT2_RSHIFT(b2_gh_speed_ref, speed, (INT32_SPEED_FRAC -
B2_GH_SPEED_REF_FRAC));
+ INT32_VECT2_RSHIFT(b2_gh_accel_ref, accel, (INT32_ACCEL_FRAC -
B2_GH_ACCEL_REF_FRAC));
+}
+
+static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
+
+ VECT2_ADD(b2_gh_pos_ref, b2_gh_speed_ref);
+ VECT2_ADD(b2_gh_speed_ref, b2_gh_accel_ref);
+
+ // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos -
pos_sp)
+ struct Int32Vect2 speed;
+ INT32_VECT2_RSHIFT(speed, b2_gh_speed_ref, (B2_GH_SPEED_REF_FRAC -
B2_GH_ACCEL_REF_FRAC));
+ VECT2_SMUL(speed, speed, -2*B2_GH_ZETA_OMEGA);
+ INT32_VECT2_RSHIFT(speed, speed, B2_GH_ZETA_OMEGA_FRAC);
+ // compute pos error in pos_sp resolution
+ struct Int32Vect2 pos_err;
+ INT32_VECT2_RSHIFT(pos_err, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC -
INT32_POS_FRAC));
+ VECT2_DIFF(pos_err, pos_err, pos_sp);
+ // convert to accel resolution
+ INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC -
B2_GH_ACCEL_REF_FRAC));
+ // compute the "pos part" of accel
+ struct Int32Vect2 pos;
+ VECT2_SMUL(pos, pos_err, (-B2_GH_OMEGA_2));
+ INT32_VECT2_RSHIFT(pos, pos, B2_GH_OMEGA_2_FRAC);
+ // sum accel
+ VECT2_SUM(b2_gh_accel_ref, speed, pos);
+
+ /* Saturate accelerations */
+ VECT2_STRIM(b2_gh_accel_ref, -B2_GH_MAX_ACCEL, B2_GH_MAX_ACCEL);
+
+ /* Saturate speed and adjust acceleration accordingly */
+ if (b2_gh_speed_ref.x <= -B2_GH_MAX_SPEED) {
+ b2_gh_speed_ref.x = -B2_GH_MAX_SPEED;
+ if (b2_gh_accel_ref.x < 0)
+ b2_gh_accel_ref.x = 0;
+ }
+ else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) {
+ b2_gh_speed_ref.x = B2_GH_MAX_SPEED;
+ if (b2_gh_accel_ref.x > 0)
+ b2_gh_accel_ref.x = 0;
+ }
+ if (b2_gh_speed_ref.y <= -B2_GH_MAX_SPEED) {
+ b2_gh_speed_ref.y = -B2_GH_MAX_SPEED;
+ if (b2_gh_accel_ref.y < 0)
+ b2_gh_accel_ref.y = 0;
+ }
+ else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) {
+ b2_gh_speed_ref.y = B2_GH_MAX_SPEED;
+ if (b2_gh_accel_ref.y > 0)
+ b2_gh_accel_ref.y = 0;
+ }
+}
+
+
+static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
+
+ VECT2_ADD(b2_gh_pos_ref, b2_gh_speed_ref);
+ VECT2_ADD(b2_gh_speed_ref, b2_gh_accel_ref);
+
+ // compute speed error
+ struct Int32Vect2 speed_err;
+ INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC -
B2_GH_SPEED_REF_FRAC));
+ VECT2_DIFF(speed_err, b2_gh_speed_ref, speed_err);
+ // convert to accel resolution
+ INT32_VECT2_RSHIFT(speed_err, speed_err, (B2_GH_SPEED_REF_FRAC -
B2_GH_ACCEL_REF_FRAC));
+ // compute accel from speed_sp
+ VECT2_SMUL(b2_gh_accel_ref, speed_err, -B2_GH_REF_INV_THAU);
+ INT32_VECT2_RSHIFT(b2_gh_accel_ref, b2_gh_accel_ref,
B2_GH_REF_INV_THAU_FRAC);
+
+ /* Saturate accelerations */
+ VECT2_STRIM(b2_gh_accel_ref, -B2_GH_MAX_ACCEL, B2_GH_MAX_ACCEL);
+
+ /* Saturate speed and adjust acceleration accordingly */
+ if (b2_gh_speed_ref.x <= -B2_GH_MAX_SPEED) {
+ b2_gh_speed_ref.x = -B2_GH_MAX_SPEED;
+ if (b2_gh_accel_ref.x < 0)
+ b2_gh_accel_ref.x = 0;
+ }
+ else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) {
+ b2_gh_speed_ref.x = B2_GH_MAX_SPEED;
+ if (b2_gh_accel_ref.x > 0)
+ b2_gh_accel_ref.x = 0;
+ }
+ if (b2_gh_speed_ref.y <= -B2_GH_MAX_SPEED) {
+ b2_gh_speed_ref.y = -B2_GH_MAX_SPEED;
+ if (b2_gh_accel_ref.y < 0)
+ b2_gh_accel_ref.y = 0;
+ }
+ else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) {
+ b2_gh_speed_ref.y = B2_GH_MAX_SPEED;
+ if (b2_gh_accel_ref.y > 0)
+ b2_gh_accel_ref.y = 0;
+ }
+}
+
+#endif /* B2_GUIDANCE_H_C */
+
+#endif /* BOOZ2_GUIDANCE_H_REF_H */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
(from rev 6003, paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
2010-09-28 14:04:47 UTC (rev 6004)
@@ -0,0 +1,283 @@
+/*
+ * $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.
+ */
+
+#define B2_GUIDANCE_V_C
+#define B2_GUIDANCE_V_USE_REF
+#include "booz2_guidance_v.h"
+
+
+#include "booz_radio_control.h"
+#include "booz_stabilization.h"
+#include "ahrs.h"
+#include "booz_fms.h"
+#include "booz2_navigation.h"
+
+#include "ins.h"
+#include "math/pprz_algebra_int.h"
+
+#include "airframe.h"
+
+uint8_t booz2_guidance_v_mode;
+int32_t booz2_guidance_v_ff_cmd;
+int32_t booz2_guidance_v_fb_cmd;
+/* command output */
+int32_t booz2_guidance_v_delta_t;
+
+/* direct throttle from radio control */
+/* range 0:200 */
+int32_t booz2_guidance_v_rc_delta_t;
+/* vertical speed setpoint from radio control */
+/* Q12.19 : accuracy 0.0000019, range +/-4096 */
+int32_t booz2_guidance_v_rc_zd_sp;
+/* altitude setpoint in meter (input) */
+/* Q23.8 : accuracy 0.0039, range 8388km */
+int32_t booz2_guidance_v_z_sp;
+/* vertical speed setpoint in meter/s (input) */
+/* Q12.19 : accuracy 0.0000019, range +/-4096 */
+int32_t booz2_guidance_v_zd_sp;
+#define BOOZ2_GUIDANCE_V_ZD_SP_FRAC INT32_SPEED_FRAC
+
+/* altitude reference in meter */
+/* Q23.8 : accuracy 0.0039, range 8388km */
+int32_t booz2_guidance_v_z_ref;
+/* vertical speed reference in meter/s */
+/* Q12.19 : accuracy 0.0000038, range 4096 */
+int32_t booz2_guidance_v_zd_ref;
+/* vertical acceleration reference in meter/s^2 */
+/* Q21.10 : accuracy 0.0009766, range 2097152 */
+int32_t booz2_guidance_v_zdd_ref;
+
+int32_t booz2_guidance_v_kp;
+int32_t booz2_guidance_v_kd;
+int32_t booz2_guidance_v_ki;
+
+int32_t booz2_guidance_v_z_sum_err;
+
+
+#define Booz2GuidanceVSetRef(_pos, _speed, _accel) { \
+ b2_gv_set_ref(_pos, _speed, _accel); \
+ booz2_guidance_v_z_ref = _pos; \
+ booz2_guidance_v_zd_ref = _speed; \
+ booz2_guidance_v_zdd_ref = _accel; \
+ }
+
+
+static inline void run_hover_loop(bool_t in_flight);
+
+
+void booz2_guidance_v_init(void) {
+
+ booz2_guidance_v_mode = BOOZ2_GUIDANCE_V_MODE_KILL;
+
+ booz2_guidance_v_kp = BOOZ2_GUIDANCE_V_HOVER_KP;
+ booz2_guidance_v_kd = BOOZ2_GUIDANCE_V_HOVER_KD;
+ booz2_guidance_v_ki = BOOZ2_GUIDANCE_V_HOVER_KI;
+
+ booz2_guidance_v_z_sum_err = 0;
+
+ b2_gv_adapt_init();
+}
+
+
+void booz2_guidance_v_read_rc(void) {
+
+ // used in RC_DIRECT directly and as saturation in CLIMB and HOVER
+ booz2_guidance_v_rc_delta_t =
(int32_t)radio_control.values[RADIO_CONTROL_THROTTLE] * 200 / MAX_PPRZ;
+ // used in RC_CLIMB
+ booz2_guidance_v_rc_zd_sp = ((MAX_PPRZ/2) -
(int32_t)radio_control.values[RADIO_CONTROL_THROTTLE]) *
+ BOOZ2_GUIDANCE_V_RC_CLIMB_COEF;
+ DeadBand(booz2_guidance_v_rc_zd_sp, BOOZ2_GUIDANCE_V_RC_CLIMB_DEAD_BAND);
+
+}
+
+void booz2_guidance_v_mode_changed(uint8_t new_mode) {
+
+ if (new_mode == booz2_guidance_v_mode)
+ return;
+
+ // switch ( booz2_guidance_v_mode ) {
+ //
+ // }
+
+ switch (new_mode) {
+
+ case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB:
+ case BOOZ2_GUIDANCE_V_MODE_CLIMB:
+ case BOOZ2_GUIDANCE_V_MODE_HOVER:
+ case BOOZ2_GUIDANCE_V_MODE_NAV:
+ booz2_guidance_v_z_sum_err = 0;
+ Booz2GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
+ break;
+ default:
+ break;
+ }
+
+
+ booz2_guidance_v_mode = new_mode;
+
+}
+
+void booz2_guidance_v_notify_in_flight( bool_t in_flight) {
+ if (in_flight)
+ b2_gv_adapt_init();
+}
+
+
+void booz2_guidance_v_run(bool_t in_flight) {
+
+ // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
+ // AKA SUPERVISION and co
+ if (in_flight) {
+ // we should use something after the supervision!!! fuck!!!
+ int32_t cmd_hack = Chop(booz_stabilization_cmd[COMMAND_THRUST], 1, 200);
+ b2_gv_adapt_run(ins_ltp_accel.z, cmd_hack);
+ }
+ else {
+ // reset vertical filter until takeoff
+ //ins_vf_realign = TRUE;
+ }
+
+ switch (booz2_guidance_v_mode) {
+
+ case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT:
+ booz2_guidance_v_z_sp = ins_ltp_pos.z; // not sure why we do that
+ Booz2GuidanceVSetRef(ins_ltp_pos.z, 0, 0); // or that - mode enter should
take care of it ?
+ booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_rc_delta_t;
+ break;
+
+ case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB:
+ booz2_guidance_v_zd_sp = booz2_guidance_v_rc_zd_sp;
+ b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp);
+ run_hover_loop(in_flight);
+ booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
+ break;
+
+ case BOOZ2_GUIDANCE_V_MODE_CLIMB:
+#ifdef USE_FMS
+ if (fms.enabled && fms.input.v_mode == BOOZ2_GUIDANCE_V_MODE_CLIMB)
+ booz2_guidance_v_zd_sp = fms.input.v_sp.climb;
+#endif
+ b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp);
+ run_hover_loop(in_flight);
+ // saturate max authority with RC stick
+ booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t,
booz2_guidance_v_delta_t);
+ break;
+
+ case BOOZ2_GUIDANCE_V_MODE_HOVER:
+#ifdef USE_FMS
+ if (fms.enabled && fms.input.v_mode == BOOZ2_GUIDANCE_V_MODE_HOVER)
+ booz2_guidance_v_z_sp = fms.input.v_sp.height;
+#endif
+ b2_gv_update_ref_from_z_sp(booz2_guidance_v_z_sp);
+ run_hover_loop(in_flight);
+ // saturate max authority with RC stick
+ booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t,
booz2_guidance_v_delta_t);
+ break;
+
+ case BOOZ2_GUIDANCE_V_MODE_NAV:
+ {
+ if (vertical_mode == VERTICAL_MODE_ALT) {
+ booz2_guidance_v_z_sp = -nav_flight_altitude;
+ b2_gv_update_ref_from_z_sp(booz2_guidance_v_z_sp);
+ run_hover_loop(in_flight);
+ }
+ else if (vertical_mode == VERTICAL_MODE_CLIMB) {
+ booz2_guidance_v_zd_sp = -nav_climb;
+ b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp);
+ nav_flight_altitude = -booz2_guidance_v_z_sp;
+ run_hover_loop(in_flight);
+ }
+ else if (vertical_mode == VERTICAL_MODE_MANUAL) {
+ booz2_guidance_v_z_sp = -nav_flight_altitude; // For display only
+ booz2_guidance_v_delta_t = nav_throttle;
+ }
+ /* use rc limitation if available */
+ if (radio_control.status == RADIO_CONTROL_OK)
+ booz_stabilization_cmd[COMMAND_THRUST] = Min(
booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t);
+ else
+ booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
+ break;
+ }
+ default:
+ break;
+ }
+}
+
+
+#define FF_CMD_FRAC 18
+
+#define BOOZ2_MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC))
+
+static inline void run_hover_loop(bool_t in_flight) {
+
+ /* convert our reference to generic representation */
+ int64_t tmp = b2_gv_z_ref>>(B2_GV_Z_REF_FRAC - INT32_POS_FRAC);
+ booz2_guidance_v_z_ref = (int32_t)tmp;
+ booz2_guidance_v_zd_ref = b2_gv_zd_ref<<(INT32_SPEED_FRAC -
B2_GV_ZD_REF_FRAC);
+ booz2_guidance_v_zdd_ref = b2_gv_zdd_ref<<(INT32_ACCEL_FRAC -
B2_GV_ZDD_REF_FRAC);
+ /* compute the error to our reference */
+ int32_t err_z = ins_ltp_pos.z - booz2_guidance_v_z_ref;
+ Bound(err_z, BOOZ2_GUIDANCE_V_MIN_ERR_Z, BOOZ2_GUIDANCE_V_MAX_ERR_Z);
+ int32_t err_zd = ins_ltp_speed.z - booz2_guidance_v_zd_ref;
+ Bound(err_zd, BOOZ2_GUIDANCE_V_MIN_ERR_ZD, BOOZ2_GUIDANCE_V_MAX_ERR_ZD);
+
+ if (in_flight) {
+ booz2_guidance_v_z_sum_err += err_z;
+ Bound(booz2_guidance_v_z_sum_err, -BOOZ2_GUIDANCE_V_MAX_SUM_ERR,
BOOZ2_GUIDANCE_V_MAX_SUM_ERR);
+ }
+ else
+ booz2_guidance_v_z_sum_err = 0;
+
+ /* our nominal command : (g + zdd)*m */
+#ifdef BOOZ2_GUIDANCE_V_INV_M
+ const int32_t inv_m = BFP_OF_REAL(BOOZ2_GUIDANCE_V_INV_M,
B2_GV_ADAPT_X_FRAC);
+#else
+ const int32_t inv_m = b2_gv_adapt_X>>(B2_GV_ADAPT_X_FRAC - FF_CMD_FRAC);
+#endif
+ const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
+ (booz2_guidance_v_zdd_ref<<(FF_CMD_FRAC -
INT32_ACCEL_FRAC));
+#if 0
+ if (g_m_zdd > 0)
+ booz2_guidance_v_ff_cmd = ( g_m_zdd + (inv_m>>1)) / inv_m;
+ else
+ booz2_guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m;
+#else
+ booz2_guidance_v_ff_cmd = g_m_zdd / inv_m;
+ int32_t cphi,ctheta,cphitheta;
+ PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi);
+ PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta);
+ cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
+ if (cphitheta < BOOZ2_MAX_BANK_COEF) cphitheta = BOOZ2_MAX_BANK_COEF;
+ booz2_guidance_v_ff_cmd = (booz2_guidance_v_ff_cmd << INT32_TRIG_FRAC) /
cphitheta;
+#endif
+
+ /* our error command */
+ booz2_guidance_v_fb_cmd = ((-booz2_guidance_v_kp * err_z) >> 12) +
+ ((-booz2_guidance_v_kd * err_zd) >> 21) +
+ ((-booz2_guidance_v_ki *
booz2_guidance_v_z_sum_err) >> 21);
+
+ booz2_guidance_v_delta_t = booz2_guidance_v_ff_cmd + booz2_guidance_v_fb_cmd;
+ // booz2_guidance_v_delta_t = booz2_guidance_v_fb_cmd;
+
+
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
(from rev 6003, paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
2010-09-28 14:04:47 UTC (rev 6004)
@@ -0,0 +1,67 @@
+/*
+ * $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 BOOZ2_GUIDANCE_V
+#define BOOZ2_GUIDANCE_V
+
+#include "std.h"
+
+#include "booz2_guidance_v_ref.h"
+#include "booz2_guidance_v_adpt.h"
+
+#define BOOZ2_GUIDANCE_V_MODE_KILL 0
+#define BOOZ2_GUIDANCE_V_MODE_RC_DIRECT 1
+#define BOOZ2_GUIDANCE_V_MODE_RC_CLIMB 2
+#define BOOZ2_GUIDANCE_V_MODE_CLIMB 3
+#define BOOZ2_GUIDANCE_V_MODE_HOVER 4
+#define BOOZ2_GUIDANCE_V_MODE_NAV 5
+
+extern uint8_t booz2_guidance_v_mode;
+
+extern int32_t booz2_guidance_v_z_sp;
+extern int32_t booz2_guidance_v_zd_sp;
+extern int32_t booz2_guidance_v_z_ref;
+extern int32_t booz2_guidance_v_zd_ref;
+extern int32_t booz2_guidance_v_zdd_ref;
+extern int32_t booz2_guidance_v_z_sum_err;
+extern int32_t booz2_guidance_v_ff_cmd;
+extern int32_t booz2_guidance_v_fb_cmd;
+extern int32_t booz2_guidance_v_delta_t;
+
+extern int32_t booz2_guidance_v_kp;
+extern int32_t booz2_guidance_v_kd;
+extern int32_t booz2_guidance_v_ki;
+
+extern void booz2_guidance_v_init(void);
+extern void booz2_guidance_v_read_rc(void);
+extern void booz2_guidance_v_mode_changed(uint8_t new_mode);
+extern void booz2_guidance_v_notify_in_flight(bool_t in_flight);
+extern void booz2_guidance_v_run(bool_t in_flight);
+
+#define booz2_guidance_v_SetKi(_val) { \
+ booz2_guidance_v_ki = _val; \
+ booz2_guidance_v_z_sum_err = 0; \
+ }
+
+
+#endif /* BOOZ2_GUIDANCE_V */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
(from rev 6003,
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h)
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
(rev 0)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
2010-09-28 14:04:47 UTC (rev 6004)
@@ -0,0 +1,147 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2009-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.
+ *
+ *
+ * Adaptation bloc of the vertical guidance
+ * This is a dimension one kalman filter estimating
+ * the ratio of vertical acceleration over thrust command ( ~ invert of the
mass )
+ * needed by the invert dynamic model to produce a nominal command
+ */
+
+#ifndef BOOZ2_GUIDANCE_V_ADPT
+#define BOOZ2_GUIDANCE_V_ADPT
+
+extern int32_t b2_gv_adapt_X;
+extern int32_t b2_gv_adapt_P;
+extern int32_t b2_gv_adapt_Xmeas;
+
+
+#ifdef B2_GUIDANCE_V_C
+
+/* Our State
+ Q13.18
+*/
+int32_t b2_gv_adapt_X;
+#define B2_GV_ADAPT_X_FRAC 18
+
+/* Our covariance
+ Q13.18
+*/
+int32_t b2_gv_adapt_P;
+#define B2_GV_ADAPT_P_FRAC 18
+
+/* Our measurement */
+int32_t b2_gv_adapt_Xmeas;
+
+
+/* Initial State and Covariance */
+#define B2_GV_ADAPT_X0_F 0.15
+#define B2_GV_ADAPT_X0 BFP_OF_REAL(B2_GV_ADAPT_X0_F, B2_GV_ADAPT_X_FRAC)
+#define B2_GV_ADAPT_P0_F 0.5
+#define B2_GV_ADAPT_P0 BFP_OF_REAL(B2_GV_ADAPT_P0_F, B2_GV_ADAPT_P_FRAC)
+
+/* System and Measuremement noises */
+#define B2_GV_ADAPT_SYS_NOISE_F 0.00005
+#define B2_GV_ADAPT_SYS_NOISE BFP_OF_REAL(B2_GV_ADAPT_SYS_NOISE_F,
B2_GV_ADAPT_P_FRAC)
+
+#ifndef USE_ADAPT_HOVER
+
+#define B2_GV_ADAPT_MEAS_NOISE_F 2.0
+#define B2_GV_ADAPT_MEAS_NOISE BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_F,
B2_GV_ADAPT_P_FRAC)
+
+#else /* USE_ADAPT_HOVER */
+
+#define B2_GV_ADAPT_MEAS_NOISE_HOVER_F 10.0
+#define B2_GV_ADAPT_MEAS_NOISE_HOVER
BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_HOVER_F, B2_GV_ADAPT_P_FRAC)
+#define B2_GV_ADAPT_MEAS_NOISE_F 50.0
+#define B2_GV_ADAPT_MEAS_NOISE BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_F,
B2_GV_ADAPT_P_FRAC)
+
+#define B2_GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(4.0)
+#define B2_GV_ADAPT_HOVER_ACCEL ACCEL_BFP_OF_REAL(1.0)
+#define B2_GV_ADAPT_MAX_CMD 180
+#define B2_GV_ADAPT_MIN_CMD 20
+#define B2_GV_ADAPT_HOVER_MAX_CMD 120
+#define B2_GV_ADAPT_HOVER_MIN_CMD 60
+#endif
+
+static inline void b2_gv_adapt_init(void) {
+ b2_gv_adapt_X = B2_GV_ADAPT_X0;
+ b2_gv_adapt_P = B2_GV_ADAPT_P0;
+}
+
+/*
+ zdd_meas : INT32_ACCEL_FRAC
+ thrust_applied : controller input [2-200]
+*/
+#define K_FRAC 12
+static inline void b2_gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
+
+ /* Do you really think we want to divide by zero ? */
+ if (thrust_applied == 0) return;
+ /* We don't propagate state, it's constant ! */
+ /* We propagate our covariance */
+ b2_gv_adapt_P = b2_gv_adapt_P + B2_GV_ADAPT_SYS_NOISE;
+ /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less
than 24 bits */
+ const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) -
zdd_meas)<<(B2_GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
+#ifdef USE_ADAPT_HOVER
+ /* Update only if accel and commands are in a valid range */
+ if (thrust_applied < B2_GV_ADAPT_MIN_CMD || thrust_applied >
B2_GV_ADAPT_MAX_CMD
+ || zdd_meas < -B2_GV_ADAPT_MAX_ACCEL || zdd_meas > B2_GV_ADAPT_MAX_ACCEL)
+ return;
+#endif
+ if ( g_m_zdd > 0)
+ b2_gv_adapt_Xmeas = (g_m_zdd + (thrust_applied>>1)) / thrust_applied;
+ else
+ b2_gv_adapt_Xmeas = (g_m_zdd - (thrust_applied>>1)) / thrust_applied;
+ /* Compute a residual */
+ int32_t residual = b2_gv_adapt_Xmeas - b2_gv_adapt_X;
+ /* Covariance Error */
+ int32_t E = 0;
+#ifdef USE_ADAPT_HOVER
+ if ((thrust_applied > B2_GV_ADAPT_HOVER_MIN_CMD && thrust_applied <
B2_GV_ADAPT_HOVER_MAX_CMD) ||
+ (zdd_meas > -B2_GV_ADAPT_HOVER_ACCEL && zdd_meas <
B2_GV_ADAPT_HOVER_ACCEL))
+ E = b2_gv_adapt_P + B2_GV_ADAPT_MEAS_NOISE_HOVER;
+ else
+#endif
+ E = b2_gv_adapt_P + B2_GV_ADAPT_MEAS_NOISE;
+ /* Kalman gain */
+ int32_t K = (b2_gv_adapt_P<<K_FRAC) / E;
+ /* Update Covariance */
+ b2_gv_adapt_P = b2_gv_adapt_P - ((K * b2_gv_adapt_P)>>K_FRAC);
+ /* Don't let covariance climb over initial value */
+ if (b2_gv_adapt_P > B2_GV_ADAPT_P0) b2_gv_adapt_P = B2_GV_ADAPT_P0;
+ /* Update State */
+ b2_gv_adapt_X = b2_gv_adapt_X + ((K * residual)>>K_FRAC);
+ /* Again don't let it climb over a value that would
+ give less than zero throttle and don't let it down to zero.
+ 30254 = MAX_ACCEL*B2_GV_ADAPT_X_FRAC/MAX_THROTTLE
+ aka
+ 30254 = 3*9.81*2^8/255
+ 2571632 = 9.81*2^18
+ */
+ Bound(b2_gv_adapt_X, 10000, 2571632);
+}
+
+
+#endif /* B2_GUIDANCE_V_C */
+
+#endif /* BOOZ2_GUIDANCE_V_ADPT */
Copied:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
(from rev 6003,
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_ref.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
2010-09-28 14:04:47 UTC (rev 6004)
@@ -0,0 +1,165 @@
+/*
+ * $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 BOOZ2_GUIDANCE_V_REF_H
+#define BOOZ2_GUIDANCE_V_REF_H
+
+#include "airframe.h"
+#include "inttypes.h"
+#include "math/pprz_algebra.h"
+#include "math/pprz_algebra_int.h"
+
+/* update frequency */
+#define B2_GV_FREQ_FRAC 9
+#define B2_GV_FREQ (1<<B2_GV_FREQ_FRAC)
+
+/* reference model vaccel in meters/sec2 (output) */
+/* Q23.8 : accuracy 0.0039 , range 8388km/s2 */
+/* int32_4_8_t */
+extern int32_t b2_gv_zdd_ref;
+#define B2_GV_ZDD_REF_FRAC 8
+
+/* reference model vspeed in meters/sec (output) */
+/* Q14.17 : accuracy 0.0000076 , range 16384m/s2 */
+extern int32_t b2_gv_zd_ref;
+#define B2_GV_ZD_REF_FRAC (B2_GV_ZDD_REF_FRAC + B2_GV_FREQ_FRAC)
+
+/* reference model altitude in meters (output) */
+/* Q37.26 : */
+extern int64_t b2_gv_z_ref;
+#define B2_GV_Z_REF_FRAC (B2_GV_ZD_REF_FRAC + B2_GV_FREQ_FRAC)
+
+/* Saturations definition */
+#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZDD
+#define BOOZ2_GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
+#endif
+#define B2_GV_MIN_ZDD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MIN_ZDD,
B2_GV_ZDD_REF_FRAC)
+
+#ifndef BOOZ2_GUIDANCE_V_REF_MAX_ZDD
+#define BOOZ2_GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
+#endif
+#define B2_GV_MAX_ZDD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MAX_ZDD,
B2_GV_ZDD_REF_FRAC)
+
+#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZD
+#define BOOZ2_GUIDANCE_V_REF_MIN_ZD (-3.)
+#endif
+#define B2_GV_MIN_ZD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MIN_ZD ,
B2_GV_ZD_REF_FRAC)
+
+#ifndef BOOZ2_GUIDANCE_V_REF_MAX_ZD
+#define BOOZ2_GUIDANCE_V_REF_MAX_ZD ( 3.)
+#endif
+#define B2_GV_MAX_ZD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MAX_ZD ,
B2_GV_ZD_REF_FRAC)
+
+/* second order model natural frequency and damping */
+#ifndef BOOZ2_GUIDANCE_V_REF_OMEGA
+#define BOOZ2_GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
+#endif
+#ifndef BOOZ2_GUIDANCE_V_REF_ZETA
+#define BOOZ2_GUIDANCE_V_REF_ZETA 0.85
+#endif
+#define B2_GV_ZETA_OMEGA_FRAC 10
+#define B2_GV_ZETA_OMEGA
BFP_OF_REAL((BOOZ2_GUIDANCE_V_REF_ZETA*BOOZ2_GUIDANCE_V_REF_OMEGA),
B2_GV_ZETA_OMEGA_FRAC)
+#define B2_GV_OMEGA_2_FRAC 7
+#define B2_GV_OMEGA_2
BFP_OF_REAL((BOOZ2_GUIDANCE_V_REF_OMEGA*BOOZ2_GUIDANCE_V_REF_OMEGA),
B2_GV_OMEGA_2_FRAC)
+
+/* first order time constant */
+#define B2_GV_REF_THAU_F 0.25
+#define B2_GV_REF_INV_THAU_FRAC 16
+#define B2_GV_REF_INV_THAU BFP_OF_REAL((1./0.25), B2_GV_REF_INV_THAU_FRAC)
+
+#ifdef B2_GUIDANCE_V_C
+static inline void b2_gv_set_ref(int32_t alt, int32_t speed, int32_t accel);
+static inline void b2_gv_update_ref_from_z_sp(int32_t z_sp);
+static inline void b2_gv_update_ref_from_zd_sp(int32_t zd_sp);
+
+int64_t b2_gv_z_ref;
+int32_t b2_gv_zd_ref;
+int32_t b2_gv_zdd_ref;
+
+static inline void b2_gv_set_ref(int32_t alt, int32_t speed, int32_t accel) {
+ int64_t new_z = ((int64_t)alt)<<(B2_GV_Z_REF_FRAC - INT32_POS_FRAC);
+ b2_gv_z_ref = new_z;
+ b2_gv_zd_ref = speed>>(INT32_SPEED_FRAC - B2_GV_ZD_REF_FRAC);
+ b2_gv_zdd_ref = accel>>(INT32_ACCEL_FRAC - B2_GV_ZDD_REF_FRAC);
+}
+
+static inline void b2_gv_update_ref_from_z_sp(int32_t z_sp) {
+
+ b2_gv_z_ref += b2_gv_zd_ref;
+ b2_gv_zd_ref += b2_gv_zdd_ref;
+
+ // compute the "speed part" of zdd = -2*zeta*omega*zd -omega^2(z_sp - z)
+ int32_t zd_zdd_res = b2_gv_zd_ref>>(B2_GV_ZD_REF_FRAC - B2_GV_ZDD_REF_FRAC);
+ int32_t zdd_speed =
((int32_t)(-2*B2_GV_ZETA_OMEGA)*zd_zdd_res)>>(B2_GV_ZETA_OMEGA_FRAC);
+ // compute z error in z_sp resolution
+ int32_t z_err_sp = z_sp -
(int32_t)(b2_gv_z_ref>>(B2_GV_Z_REF_FRAC-INT32_POS_FRAC));
+ // convert to accel resolution
+ int32_t z_err_accel = z_err_sp>>(INT32_POS_FRAC-B2_GV_ZDD_REF_FRAC);
+ int32_t zdd_pos = ((int32_t)(B2_GV_OMEGA_2)*z_err_accel)>>B2_GV_OMEGA_2_FRAC;
+ b2_gv_zdd_ref = zdd_speed + zdd_pos;
+
+ /* Saturate accelerations */
+ Bound(b2_gv_zdd_ref, B2_GV_MIN_ZDD, B2_GV_MAX_ZDD);
+
+ /* Saturate speed and adjust acceleration accordingly */
+ if (b2_gv_zd_ref <= B2_GV_MIN_ZD) {
+ b2_gv_zd_ref = B2_GV_MIN_ZD;
+ if (b2_gv_zdd_ref < 0)
+ b2_gv_zdd_ref = 0;
+ }
+ else if (b2_gv_zd_ref >= B2_GV_MAX_ZD) {
+ b2_gv_zd_ref = B2_GV_MAX_ZD;
+ if (b2_gv_zdd_ref > 0)
+ b2_gv_zdd_ref = 0;
+ }
+}
+
+
+static inline void b2_gv_update_ref_from_zd_sp(int32_t zd_sp) {
+
+ b2_gv_z_ref += b2_gv_zd_ref;
+ b2_gv_zd_ref += b2_gv_zdd_ref;
+
+ int32_t zd_err = b2_gv_zd_ref - (zd_sp>>(INT32_SPEED_FRAC -
B2_GV_ZD_REF_FRAC));
+ int32_t zd_err_zdd_res = zd_err>>(B2_GV_ZD_REF_FRAC-B2_GV_ZDD_REF_FRAC);
+ b2_gv_zdd_ref = (-(int32_t)B2_GV_REF_INV_THAU *
zd_err_zdd_res)>>B2_GV_REF_INV_THAU_FRAC;
+
+ /* Saturate accelerations */
+ Bound(b2_gv_zdd_ref, B2_GV_MIN_ZDD, B2_GV_MAX_ZDD);
+
+ /* Saturate speed and adjust acceleration accordingly */
+ if (b2_gv_zd_ref <= B2_GV_MIN_ZD) {
+ b2_gv_zd_ref = B2_GV_MIN_ZD;
+ if (b2_gv_zdd_ref < 0)
+ b2_gv_zdd_ref = 0;
+ }
+ else if (b2_gv_zd_ref >= B2_GV_MAX_ZD) {
+ b2_gv_zd_ref = B2_GV_MAX_ZD;
+ if (b2_gv_zdd_ref > 0)
+ b2_gv_zdd_ref = 0;
+ }
+}
+
+#endif /* B2_GUIDANCE_V_C */
+
+#endif /* BOOZ2_GUIDANCE_V_REF_H */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance.h (from rev
6003, paparazzi3/trunk/sw/airborne/booz/booz_guidance.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance.h
2010-09-28 14:04:47 UTC (rev 6004)
@@ -0,0 +1,7 @@
+#ifndef BOOZ_GUIDANCE_H
+#define BOOZ_GUIDANCE_H
+
+#include "guidance/booz2_guidance_h.h"
+#include "guidance/booz2_guidance_v.h"
+
+#endif /* BOOZ_GUIDANCE_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6004] moving guidance, rename still to be done,
Felix Ruess <=