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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-31 02:04:45 +00:00
parent 8cf93e6262
commit b5426ac4a5
19 changed files with 423 additions and 185 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/conf_booz.makefile
+2
View File
@@ -80,6 +80,8 @@ flt.srcs += $(SRC_ARCH)/adc_hw.c
flt.srcs += max1167.c $(SRC_ARCH)/max1167_hw.c
flt.CFLAGS += -DDISABLE_MAGNETOMETER
flt.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c
flt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
flt.srcs += multitilt.c
conf/messages.xml
+7
View File
@@ -500,6 +500,13 @@
<field name="psi" type="float" unit="degres"/>
</message>
<message name="BOOZ_SIM_GYRO_BIAS" ID="242">
<field name="bp" type="float" unit="degres/s"/>
<field name="bq" type="float" unit="degres/s"/>
<field name="br" type="float" unit="degres/s"/>
</message>
<message name="ENOSE_STATUS" ID="250">
<field name="val1" type="uint16"/>
<field name="val2" type="uint16"/>
conf/settings/booz.xml
+2
View File
@@ -8,10 +8,12 @@
<dl_setting var="booz_control_rate_r_dgain" min="0" step="0.5" max="30" module="booz_control" shortname="r_d"/>
<dl_setting var="booz_control_attitude_phi_theta_pgain" min="-2000" step="50" max="-50" module="booz_control" shortname="att_p"/>
<dl_setting var="booz_control_attitude_phi_theta_dgain" min="-2000" step="50" max="-50" module="booz_control" shortname="att_d"/>
<!--
<dl_setting var="booz_nav_vertical_pgain" min="0.01" step="0.01" max="10" module="booz_nav" shortname="vert_p"/>
<dl_setting var="booz_nav_vertical_dgain" min="0.01" step="0.01" max="10" module="booz_nav" shortname="vert_d"/>
<dl_setting var="booz_nav_horizontal_pgain" min="0.01" step="0.01" max="0.5" module="booz_nav" shortname="horiz_p"/>
<dl_setting var="booz_nav_horizontal_dgain" min="0.01" step="0.01" max="0.5" module="booz_nav" shortname="hoziz_d"/>
<dl_setting var="booz_nav_horizontal_max_pos_err" min="0" step="0.5" max="20" module="booz_nav" shortname="vert_max_pos_err"/>
-->
</dl_settings>
</settings>
conf/telemetry/booz.xml
+2
View File
@@ -34,6 +34,8 @@
<message name="IMU_GYRO_RAW" period=".25"/>
<message name="IMU_ACCEL" period=".25"/>
<message name="IMU_ACCEL_RAW" period=".25"/>
<message name="IMU_MAG" period="1."/>
<message name="IMU_MAG_RAW" period="1."/>
</mode>
<mode name="raw_sensors">
<message name="IMU_GYRO_RAW" period=".017"/>
sw/airborne/arm7/micromag.c → sw/airborne/arm7/micromag_hw.c
+10 -4
View File
@@ -5,13 +5,15 @@
*/
#include "micromag.h"
volatile uint8_t micromag_data_available;
volatile int16_t micromag_values[MM_NB_AXIS];
volatile uint8_t micromag_cur_axe;
#ifndef DISABLE_MAGNETOMETER
static void EXTINT2_ISR(void) __attribute__((naked));
#endif /* DISABLE_MAGNETOMETER */
void micromag_init( void ) {
void micromag_hw_init( void ) {
#ifndef DISABLE_MAGNETOMETER
/* configure SS pin */
SetBit(MM_SS_IODIR, MM_SS_PIN); /* pin is output */
MmUnselect(); /* pin idles high */
@@ -32,16 +34,19 @@ void micromag_init( void ) {
VICIntEnable = VIC_BIT( VIC_EINT2 ); /* enable it */
VICVectCntl9 = VIC_ENABLE | VIC_EINT2;
VICVectAddr9 = (uint32_t)EXTINT2_ISR; // address of the ISR
#endif /* DISABLE_MAGNETOMETER */
}
void micromag_read( void ) {
#ifndef DISABLE_MAGNETOMETER
MmSelect();
SpiEnable();
// micromag_cur_axe = 0;
MmTriggerRead();
#endif /* DISABLE_MAGNETOMETER */
}
#ifndef DISABLE_MAGNETOMETER
void EXTINT2_ISR(void) {
ISR_ENTRY();
/* read dummy control byte reply */
@@ -57,3 +62,4 @@ void EXTINT2_ISR(void) {
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
ISR_EXIT();
}
#endif /* DISABLE_MAGNETOMETER */
sw/airborne/arm7/micromag.h → sw/airborne/arm7/micromag_hw.h
+3 -9
View File
@@ -1,18 +1,12 @@
#ifndef MICROMAG_H
#define MICROMAG_H
#ifndef MICROMAG_HW_H
#define MICROMAG_HW_H
#include "std.h"
#include "LPC21xx.h"
#include "interrupt_hw.h"
#include "spi_hw.h"
#define MM_NB_AXIS 3
extern void micromag_init( void );
extern void micromag_read( void );
extern volatile uint8_t micromag_data_available;
extern volatile int16_t micromag_values[MM_NB_AXIS];
extern volatile uint8_t micromag_cur_axe;
#define MM_SS_PIN 20
@@ -64,4 +58,4 @@ extern volatile uint8_t micromag_cur_axe;
}
#endif /* MICROMAG_H */
#endif /* MICROMAG_HW_H */
sw/airborne/booz_filter_main.c
+2 -2
View File
@@ -78,14 +78,14 @@ static inline void on_imu_event( void ) {
imu_v3_detect_vehicle_still();
if (imu_vehicle_still) {
ImuUpdateFromAvg();
multitilt_start(imu_accel, imu_gyro);
multitilt_start(imu_accel, imu_gyro, imu_mag);
}
break;
case MT_STATUS_RUNNING :
// t0 = T0TC;
multitilt_predict(imu_gyro_prev);
multitilt_update(imu_accel);
multitilt_update(imu_accel, imu_mag);
// t1 = T0TC;
// diff = t1 - t0;
// DOWNLINK_SEND_TIME(&diff);
sw/airborne/booz_filter_telemetry.h
+14 -2
View File
@@ -37,6 +37,7 @@
&imu_accel_raw[AXIS_Y], \
&imu_accel_raw[AXIS_Z]);
#define PERIODIC_SEND_IMU_GYRO_RAW_AVG() \
DOWNLINK_SEND_IMU_GYRO_RAW_AVG(&imu_vs_gyro_raw_avg[AXIS_X], \
&imu_vs_gyro_raw_avg[AXIS_Y], \
@@ -53,8 +54,19 @@
&imu_vs_accel_raw_var[AXIS_Y], \
&imu_vs_accel_raw_var[AXIS_Z]);
#define PERIODIC_SEND_AHRS_STATE() \
DOWNLINK_SEND_AHRS_STATE(&mtt_phi, &mtt_theta, &mtt_theta, &mtt_theta, \
#define PERIODIC_SEND_IMU_MAG() \
DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], \
&imu_mag[AXIS_Y], \
&imu_mag[AXIS_Z]);
#define PERIODIC_SEND_IMU_MAG_RAW() \
DOWNLINK_SEND_IMU_MAG_RAW(&imu_mag_raw[AXIS_X], \
&imu_mag_raw[AXIS_Y], \
&imu_mag_raw[AXIS_Z]);
#define PERIODIC_SEND_AHRS_STATE() \
DOWNLINK_SEND_AHRS_STATE(&mtt_phi, &mtt_theta, &mtt_psi, &mtt_psi, \
&mtt_bp, &mtt_bq, &mtt_br);
#define PERIODIC_SEND_AHRS_COV() \
sw/airborne/imu_v3.h
+3
View File
@@ -5,6 +5,7 @@
#include "6dof.h"
#include "max1167.h"
#include "micromag.h"
#include "imu_v3_hw.h"
/* calibrated sensor readings */
@@ -138,6 +139,7 @@ extern struct adc_buf buf_bat;
max1167_status = STA_MAX1167_IDLE; \
ImuUpdateGyros(); \
ImuUpdateAccels(); \
ImuUpdateMag(); \
user_handler(); \
} \
}
@@ -148,6 +150,7 @@ extern struct adc_buf buf_bat;
} \
else { \
max1167_read(); \
micromag_read(); \
} \
}
sw/airborne/micromag.c
+14
View File
@@ -0,0 +1,14 @@
#include "micromag.h"
volatile uint8_t micromag_data_available;
volatile int16_t micromag_values[MM_NB_AXIS];
void micromag_init( void ) {
micromag_hw_init();
uint8_t i;
for (i=0; i<MM_NB_AXIS; i++)
micromag_values[i] = 0;
micromag_data_available = FALSE;
}
sw/airborne/micromag.h
+18
View File
@@ -0,0 +1,18 @@
#ifndef MICROMAG_H
#define MICROMAG_H
#include "std.h"
#define MM_NB_AXIS 3
extern void micromag_init( void );
extern void micromag_read( void );
extern volatile uint8_t micromag_data_available;
extern volatile int16_t micromag_values[MM_NB_AXIS];
extern void micromag_hw_init( void );
#include "micromag_hw.h"
#endif /* MICROMAG_H */
sw/airborne/multitilt.c
+123 -101
View File
@@ -24,13 +24,48 @@ float mtt_br;
/* covariance matrix */
float mtt_P_phi[2][2];
float mtt_P_theta[2][2];
//float mtt_P_psi[2][2];
float mtt_P_psi[2][2];
/* process covariance noise */
static const float Q_angle = 0.0005;
//static const float Q_angle = 0.0005;
static const float Q_angle = 0.00025;
static const float Q_bias = 0.0015;
/* Measurement covariance */
static const float R_accel = 0.3;
//static const float R_accel = 0.3;
static const float R_accel = 0.4;
#define WRAP(x,a) { while (x > a) x -= 2 * a; while (x <= -a) x += 2 * a;}
static inline float phi_of_accel( const float* accel) {
return atan2(accel[AXIS_Y], accel[AXIS_Z]);
}
static inline float theta_of_accel( const float* accel) {
const float g2 =
accel[AXIS_X]*accel[AXIS_X] +
accel[AXIS_Y]*accel[AXIS_Y] +
accel[AXIS_Z]*accel[AXIS_Z];
return -asin( accel[AXIS_X] / sqrt( g2 ) );
}
static inline float psi_of_mag( const int16_t* mag) {
/* untilt magnetometer */
const float ctheta = cos( mtt_theta );
const float stheta = sin( mtt_theta );
const float cphi = cos( mtt_phi );
const float sphi = sin( mtt_phi );
const float mn =
ctheta* mag[0]+
sphi*stheta* mag[1]+
cphi*stheta* mag[2];
const float me =
/* 0* mag[0]+ */
cphi* mag[1]+
-sphi* mag[2];
return -atan2( me, mn );
}
void multitilt_init(void) {
mtt_status = MT_STATUS_UNINIT;
@@ -47,13 +82,13 @@ void multitilt_init(void) {
mtt_br = 0.;
}
void multitilt_start(const float* accel, const float* gyro) {
void multitilt_start(const float* accel, const float* gyro, const int16_t* mag) {
/* reset covariance matrices */
const float cov_init[2][2] = {{1., 0.},
{0., 1.}};
memcpy(mtt_P_phi, cov_init, sizeof(cov_init));
memcpy(mtt_P_theta, cov_init, sizeof(cov_init));
// memcpy(mtt_P_psi, cov_init, sizeof(cov_init));
memcpy(mtt_P_psi, cov_init, sizeof(cov_init));
/* initialise state */
mtt_p = 0.;
@@ -64,20 +99,39 @@ void multitilt_start(const float* accel, const float* gyro) {
mtt_bq = gyro[AXIS_Q];
mtt_br = gyro[AXIS_R];
const float init_phi = atan2(accel[AXIS_Y], accel[AXIS_Z]);
const float g2 =
accel[AXIS_X]*accel[AXIS_X] +
accel[AXIS_Y]*accel[AXIS_Y] +
accel[AXIS_Z]*accel[AXIS_Z];
const float init_theta = -asin( accel[AXIS_X] / sqrt( g2 ) );
const float init_phi = phi_of_accel(accel);
const float init_theta = theta_of_accel(accel);
#ifndef DISABLE_MAGNETOMETER
const float init_psi = psi_of_mag(mag);
#endif
mtt_phi = init_phi;
mtt_theta = init_theta;
#ifndef DISABLE_MAGNETOMETER
mtt_psi = init_psi;
#endif
mtt_status = MT_STATUS_RUNNING;
}
static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]) {
(*angle) += angle_dot * DT;
const float P_dot00 = Q_angle - P[0][1] - P[1][0];
const float P_dot01 = - P[1][1];
const float P_dot10 = - P[1][1];
const float P_dot11 = Q_bias;
/* P += Pdot * dt */
P[0][0] += P_dot00 * DT;
P[0][1] += P_dot01 * DT;
P[1][0] += P_dot10 * DT;
P[1][1] += P_dot11 * DT;
}
void multitilt_predict( const float* gyro ) {
/* unbias gyro */
mtt_p = gyro[AXIS_P] - mtt_bp;
@@ -90,99 +144,67 @@ void multitilt_predict( const float* gyro ) {
float t_theta = tan(mtt_theta);
float phi_dot = mtt_p + s_phi*t_theta*mtt_q + c_phi*t_theta*mtt_r;
mtt_phi += phi_dot * DT;
float theta_dot = c_phi*mtt_q - s_phi*mtt_r;
mtt_theta += theta_dot * DT;
mtt_predict_axis(&mtt_phi, phi_dot, mtt_P_phi);
mtt_predict_axis(&mtt_theta, theta_dot, mtt_P_theta);
/* Pdot = A*P + P*A' + Q */
const float P_phi_dot00 = Q_angle - mtt_P_phi[0][1] - mtt_P_phi[1][0];
const float P_phi_dot01 = - mtt_P_phi[1][1];
const float P_phi_dot10 = - mtt_P_phi[1][1];
const float P_phi_dot11 = Q_bias;
/* P += Pdot * dt */
mtt_P_phi[0][0] += P_phi_dot00 * DT;
mtt_P_phi[0][1] += P_phi_dot01 * DT;
mtt_P_phi[1][0] += P_phi_dot10 * DT;
mtt_P_phi[1][1] += P_phi_dot11 * DT;
/* Pdot = A*P + P*A' + Q */
const float P_theta_dot00 = Q_angle - mtt_P_theta[0][1] - mtt_P_theta[1][0];
const float P_theta_dot01 = - mtt_P_theta[1][1];
const float P_theta_dot10 = - mtt_P_theta[1][1];
const float P_theta_dot11 = Q_bias;
/* P += Pdot * dt */
mtt_P_theta[0][0] += P_theta_dot00 * DT;
mtt_P_theta[0][1] += P_theta_dot01 * DT;
mtt_P_theta[1][0] += P_theta_dot10 * DT;
mtt_P_theta[1][1] += P_theta_dot11 * DT;
}
void multitilt_update( const float* accel ) {
const float measure_phi = atan2(accel[AXIS_Y], accel[AXIS_Z]);
const float err_phi = measure_phi - mtt_phi;
const float Pct_0_phi = mtt_P_phi[0][0];
const float Pct_1_phi = mtt_P_phi[1][0];
/* E = C P C' + R */
const float E_phi = R_accel + Pct_0_phi;
/* K = P C' inv(E) */
const float K_0_phi = Pct_0_phi / E_phi;
const float K_1_phi = Pct_1_phi / E_phi;
/* P = P - K C P */
const float t_0_phi = Pct_0_phi;
const float t_1_phi = mtt_P_phi[0][1];
mtt_P_phi[0][0] -= K_0_phi * t_0_phi;
mtt_P_phi[0][1] -= K_0_phi * t_1_phi;
mtt_P_phi[1][0] -= K_1_phi * t_0_phi;
mtt_P_phi[1][1] -= K_1_phi * t_1_phi;
/* X += K * err */
mtt_phi += K_0_phi * err_phi;
mtt_bp += K_1_phi * err_phi;
#ifndef DISABLE_MAGNETOMETER
float c_theta = cos(mtt_theta);
float psi_dot = s_phi/c_theta*mtt_q + c_phi/c_theta*mtt_r;
mtt_predict_axis(&mtt_psi, psi_dot, mtt_P_psi);
#endif
const float g2 =
accel[AXIS_X]*accel[AXIS_X] +
accel[AXIS_Y]*accel[AXIS_Y] +
accel[AXIS_Z]*accel[AXIS_Z];
const float measure_theta = -asin( accel[AXIS_X] / sqrt( g2 ) );
const float err_theta = measure_theta - mtt_theta;
const float Pct_0_theta = mtt_P_theta[0][0];
const float Pct_1_theta = mtt_P_theta[1][0];
/* E = C P C' + R */
const float E_theta = R_accel + Pct_0_theta;
/* K = P C' inv(E) */
const float K_0_theta = Pct_0_theta / E_theta;
const float K_1_theta = Pct_1_theta / E_theta;
/* P = P - K C P */
const float t_0_theta = Pct_0_theta;
const float t_1_theta = mtt_P_theta[0][1];
mtt_P_theta[0][0] -= K_0_theta * t_0_theta;
mtt_P_theta[0][1] -= K_0_theta * t_1_theta;
mtt_P_theta[1][0] -= K_1_theta * t_0_theta;
mtt_P_theta[1][1] -= K_1_theta * t_1_theta;
/* X += K * err */
mtt_theta += K_0_theta * err_theta;
mtt_bq += K_1_theta * err_theta;
}
static void inline MttUpdateAxis(float _err, float _P[2][2], float* angle, float* bias) {
const float Pct_0 = _P[0][0];
const float Pct_1 = _P[1][0];
/* E = C P C' + R */
const float E = R_accel + Pct_0;
/* K = P C' inv(E) */
const float K_0 = Pct_0 / E;
const float K_1 = Pct_1 / E;
/* P = P - K C P */
const float t_0 = Pct_0;
const float t_1 = _P[0][1];
_P[0][0] -= K_0 * t_0;
_P[0][1] -= K_0 * t_1;
_P[1][0] -= K_1 * t_0;
_P[1][1] -= K_1 * t_1;
/* X += K * err */
(*angle) += K_0 * _err;
(*bias) += K_1 * _err;
}
void multitilt_update( const float* accel, const int16_t* mag ) {
const float measure_phi = phi_of_accel(accel);
float err_phi = measure_phi - mtt_phi;
WRAP(err_phi, M_PI);
MttUpdateAxis(err_phi, mtt_P_phi, &mtt_phi, &mtt_bp);
WRAP(mtt_phi, M_PI);
const float measure_theta = theta_of_accel(accel);
float err_theta = measure_theta - mtt_theta;
WRAP(err_theta, M_PI_2);
MttUpdateAxis(err_theta, mtt_P_theta, &mtt_theta, &mtt_bq);
WRAP(mtt_theta, M_PI_2);
#ifndef DISABLE_MAGNETOMETER
float measure_psi = psi_of_mag(mag);
float err_psi = measure_psi - mtt_psi;
WRAP(err_psi, M_PI);
MttUpdateAxis(err_psi, mtt_P_psi, &mtt_psi, &mtt_br);
WRAP(mtt_psi, M_PI);
#endif
}
sw/airborne/multitilt.h
+2 -2
View File
@@ -25,9 +25,9 @@ extern float mtt_br;
extern void multitilt_init(void);
extern void multitilt_start( const float* accel, const float* gyro);
extern void multitilt_start( const float* accel, const float* gyro, const int16_t* mag);
extern void multitilt_predict( const float* gyro );
extern void multitilt_update( const float* accel );
extern void multitilt_update( const float* accel, const int16_t* mag );
#endif /* MULTITILT_H */
sw/simulator/Makefile
+4 -3
View File
@@ -143,14 +143,14 @@ BOOZ_AB_SRCS += $(AB)/spi.c $(AB_ARCH)/spi_hw.c
#CFLAGS += -DDATALINK=PPRZ
#BOOZ_AB_SRCS += ../airborne/datalink.c
CFLAGS += -DBOOZ_FILTER_MCU
BOOZ_AB_SRCS += $(AB)/booz_estimator.c
BOOZ_AB_SRCS += $(AB)/booz_control.c
BOOZ_AB_SRCS += $(AB)/booz_nav.c
BOOZ_AB_SRCS += $(AB)/booz_autopilot.c
BOOZ_AB_SRCS += $(AB)/commands.c
CFLAGS += -DBOOZ_FILTER_MCU
BOOZ_AB_SRCS += $(AB)/booz_filter_main.c
CFLAGS += -DADC_CHANNEL_AX=1 -DADC_CHANNEL_AY=2 -DADC_CHANNEL_AZ=3 -DADC_CHANNEL_BAT=4
@@ -160,9 +160,10 @@ BOOZ_AB_SRCS += $(AB_ARCH)/adc_hw.c
BOOZ_AB_SRCS += $(AB)/booz_filter_telemetry.c
BOOZ_AB_SRCS += $(AB)/max1167.c $(AB_ARCH)/max1167_hw.c
BOOZ_AB_SRCS += $(AB)/micromag.c $(AB_ARCH)/micromag_hw.c
BOOZ_AB_SRCS += $(AB)/imu_v3.c $(AB_ARCH)/imu_v3_hw.c
CFLAGS += -DBOOZ_ATT_FILTER_TYPE=multitilt
BOOZ_AB_SRCS += $(AB)/multitilt.c
sw/simulator/booz_flight_model.c
+6
View File
@@ -30,6 +30,12 @@ void booz_flight_model_init( void ) {
bfm.g_earth->ve[AXIS_Y] = 0.;
bfm.g_earth->ve[AXIS_Z] = G;
/* FIXME */
bfm.h_earth = v_get(AXIS_NB);
bfm.h_earth->ve[AXIS_X] = 1.;
bfm.h_earth->ve[AXIS_Y] = 0.;
bfm.h_earth->ve[AXIS_Z] = 1.;
bfm.thrust_factor = 0.5 * RHO * PROP_AREA * C_t * PROP_RADIUS * PROP_RADIUS;
bfm.torque_factor = 0.5 * RHO * PROP_AREA * C_q * PROP_RADIUS * PROP_RADIUS;
sw/simulator/booz_flight_model.h
+2
View File
@@ -33,6 +33,8 @@ struct BoozFlightModel {
VEC* state;
/* constants used in derivative computation */
/* magnetic field in earth frame */
VEC* h_earth;
/* gravitation in earth frame */
VEC* g_earth;
/* propeller thrust factor */
sw/simulator/booz_sensors_model.c
+108 -34
View File
@@ -14,6 +14,9 @@ static void booz_sensors_model_accel_run(MAT* dcm);
static void booz_sensors_model_gyro_init(void);
static void booz_sensors_model_gyro_run(double dt);
static void booz_sensors_model_mag_init(void);
static void booz_sensors_model_mag_run(MAT* dcm);
static void booz_sensors_model_gps_init(void);
static void booz_sensors_model_gps_run(double dt, MAT* dcm_t);
@@ -23,6 +26,7 @@ static VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out);
void booz_sensors_model_init(void) {
booz_sensors_model_accel_init();
booz_sensors_model_gyro_init();
booz_sensors_model_mag_init();
booz_sensors_model_gps_init();
}
@@ -45,6 +49,7 @@ void booz_sensors_model_run(double dt) {
booz_sensors_model_accel_run(dcm);
booz_sensors_model_gyro_run(dt);
booz_sensors_model_mag_run(dcm);
booz_sensors_model_gps_run(dt, dcm_t);
}
@@ -54,12 +59,13 @@ static void booz_sensors_model_accel_init(void) {
bsm.accel->ve[AXIS_X] = 0.;
bsm.accel->ve[AXIS_Y] = 0.;
bsm.accel->ve[AXIS_Z] = 0.;
bsm.accel_resolution = 1024 * 32;
bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.accel_sensitivity);
bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = -(1024. * 32) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = (1024. * 32) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = (1024. * 32) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = -(double)(bsm.accel_resolution) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = (double)(bsm.accel_resolution) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = (double)(bsm.accel_resolution) / (6. * 9.81);
bsm.accel_neutral = v_get(AXIS_NB);
bsm.accel_neutral->ve[AXIS_X] = 538 * 32;
@@ -67,14 +73,14 @@ static void booz_sensors_model_accel_init(void) {
bsm.accel_neutral->ve[AXIS_Z] = 506 * 32;
bsm.accel_noise_std_dev = v_get(AXIS_NB);
bsm.accel_noise_std_dev->ve[AXIS_X] = 2e-1 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
bsm.accel_noise_std_dev->ve[AXIS_Y] = 2e-1 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
bsm.accel_noise_std_dev->ve[AXIS_Z] = 2e-1 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
bsm.accel_noise_std_dev->ve[AXIS_X] = 2e-1; /* m2s-4 */
bsm.accel_noise_std_dev->ve[AXIS_Y] = 2e-1; /* m2s-4 */
bsm.accel_noise_std_dev->ve[AXIS_Z] = 2e-1; /* m2s-4 */
bsm.accel_bias = v_get(AXIS_NB);
bsm.accel_bias->ve[AXIS_P] = 1e-3 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
bsm.accel_bias->ve[AXIS_Q] = 1e-3 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
bsm.accel_bias->ve[AXIS_R] = 1e-3 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
bsm.accel_bias->ve[AXIS_P] = 1e-3; /* ms-2 */
bsm.accel_bias->ve[AXIS_Q] = 1e-3; /* ms-2 */
bsm.accel_bias->ve[AXIS_R] = 1e-3; /* ms-2 */
}
@@ -85,32 +91,33 @@ static void booz_sensors_model_gyro_init(void) {
bsm.gyro->ve[AXIS_P] = 0.;
bsm.gyro->ve[AXIS_Q] = 0.;
bsm.gyro->ve[AXIS_R] = 0.;
bsm.gyro_resolution = 65536;
bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.gyro_sensitivity);
bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = 32768. / RadOfDeg(-413.41848); /* degres/s - nominal 300 */
bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = 32768. / RadOfDeg(-403.65564); /* degres/s - nominal 300 */
bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = 32768. / RadOfDeg( 395.01929); /* degres/s - nominal 300 */
bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = (double)bsm.gyro_resolution / (2.*RadOfDeg(-413.41848)); /* degres/s - nominal 300 */
bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = (double)bsm.gyro_resolution / (2.*RadOfDeg(-403.65564)); /* degres/s - nominal 300 */
bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = (double)bsm.gyro_resolution / (2.*RadOfDeg( 395.01929)); /* degres/s - nominal 300 */
bsm.gyro_neutral = v_get(AXIS_NB);
bsm.gyro_neutral->ve[AXIS_P] = 65536. * 0.6238556; /* ratio of full scale - nominal 0.5 */
bsm.gyro_neutral->ve[AXIS_Q] = 65536. * 0.6242371; /* ratio of full scale - nominal 0.5 */
bsm.gyro_neutral->ve[AXIS_R] = 65536. * 0.6035156; /* ratio of full scale - nominal 0.5 */
bsm.gyro_neutral->ve[AXIS_P] = (double)bsm.gyro_resolution * 0.6238556; /* ratio of full scale - nominal 0.5 */
bsm.gyro_neutral->ve[AXIS_Q] = (double)bsm.gyro_resolution * 0.6242371; /* ratio of full scale - nominal 0.5 */
bsm.gyro_neutral->ve[AXIS_R] = (double)bsm.gyro_resolution * 0.6035156; /* ratio of full scale - nominal 0.5 */
bsm.gyro_noise_std_dev = v_get(AXIS_NB);
bsm.gyro_noise_std_dev->ve[AXIS_P] = RadOfDeg(1.) * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_noise_std_dev->ve[AXIS_Q] = RadOfDeg(1.) * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_noise_std_dev->ve[AXIS_R] = RadOfDeg(1.) * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_noise_std_dev->ve[AXIS_P] = RadOfDeg(1.);
bsm.gyro_noise_std_dev->ve[AXIS_Q] = RadOfDeg(1.);
bsm.gyro_noise_std_dev->ve[AXIS_R] = RadOfDeg(1.);
bsm.gyro_bias_initial = v_get(AXIS_NB);
bsm.gyro_bias_initial->ve[AXIS_P] = RadOfDeg( 1.0) * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_bias_initial->ve[AXIS_Q] = RadOfDeg(-1.0) * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_bias_initial->ve[AXIS_R] = RadOfDeg( 0.7) * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_bias_initial->ve[AXIS_P] = RadOfDeg( 1.0);
bsm.gyro_bias_initial->ve[AXIS_Q] = RadOfDeg(-0.25);
bsm.gyro_bias_initial->ve[AXIS_R] = RadOfDeg( 0.5);
bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB);
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = RadOfDeg(5.e-1) * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = RadOfDeg(5.e-1) * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = RadOfDeg(5.e-1) * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = RadOfDeg(5.e-1);
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = RadOfDeg(5.e-1);
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = RadOfDeg(5.e-1);
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];
@@ -119,6 +126,33 @@ static void booz_sensors_model_gyro_init(void) {
}
static void booz_sensors_model_mag_init(void) {
bsm.mag = v_get(AXIS_NB);
bsm.mag->ve[AXIS_X] = 0.;
bsm.mag->ve[AXIS_Y] = 0.;
bsm.mag->ve[AXIS_Z] = 0.;
bsm.mag_resolution = 4096;
bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.mag_sensitivity);
bsm.mag_sensitivity->me[AXIS_X][AXIS_X] = -(double)bsm.mag_resolution / 6.;
bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y] = -(double)bsm.mag_resolution / 6.;
bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z] = (double)bsm.mag_resolution / 6.;
bsm.mag_neutral = v_get(AXIS_NB);
bsm.mag_neutral->ve[AXIS_X] = 0.;
bsm.mag_neutral->ve[AXIS_Y] = 0.;
bsm.mag_neutral->ve[AXIS_Z] = 0.;
bsm.mag_noise_std_dev = v_get(AXIS_NB);
bsm.mag_noise_std_dev->ve[AXIS_X] = 2e-1;
bsm.mag_noise_std_dev->ve[AXIS_Y] = 2e-1;
bsm.mag_noise_std_dev->ve[AXIS_Z] = 2e-1;
}
static void booz_sensors_model_gps_init(void) {
bsm.speed_sensor = v_get(AXIS_NB);
@@ -153,6 +187,22 @@ static void booz_sensors_model_gps_init(void) {
bsm.pos_bias_random_walk_value->ve[AXIS_Z] = bsm.pos_bias_initial->ve[AXIS_Z];
}
#define RoundSensor(_sensor) { \
_sensor->ve[AXIS_X] = rint(_sensor->ve[AXIS_X]); \
_sensor->ve[AXIS_Y] = rint(_sensor->ve[AXIS_Y]); \
_sensor->ve[AXIS_Z] = rint(_sensor->ve[AXIS_Z]); \
}
#define BoundSensor(_sensor, _min, _max) { \
if ( _sensor->ve[AXIS_X] < _min) _sensor->ve[AXIS_X] = _min; \
if ( _sensor->ve[AXIS_X] > _max) _sensor->ve[AXIS_X] = _max; \
if ( _sensor->ve[AXIS_Y] < _min) _sensor->ve[AXIS_Y] = _min; \
if ( _sensor->ve[AXIS_Y] > _max) _sensor->ve[AXIS_Y] = _max; \
if ( _sensor->ve[AXIS_Z] < _min) _sensor->ve[AXIS_Z] = _min; \
if ( _sensor->ve[AXIS_Z] > _max) _sensor->ve[AXIS_Z] = _max; \
}
static void booz_sensors_model_accel_run( MAT* dcm ) {
/* */
@@ -196,6 +246,7 @@ static void booz_sensors_model_accel_run( MAT* dcm ) {
/* compute accel reading */
bsm.accel = mv_mlt(bsm.accel_sensitivity, accel_body, bsm.accel);
bsm.accel = v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
/* compute accel error readings */
static VEC *accel_error = VNULL;
accel_error = v_resize(accel_error, AXIS_NB);
@@ -204,18 +255,22 @@ static void booz_sensors_model_accel_run( MAT* dcm ) {
accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error);
/* constant bias */
accel_error = v_add(accel_error, bsm.accel_bias, accel_error);
/* scale to adc units FIXME : should use full adc gain ? sum ? */
accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
/* add per accel error reading */
bsm.accel = v_add(bsm.accel, accel_error, bsm.accel);
/* round signal to account for adc discretisation */
bsm.accel->ve[AXIS_X] = rint( bsm.accel->ve[AXIS_X]);
bsm.accel->ve[AXIS_Y] = rint( bsm.accel->ve[AXIS_Y]);
bsm.accel->ve[AXIS_Z] = rint( bsm.accel->ve[AXIS_Z]);
RoundSensor(bsm.accel);
/* saturation */
BoundSensor(bsm.accel, 0, bsm.accel_resolution);
// printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
}
static void booz_sensors_model_gyro_run( double dt) {
static void booz_sensors_model_gyro_run( double dt ) {
/* extract rotational speed from flight model state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
@@ -227,7 +282,7 @@ static void booz_sensors_model_gyro_run( double dt) {
bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_body, bsm.gyro);
bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro);
/* compute gyro error reading */
/* compute gyro error readings */
static VEC *gyro_error = VNULL;
gyro_error = v_resize(gyro_error, AXIS_NB);
gyro_error = v_zero(gyro_error);
@@ -239,16 +294,35 @@ static void booz_sensors_model_gyro_run( double dt) {
bsm.gyro_bias_random_walk_std_dev, dt,
bsm.gyro_bias_random_walk_value);
gyro_error = v_add(gyro_error, bsm.gyro_bias_random_walk_value, gyro_error);
/* scale to adc units FIXME : should use full adc gain ? sum ? */
gyro_error->ve[AXIS_P] = gyro_error->ve[AXIS_P] * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
gyro_error->ve[AXIS_Q] = gyro_error->ve[AXIS_Q] * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
gyro_error->ve[AXIS_R] = gyro_error->ve[AXIS_R] * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
/* add per gyro error reading */
bsm.gyro = v_add(bsm.gyro, gyro_error, bsm.gyro);
/* round signal to account for adc discretisation */
bsm.gyro->ve[AXIS_P] = rint( bsm.gyro->ve[AXIS_P]);
bsm.gyro->ve[AXIS_Q] = rint( bsm.gyro->ve[AXIS_Q]);
bsm.gyro->ve[AXIS_R] = rint( bsm.gyro->ve[AXIS_R]);
RoundSensor(bsm.gyro);
/* saturation */
BoundSensor(bsm.gyro, 0, bsm.gyro_resolution);
}
static void booz_sensors_model_mag_run( MAT* dcm ) {
/* rotate h to body frame */
static VEC *h_body = VNULL;
h_body = v_resize(h_body, AXIS_NB);
h_body = mv_mlt(dcm, bfm.h_earth, h_body);
bsm.mag = mv_mlt(bsm.mag_sensitivity, h_body, bsm.mag);
bsm.mag = v_add(bsm.mag, bsm.mag_neutral, bsm.mag);
// printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]);
// printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]);
/* round signal to account for adc discretisation */
RoundSensor(bsm.mag);
}
static void booz_sensors_model_gps_run( double dt, MAT* dcm_t ) {
/* simulate speed sensor */
sw/simulator/booz_sensors_model.h
+9
View File
@@ -10,12 +10,14 @@ extern void booz_sensors_model_run( double dt);
struct BoozSensorsModel {
VEC* accel;
unsigned int accel_resolution;
MAT* accel_sensitivity;
VEC* accel_neutral;
VEC* accel_noise_std_dev;
VEC* accel_bias;
VEC* gyro;
unsigned int gyro_resolution;
MAT* gyro_sensitivity;
VEC* gyro_neutral;
VEC* gyro_noise_std_dev;
@@ -23,6 +25,13 @@ struct BoozSensorsModel {
VEC* gyro_bias_random_walk_std_dev;
VEC* gyro_bias_random_walk_value;
VEC* mag;
unsigned int mag_resolution;
MAT* mag_sensitivity;
VEC* mag_neutral;
VEC* mag_noise_std_dev;
/* imaginary sensors - gps maybe */
VEC* speed_sensor;
VEC* speed_noise_std_dev;
sw/simulator/main_booz_sim.c
+92 -28
View File
@@ -14,8 +14,8 @@
//char* fg_host = "127.0.0.1";
char* fg_host = "10.31.4.107";
//char* fg_host = "192.168.1.191";
//char* fg_host = "10.31.4.107";
char* fg_host = "192.168.1.191";
unsigned int fg_port = 5501;
char* joystick_dev = "/dev/input/js0";
@@ -63,6 +63,7 @@ static gboolean booz_sim_periodic(gpointer data) {
booz_flight_model_run(DT, booz_sim_actuators_values);
booz_sensors_model_run(DT);
sim_time += DT;
/* call the filter periodic task to read sensors */
@@ -170,11 +171,93 @@ static inline void booz_sim_display(void) {
(bfm.state->ve[BFMS_X]),
(bfm.state->ve[BFMS_Y]),
(bfm.state->ve[BFMS_Z]));
/* one good reason to store the bias in real unit !! */
IvySendMsg("148 BOOZ_SIM_GYRO_BIAS %f %f %f",
DegOfRad(bsm.gyro_bias_random_walk_value->ve[AXIS_P]),
DegOfRad(bsm.gyro_bias_random_walk_value->ve[AXIS_Q]),
DegOfRad(bsm.gyro_bias_random_walk_value->ve[AXIS_R]));
}
}
//#define FAKE_JOYSTICK
#define BREAK_MTT() { \
int foo = sim_time/10; \
switch(foo%4) { \
case 0: \
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
break; \
case 1: \
ppm_pulses[RADIO_PITCH] = 1500 + 0.0 * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0.4 * (2050-950); \
break; \
case 2: \
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
break; \
case 3: \
ppm_pulses[RADIO_PITCH] = 1500 - 0.0 * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0.4 * (2050-950); \
break; \
} \
}
#define WALK_OVAL() { \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
int foo = sim_time/10; \
switch(foo%4) { \
case 0: \
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
break; \
case 1: \
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \
break; \
case 2: \
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
break; \
case 3: \
ppm_pulses[RADIO_YAW] = 1493 + 0.2 * (2050-950); \
break; \
} \
}
#define STATIONARY() { \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
}
#define TOUPIE() { \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
int foo = sim_time/20; \
switch(foo%4) { \
case 0: \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
break; \
case 1: \
ppm_pulses[RADIO_YAW] = 1493 + 0.2 * (2050-950); \
break; \
case 2: \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
break; \
case 3: \
ppm_pulses[RADIO_YAW] = 1493 + -0.2 * (2050-950); \
break; \
} \
}
#define CIRCLE() { \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
double alpha = 0.01 * sim_time; \
ppm_pulses[RADIO_PITCH] = 1498 + cos(alpha) * 250.; \
ppm_pulses[RADIO_ROLL] = 1500 + sin(alpha) * 250.; \
}
#define FAKE_JOYSTICK
static void booz_sim_set_ppm_from_joystick( void ) {
#ifndef FAKE_JOYSTICK
ppm_pulses[RADIO_THROTTLE] = 1223 + booz_joystick_value[JS_THROTTLE] * (2050-1223);
@@ -188,31 +271,12 @@ static void booz_sim_set_ppm_from_joystick( void ) {
// printf("joystick pitch %f %d\n", booz_joystick_value[JS_PITCH], ppm_pulses[RADIO_PITCH]);
// printf("joystick roll %f %d\n", booz_joystick_value[JS_ROLL], ppm_pulses[RADIO_ROLL]);
#else
int foo = sim_time/10;
// ppm_pulses[RADIO_YAW] = 1493 + 0.4 * (2050-950);
// ppm_pulses[RADIO_MODE] = 1500 - 0.5 * (2050-950);
switch(foo%4) {
case 0:
// ppm_pulses[RADIO_THROTTLE] = 1223 + 0.25 * (2050-1223);
// ppm_pulses[RADIO_ROLL] = 1500 + 0.2 * (2050-950);
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950);
break;
case 1:
// ppm_pulses[RADIO_THROTTLE] = 1223 + 0.25 * (2050-1223);
// ppm_pulses[RADIO_ROLL] = 1500 - 0.2 * (2050-950);
ppm_pulses[RADIO_PITCH] = 1500 + 0.0 * (2050-950);
break;
case 2:
// ppm_pulses[RADIO_THROTTLE] = 1223 + 0.9 * (2050-1223);
// ppm_pulses[RADIO_ROLL] = 1500 - 0.2 * (2050-950);
ppm_pulses[RADIO_PITCH] = 1500 - 0.2 * (2050-950);
break;
case 3:
// ppm_pulses[RADIO_THROTTLE] = 1223 + 0.9 * (2050-1223);
// ppm_pulses[RADIO_ROLL] = 1500 + 0.2 * (2050-950);
ppm_pulses[RADIO_PITCH] = 1500 - 0.0 * (2050-950);
break;
}
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.6 * (2050-1223);
BREAK_MTT();
WALK_OVAL();
// CIRCLE();
// STATIONARY();
// TOUPIE();
#endif
}