mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-07 09:36:19 +08:00
This commit is contained in:
Antoine Drouin
@@ -70,31 +70,27 @@
|
||||
#define NPS_GYRO_RESOLUTION 65536
|
||||
|
||||
/* 2^12/GYRO_X_SENS */
|
||||
#define BSM_GYRO_SENSITIVITY_PP ( 4055.)
|
||||
#define BSM_GYRO_SENSITIVITY_QQ (-4055.)
|
||||
#define BSM_GYRO_SENSITIVITY_RR (-4055.)
|
||||
#define NPS_GYRO_SENSITIVITY_PP ( 4055.)
|
||||
#define NPS_GYRO_SENSITIVITY_QQ (-4055.)
|
||||
#define NPS_GYRO_SENSITIVITY_RR (-4055.)
|
||||
|
||||
#define BSM_GYRO_NEUTRAL_P 33924
|
||||
#define BSM_GYRO_NEUTRAL_Q 33417
|
||||
#define BSM_GYRO_NEUTRAL_R 32809
|
||||
#define NPS_GYRO_NEUTRAL_P 33924
|
||||
#define NPS_GYRO_NEUTRAL_Q 33417
|
||||
#define NPS_GYRO_NEUTRAL_R 32809
|
||||
|
||||
//#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.)
|
||||
//#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
|
||||
//#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
|
||||
#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5)
|
||||
#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5)
|
||||
#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5)
|
||||
|
||||
#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5)
|
||||
#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5)
|
||||
#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5)
|
||||
#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
|
||||
#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( .0)
|
||||
#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( .0)
|
||||
|
||||
#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
|
||||
#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg( .0)
|
||||
#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .0)
|
||||
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.)
|
||||
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
|
||||
#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
|
||||
|
||||
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.)
|
||||
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
|
||||
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
|
||||
|
||||
#define BSM_GYRO_DT (1./512.)
|
||||
#define NPS_GYRO_DT (1./512.)
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -268,6 +268,12 @@
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* 3x3 matrices
|
||||
*/
|
||||
/* accessor : row and col range from 0 to 2 */
|
||||
#define MAT33_ELMT(_m, _row, _col) ((_m).m[_row*3+_col])
|
||||
|
||||
|
||||
/* multiply _vin by _mat, store in _vout */
|
||||
#define MAT33_VECT3_MUL(_vout, _mat, _vin) { \
|
||||
@@ -332,7 +338,7 @@
|
||||
*/
|
||||
|
||||
/* accessor : row and col range from 0 to 2 */
|
||||
#define RMAT_ELMT(_rm, _row, _col) (_rm.m[_row*3+_col])
|
||||
#define RMAT_ELMT(_rm, _row, _col) MAT33_ELMT(_rm, _row, _col)
|
||||
|
||||
/* trace */
|
||||
#define RMAT_TRACE(_rm) (RMAT_ELMT(_rm, 0, 0)+RMAT_ELMT(_rm, 1, 1)+RMAT_ELMT(_rm, 2, 2))
|
||||
|
||||
@@ -100,6 +100,21 @@ struct FloatRates {
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* 3x3 matrices
|
||||
*/
|
||||
#define FLOAT_MAT33_ZERO(_m) { \
|
||||
RMAT_ELMT(_m, 0, 0) = 0.; \
|
||||
RMAT_ELMT(_m, 0, 1) = 0.; \
|
||||
RMAT_ELMT(_m, 0, 2) = 0.; \
|
||||
RMAT_ELMT(_m, 1, 0) = 0.; \
|
||||
RMAT_ELMT(_m, 1, 1) = 0.; \
|
||||
RMAT_ELMT(_m, 1, 2) = 0.; \
|
||||
RMAT_ELMT(_m, 2, 0) = 0.; \
|
||||
RMAT_ELMT(_m, 2, 1) = 0.; \
|
||||
RMAT_ELMT(_m, 2, 2) = 0.; \
|
||||
}
|
||||
|
||||
/*
|
||||
* Rotation Matrices
|
||||
*/
|
||||
|
||||
@@ -16,50 +16,29 @@ bool_t nps_sensor_gyro_available() {
|
||||
void nps_sensor_gyro_init(double time) {
|
||||
VECT3_ASSIGN(sensors.gyro.value, 0., 0., 0.);
|
||||
sensors.gyro.resolution = NPS_GYRO_RESOLUTION;
|
||||
#if 0
|
||||
bsm.gyro_resolution = BSM_GYRO_RESOLUTION;
|
||||
|
||||
bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB);
|
||||
m_zero(bsm.gyro_sensitivity);
|
||||
bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = BSM_GYRO_SENSITIVITY_PP;
|
||||
bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = BSM_GYRO_SENSITIVITY_QQ;
|
||||
bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = BSM_GYRO_SENSITIVITY_RR;
|
||||
|
||||
bsm.gyro_neutral = v_get(AXIS_NB);
|
||||
bsm.gyro_neutral->ve[AXIS_P] = BSM_GYRO_NEUTRAL_P;
|
||||
bsm.gyro_neutral->ve[AXIS_Q] = BSM_GYRO_NEUTRAL_Q;
|
||||
bsm.gyro_neutral->ve[AXIS_R] = BSM_GYRO_NEUTRAL_R;
|
||||
|
||||
bsm.gyro_noise_std_dev = v_get(AXIS_NB);
|
||||
bsm.gyro_noise_std_dev->ve[AXIS_P] = BSM_GYRO_NOISE_STD_DEV_P;
|
||||
bsm.gyro_noise_std_dev->ve[AXIS_Q] = BSM_GYRO_NOISE_STD_DEV_Q;
|
||||
bsm.gyro_noise_std_dev->ve[AXIS_R] = BSM_GYRO_NOISE_STD_DEV_R;
|
||||
|
||||
bsm.gyro_bias_initial = v_get(AXIS_NB);
|
||||
bsm.gyro_bias_initial->ve[AXIS_P] = BSM_GYRO_BIAS_INITIAL_P;
|
||||
bsm.gyro_bias_initial->ve[AXIS_Q] = BSM_GYRO_BIAS_INITIAL_Q;
|
||||
bsm.gyro_bias_initial->ve[AXIS_R] = BSM_GYRO_BIAS_INITIAL_R;
|
||||
|
||||
bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB);
|
||||
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P;
|
||||
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q;
|
||||
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R;
|
||||
|
||||
bsm.gyro_bias_random_walk_value = v_get(AXIS_NB);
|
||||
bsm.gyro_bias_random_walk_value->ve[AXIS_P] = bsm.gyro_bias_initial->ve[AXIS_P];
|
||||
bsm.gyro_bias_random_walk_value->ve[AXIS_Q] = bsm.gyro_bias_initial->ve[AXIS_Q];
|
||||
bsm.gyro_bias_random_walk_value->ve[AXIS_R] = bsm.gyro_bias_initial->ve[AXIS_R];
|
||||
|
||||
bsm.gyro_next_update = time;
|
||||
bsm.gyro_available = FALSE;
|
||||
#endif
|
||||
FLOAT_MAT33_ZERO(sensors.gyro.sensitivity);
|
||||
MAT33_ELMT(sensors.gyro.sensitivity, 0, 0) = NPS_GYRO_SENSITIVITY_PP;
|
||||
MAT33_ELMT(sensors.gyro.sensitivity, 1, 1) = NPS_GYRO_SENSITIVITY_QQ;
|
||||
MAT33_ELMT(sensors.gyro.sensitivity, 2, 2) = NPS_GYRO_SENSITIVITY_RR;
|
||||
VECT3_ASSIGN(sensors.gyro.neutral,
|
||||
NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R);
|
||||
VECT3_ASSIGN(sensors.gyro.noise_std_dev,
|
||||
NPS_GYRO_NOISE_STD_DEV_P, NPS_GYRO_NOISE_STD_DEV_Q, NPS_GYRO_NOISE_STD_DEV_R);
|
||||
VECT3_ASSIGN(sensors.gyro.bias_initial,
|
||||
NPS_GYRO_BIAS_INITIAL_P, NPS_GYRO_BIAS_INITIAL_Q, NPS_GYRO_BIAS_INITIAL_R);
|
||||
VECT3_ASSIGN(sensors.gyro.bias_random_walk_std_dev,
|
||||
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P,
|
||||
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q,
|
||||
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R);
|
||||
sensors.gyro.next_update = time;
|
||||
sensors.gyro.data_available = FALSE;
|
||||
}
|
||||
|
||||
void booz_sensors_model_gyro_run( double time ) {
|
||||
#if 0
|
||||
if (time < bsm.gyro_next_update)
|
||||
if (time < sensors.gyro.next_update)
|
||||
return;
|
||||
|
||||
#if 0
|
||||
/* rotate to IMU frame */
|
||||
/* convert to imu frame */
|
||||
static VEC* rate_imu = VNULL;
|
||||
@@ -94,10 +73,10 @@ void booz_sensors_model_gyro_run( double time ) {
|
||||
RoundSensor(bsm.gyro);
|
||||
/* saturation */
|
||||
BoundSensor(bsm.gyro, 0, bsm.gyro_resolution);
|
||||
|
||||
bsm.gyro_next_update += BSM_GYRO_DT;
|
||||
bsm.gyro_available = TRUE;
|
||||
#endif
|
||||
|
||||
sensors.gyro.next_update += NPS_GYRO_DT;
|
||||
sensors.gyro.data_available = TRUE;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,7 +1,9 @@
|
||||
#ifndef NPS_SENSORS_GYRO_H
|
||||
#define NPS_SENSORS_GYRO_H
|
||||
|
||||
#include "pprz_algebra.h"
|
||||
#include "pprz_algebra_double.h"
|
||||
#include "pprz_algebra_float.h"
|
||||
#include "std.h"
|
||||
|
||||
struct NpsSensorGyro {
|
||||
|
||||
Reference in New Issue
Block a user