Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

rename B2_HF to HF

Browse Source
This commit is contained in:
Felix Ruess
2010-09-28 14:05:17 +00:00
parent 4391630566
commit 4c67137799
8 changed files with 60 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+3 -3
View File
@@ -1174,7 +1174,7 @@
<field name="g" type="float"/>
</message>
<message name="BOOZ2_HFF" id="164">
<message name="HFF" id="164">
<field name="x" type="float"/>
<field name="y" type="float"/>
<field name="xd" type="float"/>
@@ -1183,7 +1183,7 @@
<field name="ydd" type="float"/>
</message>
<message name="BOOZ2_HFF_DBG" id="165">
<message name="HFF_DBG" id="165">
<field name="x_measure" type="float"/>
<field name="y_measure" type="float"/>
<field name="yd_measure" type="float"/>
@@ -1194,7 +1194,7 @@
<field name="Pydyd" type="float"/>
</message>
<message name="BOOZ2_HFF_GPS" id="166">
<message name="HFF_GPS" id="166">
<field name="lag_cnt" type="uint16"/>
<field name="lag_cnt_err" type="int16"/>
<field name="save_cnt" type="int16"/>
conf/telemetry/telemetry_booz2.xml
+2 -2
View File
@@ -86,12 +86,12 @@
<message name="ALIVE" period="0.9"/>
<message name="BOOZ2_HOVER_LOOP" period="0.062"/>
<message name="BOOZ2_STAB_ATTITUDE" period=".4"/>
<message name="BOOZ2_HFF_DBG" period=".2"/>
<message name="HFF_DBG" period=".2"/>
<!--<message name="BOOZ2_STAB_ATTITUDE_REF" period=".4"/>-->
<message name="BOOZ2_FP" period="0.8"/>
<message name="BOOZ_STATUS" period="1.2"/>
<message name="BOOZ2_NAV_STATUS" period="1.6"/>
<message name="BOOZ2_HFF_GPS" period=".03"/>
<message name="HFF_GPS" period=".03"/>
<message name="INS_REF" period="5.1"/>
</mode>
conf/telemetry/telemetry_booz2_flixr.xml
+2 -2
View File
@@ -85,12 +85,12 @@
<message name="ALIVE" period="0.9"/>
<message name="BOOZ2_HOVER_LOOP" period="0.1"/>
<message name="BOOZ2_STAB_ATTITUDE" period=".4"/>
<message name="BOOZ2_HFF_DBG" period=".2"/>
<message name="HFF_DBG" period=".2"/>
<!--<message name="BOOZ2_STAB_ATTITUDE_REF" period=".4"/>-->
<message name="BOOZ2_FP" period="0.8"/>
<message name="BOOZ_STATUS" period="1.2"/>
<message name="BOOZ2_NAV_STATUS" period="1.6"/>
<message name="BOOZ2_HFF_GPS" period=".1"/>
<message name="HFF_GPS" period=".1"/>
<message name="INS_REF" period="5.1"/>
</mode>
sw/airborne/booz/booz2_telemetry.h
+10 -10
View File
@@ -489,8 +489,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#ifdef USE_HFF
#include "ins/hf_float.h"
#define PERIODIC_SEND_BOOZ2_HFF(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF(_chan, \
#define PERIODIC_SEND_HFF(_chan) { \
DOWNLINK_SEND_HFF(_chan, \
&b2_hff_state.x, \
&b2_hff_state.y, \
&b2_hff_state.xdot, \
@@ -498,8 +498,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&b2_hff_state.xdotdot, \
&b2_hff_state.ydotdot); \
}
#define PERIODIC_SEND_BOOZ2_HFF_DBG(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_DBG(_chan, \
#define PERIODIC_SEND_HFF_DBG(_chan) { \
DOWNLINK_SEND_HFF_DBG(_chan, \
&b2_hff_x_meas, \
&b2_hff_y_meas, \
&b2_hff_xd_meas, \
@@ -510,19 +510,19 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&b2_hff_state.yP[1][1]); \
}
#ifdef GPS_LAG
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_GPS(_chan, \
#define PERIODIC_SEND_HFF_GPS(_chan) { \
DOWNLINK_SEND_HFF_GPS(_chan, \
&b2_hff_rb_last->lag_counter, \
&lag_counter_err, \
&save_counter); \
}
#else
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {}
#define PERIODIC_SEND_HFF_GPS(_chan) {}
#endif
#else
#define PERIODIC_SEND_BOOZ2_HFF(_chan) {}
#define PERIODIC_SEND_BOOZ2_HFF_DBG(_chan) {}
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {}
#define PERIODIC_SEND_HFF(_chan) {}
#define PERIODIC_SEND_HFF_DBG(_chan) {}
#define PERIODIC_SEND_HFF_GPS(_chan) {}
#endif
#define PERIODIC_SEND_GUIDANCE(_chan) { \
sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c
+3 -3
View File
@@ -21,11 +21,11 @@
* Boston, MA 02111-1307, USA.
*/
#include "hf_float.h"
#include "ins.h"
#include "hf_float-old.h"
#include <firmwares/rotorcraft/ins.h>
#include <firmwares/rotorcraft/imu.h>
#include "ahrs.h"
#include <firmwares/rotorcraft/ahrs.h>
#include "math/pprz_algebra_int.h"
sw/airborne/firmwares/rotorcraft/ins/hf_float-old.h
+3 -3
View File
@@ -21,8 +21,8 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_HF_FLOAT_H
#define BOOZ2_HF_FLOAT_H
#ifndef HF_FLOAT_H
#define HF_FLOAT_H
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
@@ -43,4 +43,4 @@ extern void b2ins_init(void);
extern void b2ins_propagate(void);
extern void b2ins_update_gps(void);
#endif /* BOOZ2_HF_FLOAT_H */
#endif /* HF_FLOAT_H */
sw/airborne/firmwares/rotorcraft/ins/hf_float.c
+29 -29
View File
@@ -23,9 +23,9 @@
*/
#include "hf_float.h"
#include "ins.h"
#include <firmwares/rotorcraft/ins.h>
#include <firmwares/rotorcraft/imu.h>
#include "ahrs.h"
#include <firmwares/rotorcraft/ahrs.h>
#include "booz_gps.h"
#include <stdlib.h>
@@ -46,25 +46,25 @@
/* initial covariance diagonal */
#define INIT_PXX 1.
/* process noise (is the same for x and y)*/
#ifndef B2_HFF_ACCEL_NOISE
#define B2_HFF_ACCEL_NOISE 0.5
#ifndef HFF_ACCEL_NOISE
#define HFF_ACCEL_NOISE 0.5
#endif
#define Q B2_HFF_ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
#define Qdotdot B2_HFF_ACCEL_NOISE*DT_HFILTER
#define Q HFF_ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
#define Qdotdot HFF_ACCEL_NOISE*DT_HFILTER
//TODO: proper measurement noise
#ifndef B2_HFF_R_POS
#define B2_HFF_R_POS 8.
#ifndef HFF_R_POS
#define HFF_R_POS 8.
#endif
#ifndef B2_HFF_R_POS_MIN
#define B2_HFF_R_POS_MIN 3.
#ifndef HFF_R_POS_MIN
#define HFF_R_POS_MIN 3.
#endif
#ifndef B2_HFF_R_SPEED
#define B2_HFF_R_SPEED 2.
#ifndef HFF_R_SPEED
#define HFF_R_SPEED 2.
#endif
#ifndef B2_HFF_R_SPEED_MIN
#define B2_HFF_R_SPEED_MIN 1.
#ifndef HFF_R_SPEED_MIN
#define HFF_R_SPEED_MIN 1.
#endif
/* gps measurement noise */
@@ -235,8 +235,8 @@ static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel,
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
Rgps_pos = B2_HFF_R_POS;
Rgps_vel = B2_HFF_R_SPEED;
Rgps_pos = HFF_R_POS;
Rgps_vel = HFF_R_SPEED;
b2_hff_init_x(init_x, init_xdot);
b2_hff_init_y(init_y, init_ydot);
/* init buffer for mean accel calculation */
@@ -272,15 +272,15 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
past_save_counter = SAVE_DONE;
b2_hff_ps_counter = 1;
b2_hff_lost_counter = 0;
b2_hff_lost_limit = B2_HFF_LOST_LIMIT;
b2_hff_lost_limit = HFF_LOST_LIMIT;
}
static inline void b2_hff_init_x(float init_x, float init_xdot) {
b2_hff_state.x = init_x;
b2_hff_state.xdot = init_xdot;
int i, j;
for (i=0; i<B2_HFF_STATE_SIZE; i++) {
for (j=0; j<B2_HFF_STATE_SIZE; j++)
for (i=0; i<HFF_STATE_SIZE; i++) {
for (j=0; j<HFF_STATE_SIZE; j++)
b2_hff_state.xP[i][j] = 0.;
b2_hff_state.xP[i][i] = INIT_PXX;
}
@@ -291,8 +291,8 @@ static inline void b2_hff_init_y(float init_y, float init_ydot) {
b2_hff_state.y = init_y;
b2_hff_state.ydot = init_ydot;
int i, j;
for (i=0; i<B2_HFF_STATE_SIZE; i++) {
for (j=0; j<B2_HFF_STATE_SIZE; j++)
for (i=0; i<HFF_STATE_SIZE; i++) {
for (j=0; j<HFF_STATE_SIZE; j++)
b2_hff_state.yP[i][j] = 0.;
b2_hff_state.yP[i][i] = INIT_PXX;
}
@@ -369,8 +369,8 @@ static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFlo
dest->y = source->y;
dest->ydot = source->ydot;
dest->ydotdot = source->ydotdot;
for (int i=0; i < B2_HFF_STATE_SIZE; i++) {
for (int j=0; j < B2_HFF_STATE_SIZE; j++) {
for (int i=0; i < HFF_STATE_SIZE; i++) {
for (int j=0; j < HFF_STATE_SIZE; j++) {
dest->xP[i][j] = source->xP[i][j];
dest->yP[i][j] = source->yP[i][j];
}
@@ -489,12 +489,12 @@ void b2_hff_update_gps(void) {
#ifdef USE_GPS_ACC4R
Rgps_pos = (float) booz_gps_state.pacc / 100.;
if (Rgps_pos < B2_HFF_R_POS_MIN)
Rgps_pos = B2_HFF_R_POS_MIN;
if (Rgps_pos < HFF_R_POS_MIN)
Rgps_pos = HFF_R_POS_MIN;
Rgps_vel = (float) booz_gps_state.sacc / 100.;
if (Rgps_vel < B2_HFF_R_SPEED_MIN)
Rgps_vel = B2_HFF_R_SPEED_MIN;
if (Rgps_vel < HFF_R_SPEED_MIN)
Rgps_vel = HFF_R_SPEED_MIN;
#endif
#ifdef GPS_LAG
@@ -504,7 +504,7 @@ void b2_hff_update_gps(void) {
/* update filter state with measurement */
b2_hff_update_x(&b2_hff_state, ins_gps_pos_m_ned.x, Rgps_pos);
b2_hff_update_y(&b2_hff_state, ins_gps_pos_m_ned.y, Rgps_pos);
#ifdef B2_HFF_UPDATE_SPEED
#ifdef HFF_UPDATE_SPEED
b2_hff_update_xdot(&b2_hff_state, ins_gps_speed_m_s_ned.x, Rgps_vel);
b2_hff_update_ydot(&b2_hff_state, ins_gps_speed_m_s_ned.y, Rgps_vel);
#endif
@@ -526,7 +526,7 @@ void b2_hff_update_gps(void) {
b2_hff_rb_last->rollback = TRUE;
b2_hff_update_x(b2_hff_rb_last, ins_gps_pos_m_ned.x, Rgps_pos);
b2_hff_update_y(b2_hff_rb_last, ins_gps_pos_m_ned.y, Rgps_pos);
#ifdef B2_HFF_UPDATE_SPEED
#ifdef HFF_UPDATE_SPEED
b2_hff_update_xdot(b2_hff_rb_last, ins_gps_speed_m_s_ned.x, Rgps_vel);
b2_hff_update_ydot(b2_hff_rb_last, ins_gps_speed_m_s_ned.y, Rgps_vel);
#endif
sw/airborne/firmwares/rotorcraft/ins/hf_float.h
+8 -8
View File
@@ -21,13 +21,13 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_HF_FLOAT_H
#define BOOZ2_HF_FLOAT_H
#ifndef HF_FLOAT_H
#define HF_FLOAT_H
#include "std.h"
#include "math/pprz_algebra_float.h"
#define B2_HFF_STATE_SIZE 2
#define HFF_STATE_SIZE 2
#ifndef HFF_PRESCALER
#define HFF_PRESCALER 16
@@ -37,7 +37,7 @@
#define HFF_FREQ (512./HFF_PRESCALER)
#define DT_HFILTER (1./HFF_FREQ)
#define B2_HFF_UPDATE_SPEED
#define HFF_UPDATE_SPEED
struct HfilterFloat {
float x;
@@ -48,8 +48,8 @@ struct HfilterFloat {
/* float ybias; */
float ydot;
float ydotdot;
float xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
float yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
float xP[HFF_STATE_SIZE][HFF_STATE_SIZE];
float yP[HFF_STATE_SIZE][HFF_STATE_SIZE];
uint8_t lag_counter;
bool_t rollback;
};
@@ -70,7 +70,7 @@ extern void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos);
extern void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel);
extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel);
#define B2_HFF_LOST_LIMIT 1000
#define HFF_LOST_LIMIT 1000
extern uint16_t b2_hff_lost_limit;
extern uint16_t b2_hff_lost_counter;
@@ -80,4 +80,4 @@ extern struct HfilterFloat *b2_hff_rb_last;
extern int lag_counter_err;
extern int save_counter;
#endif /* BOOZ2_HF_FLOAT_H */
#endif /* HF_FLOAT_H */