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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2008-01-26 03:39:44 +00:00
parent 6096776a42
commit e97f91250b
13 changed files with 172 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/conf_booz.makefile
+3 -4
View File
@@ -28,9 +28,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.CFLAGS += -I2C_BUF_LEN=32 -DI2C_SCLL=150 -DI2C_SCLH=150 -DI2C_VIC_SLOT=10
#flt.CFLAGS += -DDISABLE_MAGNETOMETER
flt.CFLAGS += -DUSE_AMI601 -I2C_BUF_LEN=32 -DI2C_SCLL=150 -DI2C_SCLH=150 -DI2C_VIC_SLOT=10
flt.srcs += AMI601.c i2c.c $(SRC_ARCH)/i2c_hw.c
flt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
@@ -90,7 +89,7 @@ ctl.srcs += booz_link_mcu.c $(SRC_ARCH)/booz_link_mcu_hw.c
ctl.srcs += spi.c $(SRC_ARCH)/spi_hw.c
ctl.srcs += commands.c
ctl.CFLAGS += -DDISABLE_NAV
ctl.CFLAGS += -DDISABLE_NAV -DDISABLE_PSI_CONTROL
ctl.srcs += booz_estimator.c \
booz_control.c \
booz_nav.c \
conf/messages.xml
+7 -3
View File
@@ -291,9 +291,9 @@
</message>
<message name="IMU_MAG" ID="201">
<field name="mx" type="int16" />
<field name="my" type="int16" />
<field name="mz" type="int16" />
<field name="mx" type="float" />
<field name="my" type="float" />
<field name="mz" type="float" />
</message>
<message name="IMU_ACCEL" ID="202">
@@ -535,6 +535,10 @@
<field name="vz" type="float" unit="m/s"/>
</message>
<message name="BOOZ_DEBUG_FOO" ID="245">
<field name="ami_stat" type="uint8"/>
</message>
<message name="ENOSE_STATUS" ID="250">
<field name="val1" type="uint16"/>
<field name="val2" type="uint16"/>
conf/telemetry/booz.xml
+7 -5
View File
@@ -28,12 +28,14 @@
<process name="Filter">
<mode name="default">
<!-- <message name="IMU_GYRO" period=".017"/> -->
<message name="IMU_GYRO_RAW" period=".017"/>
<!-- <message name="IMU_GYRO_RAW" period=".017"/> -->
<!-- <message name="IMU_GYRO_LP" period=".017"/> -->
<!-- <message name="AHRS_STATE" period=".05"/> -->
<!-- <message name="AHRS_COV" period=".2"/> -->
<!-- <message name="AHRS_MEASURE" period=".05"/> -->
<message name="IMU_ACCEL_RAW" period=".017"/>
<message name="AHRS_STATE" period=".05"/>
<message name="AHRS_COV" period=".2"/>
<message name="AHRS_MEASURE" period=".05"/>
<!-- <message name="IMU_ACCEL_RAW" period=".2"/> -->
<message name="IMU_MAG_RAW" period=".2"/>
<message name="IMU_MAG" period=".2"/>
</mode>
<mode name="raw_sensors">
<message name="IMU_GYRO_RAW" period=".017"/>
sw/airborne/AMI601.c
+14 -86
View File
@@ -1,33 +1,20 @@
#include "AMI601.h"
#include "i2c.h"
#include "led.h"
#include <string.h>
#define FOO_DELAY() { \
uint16_t foo = 1; \
while (foo) foo++; \
}
#define AMI601_SLAVE_ADDR 0x60
float ami601_ax;
float ami601_ay;
float ami601_az;
float ami601_mx;
float ami601_my;
float ami601_mz;
uint8_t ami601_foo1;
uint8_t ami601_foo2;
uint8_t ami601_foo3;
uint16_t ami601_val[AMI601_NB_CHAN];
static volatile bool_t ami601_i2c_done;
volatile uint8_t ami601_status;
volatile bool_t ami601_i2c_done;
volatile uint32_t ami601_nb_err;
/* TRG ( trigger) P0.28 */
#define AMI601_TRG_PIN 28
@@ -62,83 +49,24 @@ void ami601_init( void ) {
ami601_val[i] = 0;
}
ami601_i2c_done = TRUE;
ami601_nb_err = 0;
ami601_status = AMI601_IDLE;
// FOO_DELAY();
/* assert reset */
// SetBit(AMI601_RST_IOSET , AMI601_RST_PIN);
}
void ami601_periodic( void ) {
/* pulse TRG */
// SetBit(AMI601_TRG_IOCLR , AMI601_TRG_PIN);
// FOO_DELAY();
// SetBit(AMI601_TRG_IOSET , AMI601_TRG_PIN);
if (ami601_i2c_done) {
// LED_ON(2);
void ami601_read( void ) {
if (ami601_status != AMI601_IDLE) {
ami601_nb_err++;
}
else {
ami601_i2c_done = FALSE;
ami601_status = AMI601_SENDING_REQ;
const uint8_t read_cmd[] = { 0x55, 0xAA, 0X14};
memcpy((void*)i2c_buf, read_cmd, sizeof(read_cmd));
i2c_transmit(AMI601_SLAVE_ADDR, sizeof(read_cmd), &ami601_i2c_done);
ami601_i2c_done = FALSE;
while (!ami601_i2c_done);
FOO_DELAY();
i2c_receive(AMI601_SLAVE_ADDR, 15, &ami601_i2c_done);
while (!ami601_i2c_done);
// LED_OFF(2);
ami601_foo1 = i2c_buf[0]; // AA ?
ami601_foo2 = i2c_buf[1]; // 55 ?
ami601_foo3 = i2c_buf[2]; // ERR ?
uint8_t i;
for (i=0; i< AMI601_NB_CHAN; i++) {
ami601_val[i] = i2c_buf[3 + 2 * i];
ami601_val[i] += i2c_buf[3 + 2 * i + 1] * 256;
}
}
}
void ami601_send_sleep( void ) {
}
#define AX_NEUTRAL 2323
#define AY_NEUTRAL 2323
#define AZ_NEUTRAL 2323
#define AX_GAIN 0.0283936
#define AY_GAIN -0.0283936
#define AZ_GAIN 0.0283936
#define MX_NEUTRAL 2048
#define MY_NEUTRAL 2048
#define MZ_NEUTRAL 2048
#define MX_GAIN -1.
#define MY_GAIN 1.
#define MZ_GAIN 1.
void ami601_scale_measures(void) {
ami601_ax = (ami601_val[3] - AX_NEUTRAL) * AX_GAIN;
ami601_ay = (ami601_val[5] - AY_NEUTRAL) * AY_GAIN;
ami601_az = (ami601_val[1] - AZ_NEUTRAL) * AZ_GAIN;
ami601_mx = (ami601_val[0] - MX_NEUTRAL) * MX_GAIN;
ami601_my = (ami601_val[4] - MY_NEUTRAL) * MY_GAIN;
ami601_mz = (ami601_val[2] - MZ_NEUTRAL) * MZ_GAIN;
}
sw/airborne/AMI601.h
+50
View File
@@ -2,9 +2,11 @@
#define AMI601_H
#include "std.h"
#include "i2c.h"
extern void ami601_init( void );
extern void ami601_read( void );
extern void ami601_periodic( void );
extern void ami601_scale_measures(void);
@@ -22,5 +24,53 @@ extern float ami601_mx;
extern float ami601_my;
extern float ami601_mz;
#define AMI601_IDLE 0
#define AMI601_SENDING_REQ 1
#define AMI601_WAITING_MEASURE 2
#define AMI601_READING_MEASURE 3
#define AMI601_DATA_AVAILABLE 4
extern volatile uint8_t ami601_status;
extern volatile bool_t ami601_i2c_done;
extern volatile uint32_t ami601_nb_err;
#define AMI601_SLAVE_ADDR 0x60
#define AMI601EventCheckAndHandle() { \
switch (ami601_status) { \
case AMI601_SENDING_REQ : \
if ( ami601_i2c_done ) { \
/* trigger delay for measurement */ \
T0MR1 = T0TC + SYS_TICS_OF_USEC(4096); \
/* clear match 1 interrupt */ \
T0IR = TIR_MR1I; \
/* enable match 1 interrupt */ \
T0MCR |= TMCR_MR1_I; \
ami601_status = AMI601_WAITING_MEASURE; \
} \
break; \
case AMI601_READING_MEASURE : \
if ( ami601_i2c_done ) { \
ami601_foo1 = i2c_buf[0]; /* AA ? */ \
ami601_foo2 = i2c_buf[1]; /* 55 ? */ \
ami601_foo3 = i2c_buf[2]; /* ERR ? */ \
uint8_t i; \
for (i=0; i< AMI601_NB_CHAN; i++) { \
ami601_val[i] = i2c_buf[3 + 2 * i]; \
ami601_val[i] += i2c_buf[3 + 2 * i + 1] * 256; \
} \
ami601_status = AMI601_DATA_AVAILABLE; \
} \
break; \
} \
}
#define AMI601ReadMeasure() { \
/* disable match 1 interrupt */ \
/* T0MCR |= TMCR_MR1_I; */ \
ami601_i2c_done = FALSE; \
ami601_status = AMI601_READING_MEASURE; \
i2c_receive(AMI601_SLAVE_ADDR, 15, &ami601_i2c_done); \
}
#endif /* AMI601_H */
sw/airborne/arm7/sys_time_hw.c
+10
View File
@@ -22,6 +22,10 @@ uint32_t last_periodic_event;
#define TIMER0_IT_MASK (TIR_CR2I | TIR_MR1I)
#endif /* MB_TACHO */
#ifdef USE_AMI601
#include "AMI601.h"
#endif
void TIMER0_ISR ( void ) {
ISR_ENTRY();
@@ -62,6 +66,12 @@ void TIMER0_ISR ( void ) {
T0IR = TIR_CR0I;
}
#endif /* MB_TACHO */
#ifdef USE_AMI601
if (T0IR&TIR_MR1I) {
AMI601ReadMeasure();
T0IR = TIR_MR1I;
}
#endif /* USE_AMI601 */
}
VICVectAddr = 0x00000000;
sw/airborne/booz_ahrs.h
+2 -2
View File
@@ -26,8 +26,8 @@ extern FLOAT_T booz_ahrs_measure_theta;
extern FLOAT_T booz_ahrs_measure_psi;
extern void booz_ahrs_init(void);
extern void booz_ahrs_start( const float* accel, const float* gyro, const int16_t* mag);
extern void booz_ahrs_run(const float* accel, const float* gyro, const int16_t* mag);
extern void booz_ahrs_start( const float* accel, const float* gyro, const float* mag);
extern void booz_ahrs_run(const float* accel, const float* gyro, const float* mag);
/*
extern void booz_ahrs_predict( const float* gyro );
extern void booz_ahrs_update( const float* accel, const int16_t* mag );
sw/airborne/booz_control.c
+4 -4
View File
@@ -116,12 +116,12 @@ void booz_control_attitude_read_setpoints_from_rc(void) {
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
booz_control_attitude_theta_sp = rc_values[RADIO_PITCH] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PHI_THETA_MAX_SP)/MAX_PPRZ;
#ifndef DISABLE_MAGNETOMETER
#ifndef DISABLE_PSI_CONTROL
booz_control_attitude_psi_sp = rc_values[RADIO_YAW] *
RadOfDeg(BOOZ_CONTROL_ATTITUDE_PSI_MAX_SP)/MAX_PPRZ;
#else
booz_control_r_sp = -rc_values[RADIO_YAW] * RadOfDeg(BOOZ_CONTROL_RATE_R_MAX_SP)/MAX_PPRZ;
#endif /* DISABLE_MAGNETOMETER */
#endif /* DISABLE_PSI_CONTROL */
booz_control_power_sp = rc_values[RADIO_THROTTLE] / (float)MAX_PPRZ;
}
@@ -136,7 +136,7 @@ void booz_control_attitude_run(void) {
booz_control_attitude_phi_theta_dgain * booz_estimator_q;
#ifndef DISABLE_MAGNETOMETER
#ifndef DISABLE_PSI_CONTROL
float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp;
NormRadAngle(att_err_psi);
const float cmd_r = booz_control_attitude_psi_pgain * att_err_psi +
@@ -146,7 +146,7 @@ void booz_control_attitude_run(void) {
const float rate_d_err_r = rate_err_r - booz_control_rate_last_err_r;
booz_control_rate_last_err_r = rate_err_r;
const float cmd_r = booz_control_rate_r_pgain * ( rate_err_r + booz_control_rate_r_dgain * rate_d_err_r );
#endif /* DISABLE_MAGNETOMETER */
#endif /* DISABLE_PSI_CONTROL */
booz_control_commands[COMMAND_P] = TRIM_PPRZ((int16_t)cmd_p);
booz_control_commands[COMMAND_Q] = TRIM_PPRZ((int16_t)cmd_q);
sw/airborne/booz_filter_main.c
+3
View File
@@ -74,6 +74,8 @@ STATIC_INLINE void booz_filter_main_periodic_task( void ) {
case 0:
booz_filter_telemetry_periodic_task();
break;
<<<<<<< booz_filter_main.c
=======
#ifndef SITL
// case 1:
/* triger measurements */
@@ -81,6 +83,7 @@ STATIC_INLINE void booz_filter_main_periodic_task( void ) {
// Z Y X
// DOWNLINK_SEND_IMU_MAG_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]);
#endif
>>>>>>> 1.11
}
}
sw/airborne/imu_v3.c
+2 -6
View File
@@ -5,13 +5,9 @@
#include CONFIG
#include "adc.h"
/* accelerometers in arbitrary unit */
float imu_accel[AXIS_NB];
/* gyros in rad/s */
float imu_gyro[AXIS_NB];
/* magnetometer in arbitrary unit */
int16_t imu_mag[AXIS_NB];
/* battery in volts */
float imu_gyro[AXIS_NB];
float imu_mag[AXIS_NB];
float imu_bat;
float imu_gyro_prev[AXIS_NB];
sw/airborne/imu_v3.h
+46 -17
View File
@@ -11,7 +11,7 @@
/* calibrated sensor readings */
extern float imu_accel[AXIS_NB]; /* accelerometers in arbitrary unit */
extern float imu_gyro[AXIS_NB]; /* gyros in rad/s */
extern int16_t imu_mag[AXIS_NB]; /* magnetometer in arbitrary unit */
extern float imu_mag[AXIS_NB]; /* magnetometer in arbitrary unit */
extern float imu_bat; /* battery in volts */
extern float imu_gyro_prev[AXIS_NB]; /* previous gyros in rad/s */
@@ -92,6 +92,7 @@ extern struct adc_buf buf_bat;
( 1. - IMU_GYRO_LP_ALPHA) * imu_gyro_lp[AXIS_R]; \
}
#ifdef USE_MICROMAG
#define ImuUpdateMag() { \
imu_mag_raw[AXIS_X] = micromag_values[0]; \
imu_mag_raw[AXIS_Y] = micromag_values[1]; \
@@ -100,7 +101,31 @@ extern struct adc_buf buf_bat;
imu_mag[AXIS_Y] = -micromag_values[1]; \
imu_mag[AXIS_Z] = micromag_values[2]; \
}
#endif /* USE_MICROMAG */
#ifdef USE_AMI601
#define AMI_MCX 2
#define AMI_MCY 4
#define AMI_MCZ 0
#define AMI_MNX 1977.5055
#define AMI_MNY 2145.2051
#define AMI_MNZ 1931.1511
#define AMI_MGX 447.26642
#define AMI_MGY 450.52012
#define AMI_MGZ 467.83817
#define ImuUpdateMag() { \
imu_mag_raw[AXIS_X] = ami601_val[AMI_MCX]; \
imu_mag_raw[AXIS_Y] = ami601_val[AMI_MCY]; \
imu_mag_raw[AXIS_Z] = ami601_val[AMI_MCZ]; \
imu_mag[AXIS_X] = AMI_MGX * ((float)imu_mag_raw[AXIS_X] - AMI_MNX); \
imu_mag[AXIS_Y] = AMI_MGY * ((float)imu_mag_raw[AXIS_Y] - AMI_MNY); \
imu_mag[AXIS_Z] = AMI_MGZ * ((float)imu_mag_raw[AXIS_Z] - AMI_MNZ); \
}
#endif /* USE_AMI601 */
#define IMU_BAT_NEUTRAL 2
#define IMU_BAT_GAIN .010101
@@ -134,24 +159,28 @@ extern struct adc_buf buf_bat;
}
#define ImuEventCheckAndHandle(user_handler) { \
if (max1167_status == STA_MAX1167_DATA_AVAILABLE) { \
max1167_status = STA_MAX1167_IDLE; \
ImuUpdateGyros(); \
ImuUpdateAccels(); \
ImuUpdateMag(); \
user_handler(); \
} \
#define ImuEventCheckAndHandle(user_handler) { \
if (max1167_status == STA_MAX1167_DATA_AVAILABLE) { \
max1167_status = STA_MAX1167_IDLE; \
ImuUpdateGyros(); \
ImuUpdateAccels(); \
user_handler(); \
} \
AMI601EventCheckAndHandle(); \
if (ami601_status == AMI601_DATA_AVAILABLE) { \
ImuUpdateMag(); \
ami601_status = AMI601_IDLE; \
} \
}
#define ImuPeriodic() { \
/* if (max1167_status != STA_MAX1167_IDLE) { \
DOWNLINK_SEND_AHRS_OVERRUN(); \
} \
else { */ \
max1167_read(); \
micromag_read(); \
/* } */ \
#define ImuPeriodic() { \
max1167_read(); \
static uint8_t imu_pre; \
imu_pre++; \
if (imu_pre > 4) { \
imu_pre=0; \
ami601_read(); \
} \
}
sw/airborne/multitilt.c
+5 -5
View File
@@ -20,7 +20,7 @@ static const float Q_bias = 0.0005;
//static const float R_accel = 0.3;
static const float R_accel = 0.5;
static inline void multitilt_update( const float* accel, const int16_t* mag );
static inline void multitilt_update( const float* accel, const float* mag );
static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, float* bias);
static inline void multitilt_predict( const float* gyro );
static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]);
@@ -40,7 +40,7 @@ static inline float theta_of_accel( const float* accel) {
return asin( accel[AXIS_X] / sqrt( g2 ) );
}
static inline float psi_of_mag( const int16_t* mag) {
static inline float psi_of_mag( const float* mag) {
/* untilt magnetometer */
const float ctheta = cos( booz_ahrs_theta );
const float stheta = sin( booz_ahrs_theta );
@@ -73,7 +73,7 @@ void booz_ahrs_init(void) {
booz_ahrs_br = 0.;
}
void booz_ahrs_start(const float* accel, const float* gyro, const int16_t* mag) {
void booz_ahrs_start(const float* accel, const float* gyro, const float* mag) {
/* reset covariance matrices */
const float cov_init[2][2] = {{1., 0.},
{0., 1.}};
@@ -104,7 +104,7 @@ void booz_ahrs_start(const float* accel, const float* gyro, const int16_t* mag)
booz_ahrs_status = BOOZ_AHRS_STATUS_RUNNING;
}
void booz_ahrs_run(const float* accel, const float* gyro, const int16_t* mag) {
void booz_ahrs_run(const float* accel, const float* gyro, const float* mag) {
multitilt_predict(gyro);
multitilt_update(accel, mag);
}
@@ -175,7 +175,7 @@ static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, flo
}
static inline void multitilt_update( const float* accel, const int16_t* mag ) {
static inline void multitilt_update( const float* accel, const float* mag ) {
booz_ahrs_measure_phi = phi_of_accel(accel);
float err_phi = booz_ahrs_measure_phi - booz_ahrs_phi;
sw/in_progress/inertial/scilab/calib_rotation.sce
+19 -16
View File
@@ -3,7 +3,7 @@ clf();
//##########
//##########
function [rm_z, rm_y, rm_x, ra_x, ra_y, ra_z] = read_log(filename)
function [rm_x, rm_y, rm_z, ra_x, ra_y, ra_z] = read_log(filename)
rm_x=[];
rm_y=[];
@@ -20,9 +20,9 @@ function [rm_z, rm_y, rm_x, ra_x, ra_y, ra_z] = read_log(filename)
if strindex(line, '#') ~= 1 & length(line) ~= 0,
[nb_scan, _mz, _my, _mx] = msscanf(1, line, '148 IMU_MAG_RAW %f %f %f');
if nb_scan == 3,
rm_z = [rm_z _mz];
rm_y = [rm_y _my];
rm_x = [rm_x _mx];
rm_y = [rm_y _my];
rm_z = [rm_z _mz];
else
[nb_scan, _ax, _ay, _az] = msscanf(1, line, '148 IMU_ACCEL_RAW %f %f %f');
if nb_scan == 3,
@@ -57,7 +57,7 @@ function [nx, ny, nz, gx, gy, gz] = min_max_calib(dx, dy, dz)
gz = (max_z - min_z) / 2.;
endfunction
[rm_z, rm_y, rm_x, ra_x, ra_y, ra_z] = read_log('log_magnetometer');
[rm_x, rm_y, rm_z, ra_x, ra_y, ra_z] = read_log('log_magnetometer');
[m_mm_nx, m_mm_ny, m_mm_nz, m_mm_gx, m_mm_gy, m_mm_gz] = ...
min_max_calib(rm_x, rm_y, rm_z);
@@ -77,21 +77,24 @@ m_mm_norm = sqrt(m_mm_x^2 + m_mm_y^2 + m_mm_z^2);
idx_m = 1:length(rm_z);
a_mm_x = (ra_x - a_mm_nx) / a_mm_gx;
a_mm_y = (ra_y - a_mm_ny) / a_mm_gy;
a_mm_z = (ra_z - a_mm_nz) / a_mm_gz;
//a_mm_x = (ra_x - a_mm_nx) / a_mm_gx;
//a_mm_y = (ra_y - a_mm_ny) / a_mm_gy;
//a_mm_z = (ra_z - a_mm_nz) / a_mm_gz;
a_mm_norm = sqrt(a_mm_x^2 + a_mm_y^2 + a_mm_z^2);
idx_a = 1:length(ra_z);
//a_mm_norm = sqrt(a_mm_x^2 + a_mm_y^2 + a_mm_z^2);
//idx_a = 1:length(ra_z);
subplot(3,2,1);
param3d(rm_x, rm_y, rm_z);
subplot(3,2,2);
param3d(ra_x, ra_y, ra_z);
plot(idx_m, rm_x);
//subplot(3,2,2);
//param3d(ra_x, ra_y, ra_z);
subplot(3,2,3);
plot(idx_m, m_mm_norm, idx_m, ones(1, length(rm_z)));
subplot(3,2,4);
plot(idx_a, a_mm_norm, idx_a, ones(1, length(ra_z)));
//subplot(3,2,4);
//plot(idx_a, a_mm_norm, idx_a, ones(1, length(ra_z)));
subplot(3,2,5);
alpha = 0:0.1:2*%pi;
@@ -99,10 +102,10 @@ c_x = m_mm_nx+ m_mm_gx * cos(alpha);
c_y = m_mm_ny+ m_mm_gy * sin(alpha);
plot(rm_x, rm_y, c_x, c_y)
subplot(3,2,6);
c_x = a_mm_nx+ a_mm_gx * cos(alpha);
c_y = a_mm_ny+ a_mm_gy * sin(alpha);
plot(ra_x, ra_y, c_x, c_y)
//subplot(3,2,6);
//c_x = a_mm_nx+ a_mm_gx * cos(alpha);
//c_y = a_mm_ny+ a_mm_gy * sin(alpha);
//plot(ra_x, ra_y, c_x, c_y)
//