[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4173] remove trailing whitespaces
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [4173] remove trailing whitespaces |
|
Date: |
Fri, 18 Sep 2009 11:57:21 +0000 |
Revision: 4173
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4173
Author: flixr
Date: 2009-09-18 11:57:21 +0000 (Fri, 18 Sep 2009)
Log Message:
-----------
remove trailing whitespaces
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c
paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c
paparazzi3/trunk/sw/airborne/booz/booz2_debug.c
paparazzi3/trunk/sw/airborne/booz/booz2_debug.h
paparazzi3/trunk/sw/airborne/booz/booz2_gps.c
paparazzi3/trunk/sw/airborne/booz/booz_stabilization.c
paparazzi3/trunk/sw/airborne/booz/booz_stabilization.h
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.h
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_ref.h
paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.c
Modified: paparazzi3/trunk/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c
2009-09-18 11:57:13 UTC (rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c
2009-09-18 11:57:21 UTC (rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#include "booz2_analog.h"
@@ -50,8 +50,8 @@
AD0CR |= 4 << 24;
- /* clear match 1 */
- T0EMR &= ~TEMR_EM1;
+ /* clear match 1 */
+ T0EMR &= ~TEMR_EM1;
/* set high on match 1 */
T0EMR |= TEMR_EMC1_2;
/* first match in a while */
@@ -74,8 +74,8 @@
AD1CR |= 5 << 24;
- /* clear match 2 */
- T0EMR &= ~TEMR_EM3;
+ /* clear match 2 */
+ T0EMR &= ~TEMR_EM3;
/* set high on match 2 */
T0EMR |= TEMR_EMC3_2;
/* first match in a while */
@@ -94,7 +94,7 @@
/* trigger next convertion */
T0MR1 += BOOZ2_ANALOG_BATTERY_PERIOD;
/* lower clock */
- T0EMR &= ~TEMR_EM1;
+ T0EMR &= ~TEMR_EM1;
VICVectAddr = 0x00000000; // clear this interrupt from the
VIC
ISR_EXIT(); // recover registers and return
}
@@ -107,7 +107,7 @@
/* trigger next convertion */
T0MR3 += BOOZ2_ANALOG_BARO_PERIOD;
/* lower clock */
- T0EMR &= ~TEMR_EM3;
+ T0EMR &= ~TEMR_EM3;
VICVectAddr = 0x00000000; // clear this interrupt from the
VIC
ISR_EXIT(); // recover registers and return
}
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c 2009-09-18 11:57:13 UTC
(rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c 2009-09-18 11:57:21 UTC
(rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#define DATALINK_C
@@ -44,13 +44,13 @@
void dl_parse_msg(void) {
uint8_t msg_id = IdOfMsg(dl_buffer);
switch (msg_id) {
-
+
case DL_PING:
{
DOWNLINK_SEND_PONG(DefaultChannel);
}
break;
-
+
case DL_SETTING :
{
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break;
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_debug.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_debug.c 2009-09-18 11:57:13 UTC
(rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_debug.c 2009-09-18 11:57:21 UTC
(rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#include "booz2_debug.h"
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_debug.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_debug.h 2009-09-18 11:57:13 UTC
(rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_debug.h 2009-09-18 11:57:21 UTC
(rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_DEBUG_H
@@ -40,12 +40,12 @@
#define DEBUG_LINK_MCU_IMU 3
-#define ASSERT(cond, mod, err) { \
- if (!(cond)) { \
- booz_debug_mod = mod; \
- booz_debug_err = err; \
+#define ASSERT(cond, mod, err) { \
+ if (!(cond)) { \
+ booz_debug_mod = mod; \
+ booz_debug_err = err; \
DOWNLINK_SEND_BOOZ_ERROR(&booz_debug_mod, &booz_debug_err); \
- } \
+ } \
}
#else
#define ASSERT(cond, mod, err) {}
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_gps.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_gps.c 2009-09-18 11:57:13 UTC
(rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_gps.c 2009-09-18 11:57:21 UTC
(rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#include "booz2_gps.h"
@@ -137,7 +137,7 @@
case GOT_CLASS:
ubx_id = c;
ubx_status++;
- break;
+ break;
case GOT_ID:
ubx_len = c;
ubx_status++;
@@ -169,7 +169,7 @@
break;
}
return;
- error:
+ error:
restart:
ubx_status = UNINIT;
return;
Modified: paparazzi3/trunk/sw/airborne/booz/booz_stabilization.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_stabilization.c 2009-09-18
11:57:13 UTC (rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/booz_stabilization.c 2009-09-18
11:57:21 UTC (rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#include "booz_stabilization.h"
Modified: paparazzi3/trunk/sw/airborne/booz/booz_stabilization.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_stabilization.h 2009-09-18
11:57:13 UTC (rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/booz_stabilization.h 2009-09-18
11:57:21 UTC (rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_H
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
2009-09-18 11:57:13 UTC (rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
2009-09-18 11:57:21 UTC (rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#include "booz2_guidance_h.h"
@@ -92,13 +92,13 @@
// booz_stabilization_rate_exit();
// break;
}
-
+
switch (new_mode) {
case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
booz_stabilization_attitude_enter();
break;
-
+
case BOOZ2_GUIDANCE_H_MODE_HOVER:
booz2_guidance_h_hover_enter();
break;
@@ -110,17 +110,17 @@
}
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);
#ifdef USE_FMS
@@ -128,7 +128,7 @@
BOOZ_STABILIZATION_ATTITUDE_ADD_SP(fms.input.h_sp.attitude);
#endif
break;
-
+
case BOOZ2_GUIDANCE_H_MODE_HOVER:
#ifdef USE_FMS
if (fms.enabled && fms.input.h_mode >= BOOZ2_GUIDANCE_H_MODE_HOVER)
@@ -136,7 +136,7 @@
#endif
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);
@@ -161,12 +161,12 @@
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();
@@ -188,7 +188,7 @@
booz_stabilization_attitude_run(in_flight);
break;
}
-
+
}
@@ -214,34 +214,34 @@
VECT2_COPY(booz2_guidance_h_speed_err, booz_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_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);
+ 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, booz_ahrs.ltp_to_body_euler.psi);
- PPRZ_ITRIG_COS(c_psi, booz_ahrs.ltp_to_body_euler.psi);
+ PPRZ_ITRIG_SIN(s_psi, booz_ahrs.ltp_to_body_euler.psi);
+ PPRZ_ITRIG_COS(c_psi, booz_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 =
+ 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 =
+ 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);
@@ -272,7 +272,7 @@
VECT2_COPY(booz2_guidance_h_speed_err, booz_ins_ltp_speed);
/* 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
@@ -318,13 +318,13 @@
/* Rotate to body frame */
int32_t s_psi, c_psi;
- PPRZ_ITRIG_SIN(s_psi, booz_ahrs.ltp_to_body_euler.psi);
- PPRZ_ITRIG_COS(c_psi, booz_ahrs.ltp_to_body_euler.psi);
+ PPRZ_ITRIG_SIN(s_psi, booz_ahrs.ltp_to_body_euler.psi);
+ PPRZ_ITRIG_COS(c_psi, booz_ahrs.ltp_to_body_euler.psi);
// Restore angle ref resolution after rotation
- booz2_guidance_h_command_body.phi =
+ 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 =
+ 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));
booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi;
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h
2009-09-18 11:57:13 UTC (rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.h
2009-09-18 11:57:21 UTC (rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_GUIDANCE_H_H
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
2009-09-18 11:57:13 UTC (rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
2009-09-18 11:57:21 UTC (rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#define B2_GUIDANCE_V_C
@@ -106,18 +106,18 @@
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:
@@ -130,7 +130,7 @@
}
-
+
booz2_guidance_v_mode = new_mode;
}
@@ -154,7 +154,7 @@
// reset vertical filter until takeoff
//booz_ins_vff_realign = TRUE;
}
-
+
switch (booz2_guidance_v_mode) {
case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT:
@@ -248,10 +248,10 @@
#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) -
+ 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)
+ 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;
@@ -259,9 +259,9 @@
booz2_guidance_v_ff_cmd = g_m_zdd / inv_m;
#endif
// booz2_guidance_v_ff_cmd = BOOZ2_GUIDANCE_V_HOVER_POWER;
-
+
/* our error command */
- booz2_guidance_v_fb_cmd = ((-booz2_guidance_v_kp * err_z) >> 12) +
+ 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);
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.h
2009-09-18 11:57:13 UTC (rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.h
2009-09-18 11:57:21 UTC (rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_GUIDANCE_V
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
2009-09-18 11:57:13 UTC (rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_adpt.h
2009-09-18 11:57:21 UTC (rev 4173)
@@ -1,8 +1,8 @@
/*
* $Id$
*
- * Copyright (C) 2009 Pascal Brisset <address@hidden>,
- * Antoine Drouin <address@hidden>,
+ * Copyright (C) 2009 Pascal Brisset <address@hidden>,
+ * Antoine Drouin <address@hidden>,
* Gautier Hattenberger <address@hidden>
*
* This file is part of paparazzi.
@@ -20,7 +20,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*
*
* Adaptation bloc of the vertical guidance
@@ -39,14 +39,14 @@
#ifdef B2_GUIDANCE_V_C
-/* Our State
- Q13.18
+/* Our State
+ Q13.18
*/
int32_t b2_gv_adapt_X;
#define B2_GV_ADAPT_X_FRAC 18
/* Our covariance
- Q13.18
+ Q13.18
*/
int32_t b2_gv_adapt_P;
#define B2_GV_ADAPT_P_FRAC 18
@@ -67,7 +67,7 @@
#ifndef USE_ADAPT_HOVER
-#define B2_GV_ADAPT_MEAS_NOISE_F 2.0
+#define B2_GV_ADAPT_MEAS_NOISE_F 2.0
#define B2_GV_ADAPT_MEAS_NOISE_HOVER
BFP_OF_REAL(B2_GV_ADAPT_MEAS_NOISE_HOVER_F, B2_GV_ADAPT_P_FRAC)
#else /* USE_ADAPT_HOVER */
@@ -129,17 +129,17 @@
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 */
+ /* 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
+ /* 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);
}
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_ref.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_ref.h
2009-09-18 11:57:13 UTC (rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v_ref.h
2009-09-18 11:57:21 UTC (rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_GUIDANCE_V_REF_H
@@ -85,7 +85,7 @@
/* 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)
+#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);
@@ -107,7 +107,7 @@
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);
@@ -120,7 +120,7 @@
/* 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;
@@ -146,7 +146,7 @@
/* 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;
Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.c 2009-09-18
11:57:13 UTC (rev 4172)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.c 2009-09-18
11:57:21 UTC (rev 4173)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*/
#include "booz2_vf_float.h"
@@ -27,9 +27,9 @@
X = [ z zdot bias ]
-temps :
+temps :
propagate 86us
- update 46us
+ update 46us
*/
/* initial covariance diagonal */
@@ -56,7 +56,7 @@
b2_vff_bias = init_bias;
int i, j;
for (i=0; i<B2_VFF_STATE_SIZE; i++) {
- for (j=0; j<B2_VFF_STATE_SIZE; j++)
+ for (j=0; j<B2_VFF_STATE_SIZE; j++)
b2_vff_P[i][j] = 0.;
b2_vff_P[i][i] = INIT_PXX;
}
@@ -69,15 +69,15 @@
F = [ 1 dt -dt^2/2
0 1 -dt
0 0 1 ];
-
+
B = [ dt^2/2 dt 0]';
-
+
Q = [ 0.01 0 0
0 0.01 0
0 0 0.001 ];
-
+
Xk1 = F * Xk0 + B * accel;
-
+
Pk1 = F * Pk0 * F' + Q;
*/
@@ -85,18 +85,18 @@
/* update state */
b2_vff_zdotdot = accel + 9.81 - b2_vff_bias;
b2_vff_z = b2_vff_z + DT_VFILTER * b2_vff_zdot;
- b2_vff_zdot = b2_vff_zdot + DT_VFILTER * b2_vff_zdotdot;
+ b2_vff_zdot = b2_vff_zdot + DT_VFILTER * b2_vff_zdotdot;
/* update covariance */
- const float FPF00 = b2_vff_P[0][0] + DT_VFILTER * ( b2_vff_P[1][0] +
b2_vff_P[0][1] + DT_VFILTER * b2_vff_P[1][1] );
+ const float FPF00 = b2_vff_P[0][0] + DT_VFILTER * ( b2_vff_P[1][0] +
b2_vff_P[0][1] + DT_VFILTER * b2_vff_P[1][1] );
const float FPF01 = b2_vff_P[0][1] + DT_VFILTER * ( b2_vff_P[1][1] -
b2_vff_P[0][2] - DT_VFILTER * b2_vff_P[1][2] );
const float FPF02 = b2_vff_P[0][2] + DT_VFILTER * ( b2_vff_P[1][2] );
- const float FPF10 = b2_vff_P[1][0] + DT_VFILTER * (-b2_vff_P[2][0] +
b2_vff_P[1][1] - DT_VFILTER * b2_vff_P[2][1] );
- const float FPF11 = b2_vff_P[1][1] + DT_VFILTER * (-b2_vff_P[2][1] -
b2_vff_P[1][2] + DT_VFILTER * b2_vff_P[2][2] );
+ const float FPF10 = b2_vff_P[1][0] + DT_VFILTER * (-b2_vff_P[2][0] +
b2_vff_P[1][1] - DT_VFILTER * b2_vff_P[2][1] );
+ const float FPF11 = b2_vff_P[1][1] + DT_VFILTER * (-b2_vff_P[2][1] -
b2_vff_P[1][2] + DT_VFILTER * b2_vff_P[2][2] );
const float FPF12 = b2_vff_P[1][2] + DT_VFILTER * (-b2_vff_P[2][2] );
const float FPF20 = b2_vff_P[2][0] + DT_VFILTER * ( b2_vff_P[2][1] );
const float FPF21 = b2_vff_P[2][1] + DT_VFILTER * (-b2_vff_P[2][2] );
const float FPF22 = b2_vff_P[2][2];
-
+
b2_vff_P[0][0] = FPF00 + Qzz;
b2_vff_P[0][1] = FPF01;
b2_vff_P[0][2] = FPF02;
@@ -127,14 +127,14 @@
const float y = z_meas - b2_vff_z;
const float S = b2_vff_P[0][0] + R;
- const float K1 = b2_vff_P[0][0] * 1/S;
- const float K2 = b2_vff_P[1][0] * 1/S;
- const float K3 = b2_vff_P[2][0] * 1/S;
-
- b2_vff_z = b2_vff_z + K1 * y;
- b2_vff_zdot = b2_vff_zdot + K2 * y;
- b2_vff_bias = b2_vff_bias + K3 * y;
+ const float K1 = b2_vff_P[0][0] * 1/S;
+ const float K2 = b2_vff_P[1][0] * 1/S;
+ const float K3 = b2_vff_P[2][0] * 1/S;
+ b2_vff_z = b2_vff_z + K1 * y;
+ b2_vff_zdot = b2_vff_zdot + K2 * y;
+ b2_vff_bias = b2_vff_bias + K3 * y;
+
const float P11 = (1. - K1) * b2_vff_P[0][0];
const float P12 = (1. - K1) * b2_vff_P[0][1];
const float P13 = (1. - K1) * b2_vff_P[0][2];
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4173] remove trailing whitespaces,
Felix Ruess <=