Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

added an accel ringbuffer

Browse Source
This commit is contained in:
Felix Ruess
2009-08-21 08:25:14 +00:00
parent 9886a9bf97
commit 47398d6a9c
3 changed files with 88 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/booz2_main.c
+3
View File
@@ -99,6 +99,7 @@ STATIC_INLINE void booz2_main_init( void ) {
booz_ahrs_aligner_init();
booz_ahrs_init();
booz_ahrs_init_accel_rb();
booz_ins_init();
@@ -173,6 +174,8 @@ static inline void on_gyro_accel_event( void ) {
BoozImuScaleGyro();
BoozImuScaleAccel();
booz_ahrs_store_accel();
if (booz_ahrs.status == BOOZ_AHRS_UNINIT) {
booz_ahrs_aligner_run();
if (booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED)
sw/airborne/booz/booz_ahrs.c
+70
View File
@@ -1,4 +1,74 @@
/*
* $Id$
*
* Copyright (C) 2009 Felix Ruess <felix.ruess@gmail.com>
*
* 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_ahrs.h"
#include "booz_imu.h"
struct BoozAhrs booz_ahrs;
struct BoozAhrsFloat booz_ahrs_float;
#define RB_MAXN 64
#define RB_TOP RB_MAXN - 1
struct Int32Vect3 accel_buf[RB_MAXN];
struct Int32Vect3 booz_ahrs_accel_mean;
uint8_t rb_r; /* pos to read from, oldest measurement */
uint8_t rb_w; /* pos to write to */
uint8_t rb_n; /* number of elements in rb */
void booz_ahrs_init_accel_rb(void) {
rb_r = 0;
rb_w = 0;
rb_n = 0;
}
void booz_ahrs_store_accel(void) {
VECT3_COPY(accel_buf[rb_w], booz_imu.accel);
rb_w = (rb_w + 1) < RB_TOP ? rb_w + 1 : 0;
/* once the buffer is full it always has the last RB_MAXN accel measurements */
if (rb_n < RB_MAXN) {
rb_n++;
} else {
rb_r++;
}
}
/* compute the mean of the last n accel measurements */
void booz_ahrs_compute_accel_mean(uint8_t n) {
struct Int32Vect3 sum;
int i, j;
INT_VECT3_ZERO(sum);
if (n > rb_n) {
n = rb_n;
}
for (i = 0; i < n; i++) {
j = (rb_r + i) < RB_TOP ? rb_r + i : rb_r + i - RB_TOP;
VECT3_ADD(sum, accel_buf[j]);
}
VECT3_SDIV(booz_ahrs_accel_mean, sum, n);
}
sw/airborne/booz/booz_ahrs.h
+15 -11
View File
@@ -1,6 +1,6 @@
/*
* $Id$
*
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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_AHRS_H
@@ -33,15 +33,15 @@
#define BOOZ_AHRS_RUNNING 1
struct BoozAhrs {
struct Int32Eulers ltp_to_body_euler;
struct Int32Quat ltp_to_body_quat;
struct Int32RMat ltp_to_body_rmat;
struct Int32Eulers ltp_to_imu_euler;
struct Int32Quat ltp_to_imu_quat;
struct Int32RMat ltp_to_imu_rmat;
struct Int32Rates imu_rate;
struct Int32Rates body_rate;
uint8_t status;
struct Int32Eulers ltp_to_body_euler;
struct Int32Quat ltp_to_body_quat;
struct Int32RMat ltp_to_body_rmat;
struct Int32Eulers ltp_to_imu_euler;
struct Int32Quat ltp_to_imu_quat;
struct Int32RMat ltp_to_imu_rmat;
struct Int32Rates imu_rate;
struct Int32Rates body_rate;
uint8_t status;
};
struct BoozAhrsFloat {
@@ -53,6 +53,7 @@ struct BoozAhrsFloat {
extern struct BoozAhrs booz_ahrs;
extern struct BoozAhrsFloat booz_ahrs_float;
extern struct Int32Vect3 booz_ahrs_accel_mean;
#define BOOZ_AHRS_FLOAT_OF_INT32() { \
QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \
@@ -69,5 +70,8 @@ extern void booz_ahrs_update(void);
extern void booz_ahrs_update_accel(void);
extern void booz_ahrs_update_mag(void);
extern void booz_ahrs_init_accel_rb(void);
extern void booz_ahrs_store_accel(void);
extern void booz_ahrs_compute_accel_mean(uint8_t n);
#endif /* BOOZ_AHRS_H */