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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-05 01:06:19 +00:00
parent f0e61f4add
commit 3e39cb29d1
5 changed files with 242 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+57 -12
View File
@@ -332,18 +332,63 @@
</message>
<message name="IMU_MAG" ID="201">
<field name="ax" type="int16" />
<field name="ay" type="int16" />
<field name="az" type="int16" />
<field name="mx" type="int16" />
<field name="my" type="int16" />
<field name="mz" type="int16" />
</message>
<message name="IMU_ACCEL" ID="202">
<field name="ax" type="float" unit="rad/s"/>
<field name="ay" type="float" unit="rad/s"/>
<field name="az" type="float" unit="rad/s"/>
<field name="ax" type="float" unit="m/s-2"/>
<field name="ay" type="float" unit="m/s-2"/>
<field name="az" type="float" unit="m/s-2"/>
</message>
<message name="AHRS_STATE" ID="203">
<message name="IMU_GYRO_RAW" ID="203">
<field name="gx" type="uint16" unit="adc"/>
<field name="gy" type="uint16" unit="adc"/>
<field name="gz" type="uint16" unit="adc"/>
</message>
<message name="IMU_ACCEL_RAW" ID="204">
<field name="ax" type="uint16" unit="adc"/>
<field name="ay" type="uint16" unit="adc"/>
<field name="az" type="uint16" unit="adc"/>
</message>
<message name="IMU_MAG_RAW" ID="205">
<field name="mx" type="int16" unit="adc"/>
<field name="my" type="int16" unit="adc"/>
<field name="mz" type="int16" unit="adc"/>
</message>
<message name="IMU_GYRO_RAW_AVG" ID="206">
<field name="gx" type="uint16" unit="adc"/>
<field name="gy" type="uint16" unit="adc"/>
<field name="gz" type="uint16" unit="adc"/>
<field name="vgx" type="float" unit="adc" format="%.0f"/>
<field name="vgy" type="float" unit="adc" format="%.0f"/>
<field name="vgz" type="float" unit="adc" format="%.0f"/>
</message>
<message name="IMU_ACCEL_RAW_AVG" ID="207">
<field name="ax" type="uint16" unit="adc"/>
<field name="ay" type="uint16" unit="adc"/>
<field name="az" type="uint16" unit="adc"/>
<field name="vax" type="float" unit="adc"/>
<field name="vay" type="float" unit="adc"/>
<field name="vaz" type="float" unit="adc"/>
</message>
<message name="IMU_MAG_RAW_AVG" ID="208">
<field name="mx" type="int16" unit="adc"/>
<field name="my" type="int16" unit="adc"/>
<field name="mz" type="int16" unit="adc"/>
<field name="vmx" type="float" unit="adc"/>
<field name="vmy" type="float" unit="adc"/>
<field name="vmz" type="float" unit="adc"/>
</message>
<message name="AHRS_STATE" ID="210">
<field name="q0" type="float"/>
<field name="q1" type="float"/>
<field name="q2" type="float"/>
@@ -353,7 +398,7 @@
<field name="bz" type="float"/>
</message>
<message name="AHRS_COV" ID="204">
<message name="AHRS_COV" ID="211">
<field name="p00" type="float"/>
<field name="p11" type="float"/>
<field name="p22" type="float"/>
@@ -363,14 +408,14 @@
<field name="p66" type="float"/>
</message>
<message name="TIME" ID="206">
<message name="TIME" ID="212">
<field name="t" type="uint32"/>
</message>
<message name="AHRS_OVERRUN" ID="207">
<message name="AHRS_OVERRUN" ID="213">
</message>
<message name="ENOSE_STATUS" ID="210">
<message name="ENOSE_STATUS" ID="218">
<field name="val1" type="uint16"/>
<field name="val2" type="uint16"/>
<field name="val3" type="uint16"/>
@@ -378,7 +423,7 @@
<field name="heat" type="uint8[]"/>
</message>
<message name="DPICCO_STATUS" ID="211">
<message name="DPICCO_STATUS" ID="219">
<field name="humid" type="uint16"/>
<field name="temp" type="uint16"/>
</message>
sw/airborne/i2c.h
+1 -1
View File
@@ -28,7 +28,7 @@ extern void i2c_transmit(uint8_t slave_addr, uint8_t len, volatile bool_t* finis
extern volatile uint8_t i2c_status;
#ifndef I2C_BUF_LEN
#define I2C_BUF_LEN 8
#define I2C_BUF_LEN 16
#endif
extern volatile uint8_t i2c_buf[I2C_BUF_LEN];
sw/airborne/imu_v3.c
+104
View File
@@ -12,14 +12,118 @@ int16_t imu_mag[AXIS_NB];
/* battery in volts */
float imu_bat;
uint16_t imu_accel_raw[AXIS_NB];
uint16_t imu_gyro_raw[AXIS_NB];
int16_t imu_mag_raw[AXIS_NB];
struct adc_buf buf_ax;
struct adc_buf buf_ay;
struct adc_buf buf_az;
struct adc_buf buf_bat;
#define IMU_DETECT_STILL_LEN 256
bool_t imu_vehicle_still;
uint16_t imu_vs_accel_raw[AXIS_NB][IMU_DETECT_STILL_LEN];
uint32_t imu_vs_accel_raw_sum[AXIS_NB];
uint32_t imu_vs_accel_raw_avg[AXIS_NB];
float imu_vs_accel_raw_var[AXIS_NB];
uint16_t imu_vs_gyro_raw[AXIS_NB][IMU_DETECT_STILL_LEN];
uint32_t imu_vs_gyro_raw_sum[AXIS_NB];
uint32_t imu_vs_gyro_raw_avg[AXIS_NB];
float imu_vs_gyro_raw_var[AXIS_NB];
int16_t imu_vs_mag_raw[AXIS_NB][IMU_DETECT_STILL_LEN];
int32_t imu_vs_mag_raw_sum[AXIS_NB];
int32_t imu_vs_mag_raw_avg[AXIS_NB];
float imu_vs_mag_raw_var[AXIS_NB];
static uint8_t imu_vs_buf_head;
static bool_t imu_vs_buf_filled;
#define IMU_VS_ACCEL_VAR_MAX 1100.
void imu_init(void) {
adc_buf_channel(ADC_CHANNEL_AX, &buf_ax, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(ADC_CHANNEL_AY, &buf_ay, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(ADC_CHANNEL_AZ, &buf_az, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(ADC_CHANNEL_BAT, &buf_bat, DEFAULT_AV_NB_SAMPLE);
imu_vehicle_still = FALSE;
imu_vs_buf_filled = FALSE;
imu_vs_buf_head = 0;
uint8_t i, j;
for (i=0; i<AXIS_NB; i++) {
imu_vs_accel_raw_sum[i] = 0;
imu_vs_gyro_raw_sum[i] = 0;
imu_vs_mag_raw_sum[i] = 0;
for (j=0; j<IMU_DETECT_STILL_LEN; j++) {
imu_vs_accel_raw[i][j] = 0;
imu_vs_gyro_raw[i][j] = 0;
imu_vs_mag_raw[i][j] = 0;
}
}
}
void imu_detect_vehicle_still(void) {
/* update the sliding average of sensors readings */
imu_vs_buf_head++;
if (imu_vs_buf_head >= IMU_DETECT_STILL_LEN) {
imu_vs_buf_filled = TRUE;
imu_vs_buf_head = 0;
}
uint8_t i, j;
for (i=0; i<AXIS_NB; i++) {
imu_vs_accel_raw_sum[i] -= imu_vs_accel_raw[i][imu_vs_buf_head];
imu_vs_accel_raw[i][imu_vs_buf_head] = imu_accel_raw[i];
imu_vs_accel_raw_sum[i] += imu_accel_raw[i];
imu_vs_gyro_raw_sum[i] -= imu_vs_gyro_raw[i][imu_vs_buf_head];
imu_vs_gyro_raw[i][imu_vs_buf_head] = imu_gyro_raw[i];
imu_vs_gyro_raw_sum[i] += imu_gyro_raw[i];
imu_vs_mag_raw_sum[i] -= imu_vs_mag_raw[i][imu_vs_buf_head];
imu_vs_mag_raw[i][imu_vs_buf_head] = imu_mag_raw[i];
imu_vs_mag_raw_sum[i] += imu_mag_raw[i];
}
/* compute variance */
if (imu_vs_buf_filled) {
for (i=0; i<AXIS_NB; i++) {
imu_vs_accel_raw_avg[i] = imu_vs_accel_raw_sum[i] / IMU_DETECT_STILL_LEN;
imu_vs_gyro_raw_avg[i] = imu_vs_gyro_raw_sum[i] / IMU_DETECT_STILL_LEN;
imu_vs_mag_raw_avg[i] = imu_vs_mag_raw_sum[i] / IMU_DETECT_STILL_LEN;
imu_vs_accel_raw_var[i] = 0.;
imu_vs_gyro_raw_var[i] = 0.;
imu_vs_mag_raw_var[i] = 0.;
for (j=0; j<IMU_DETECT_STILL_LEN; j++) {
int32_t diff = imu_vs_gyro_raw[i][j] - imu_vs_gyro_raw_avg[i];
imu_vs_gyro_raw_var[i] += (float)(diff*diff);
diff = imu_vs_accel_raw[i][j] - imu_vs_accel_raw_avg[i];
imu_vs_accel_raw_var[i] += (float)(diff*diff);
diff = imu_vs_mag_raw[i][j] - imu_vs_mag_raw_avg[i];
imu_vs_mag_raw_var[i] += (float)(diff*diff);
}
imu_vs_gyro_raw_var[i] /= (float)IMU_DETECT_STILL_LEN;
imu_vs_accel_raw_var[i] /= (float)IMU_DETECT_STILL_LEN;
imu_vs_mag_raw_var[i] /= (float)IMU_DETECT_STILL_LEN;
}
if (imu_vs_accel_raw_var[0] < IMU_VS_ACCEL_VAR_MAX &&
imu_vs_accel_raw_var[1] < IMU_VS_ACCEL_VAR_MAX &&
imu_vs_accel_raw_var[2] < IMU_VS_ACCEL_VAR_MAX )
imu_vehicle_still = TRUE;
}
}
sw/airborne/imu_v3.h
+46 -28
View File
@@ -1,20 +1,30 @@
#ifndef IMU_V3_H
#define IMU_V3_H
#include <inttypes.h>
#include "std.h"
#include "6dof.h"
/* accelerometers in arbitrary unit */
extern float imu_accel[AXIS_NB];
/* gyros in rad/s */
extern float imu_gyro[AXIS_NB];
/* magnetometer in arbitrary unit */
extern int16_t imu_mag[AXIS_NB];
/* battery in volts */
extern float imu_bat;
/* 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_bat; /* battery in volts */
/* raw sensors readings */
extern uint16_t imu_accel_raw[AXIS_NB];
extern uint16_t imu_gyro_raw[AXIS_NB];
extern int16_t imu_mag_raw[AXIS_NB];
extern void imu_init(void);
extern void imu_detect_vehicle_still(void);
extern bool_t imu_vehicle_still;
/* sliding average for still vehicle detection */
extern uint32_t imu_vs_accel_raw_avg[AXIS_NB];
extern float imu_vs_accel_raw_var[AXIS_NB];
extern uint32_t imu_vs_gyro_raw_avg[AXIS_NB];
extern float imu_vs_gyro_raw_var[AXIS_NB];
extern int32_t imu_vs_mag_raw_avg[AXIS_NB];
extern float imu_vs_mag_raw_var[AXIS_NB];
extern struct adc_buf buf_ax;
extern struct adc_buf buf_ay;
@@ -26,21 +36,24 @@ extern struct adc_buf buf_bat;
#define IMU_ACCEL_Z_NEUTRAL 506
#if 0
#define ImuUpdateAccels() { \
int_disable(); \
imu_accel[AXIS_X]= buf_ax.sum; \
imu_accel[AXIS_Y]= buf_ay.sum; \
imu_accel[AXIS_Z]= buf_az.sum; \
int_enable(); \
imu_accel[AXIS_X] = -((imu_accel[AXIS_X] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_X_NEUTRAL); \
imu_accel[AXIS_Y] = (imu_accel[AXIS_Y] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \
imu_accel[AXIS_Z] = (imu_accel[AXIS_Z] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \
#define ImuUpdateAccels() { \
int_disable(); \
imu_accel_raw[AXIS_X]= buf_ax.sum; \
imu_accel_raw[AXIS_Y]= buf_ay.sum; \
imu_accel_raw[AXIS_Z]= buf_az.sum; \
int_enable(); \
imu_accel[AXIS_X] = -((imu_accel_raw[AXIS_X] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_X_NEUTRAL); \
imu_accel[AXIS_Y] = (imu_accel_raw[AXIS_Y] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \
imu_accel[AXIS_Z] = (imu_accel_raw[AXIS_Z] / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \
}
#else
#define ImuUpdateAccels() { \
#define ImuUpdateAccels() { \
imu_accel_raw[AXIS_X]= buf_ax.sum; \
imu_accel_raw[AXIS_Y]= buf_ay.sum; \
imu_accel_raw[AXIS_Z]= buf_az.sum; \
imu_accel[AXIS_X]= -((buf_ax.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_X_NEUTRAL); \
imu_accel[AXIS_Y]= (buf_ay.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \
imu_accel[AXIS_Z]= (buf_az.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \
imu_accel[AXIS_Y]= (buf_ay.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Y_NEUTRAL; \
imu_accel[AXIS_Z]= (buf_az.sum / DEFAULT_AV_NB_SAMPLE) - IMU_ACCEL_Z_NEUTRAL; \
}
#endif
@@ -53,8 +66,10 @@ extern struct adc_buf buf_bat;
#define IMU_GYRO_Y_GAIN -0.000214915108532129801
#define IMU_GYRO_Z_GAIN 0.0002104
#define ImuUpdateGyros() { \
\
#define ImuUpdateGyros() { \
imu_gyro_raw[AXIS_X] = max1167_values[0]; \
imu_gyro_raw[AXIS_Y] = max1167_values[1]; \
imu_gyro_raw[AXIS_Z] = max1167_values[2]; \
imu_gyro[AXIS_X] = (float)((int32_t)max1167_values[0] - IMU_GYRO_X_NEUTRAL) \
* IMU_GYRO_X_GAIN; \
imu_gyro[AXIS_Y] = (float)((int32_t)max1167_values[1] - IMU_GYRO_Y_NEUTRAL) \
@@ -63,10 +78,13 @@ extern struct adc_buf buf_bat;
* IMU_GYRO_Z_GAIN; \
}
#define ImuUpdateMag() { \
imu_mag[AXIS_X] = -micromag_values[0]; \
imu_mag[AXIS_Y] = -micromag_values[1]; \
imu_mag[AXIS_Z] = micromag_values[2]; \
#define ImuUpdateMag() { \
imu_mag_raw[AXIS_X] = micromag_values[0]; \
imu_mag_raw[AXIS_Y] = micromag_values[1]; \
imu_mag_raw[AXIS_Z] = micromag_values[2]; \
imu_mag[AXIS_X] = -micromag_values[0]; \
imu_mag[AXIS_Y] = -micromag_values[1]; \
imu_mag[AXIS_Z] = micromag_values[2]; \
}
sw/airborne/main_imu.c
+34 -7
View File
@@ -19,9 +19,15 @@
#include "messages.h"
#include "downlink.h"
#define SEND_ACCEL 1
#define SEND_MAG 1
#define SEND_GYRO 1
//#define SEND_ACCEL 1
//#define SEND_MAG 1
//#define SEND_GYRO 1
#define SEND_ACCEL_RAW 1
#define SEND_MAG_RAW 1
#define SEND_GYRO_RAW 1
#define SEND_ACCEL_RAW_AVG 1
#define SEND_MAG_RAW_AVG 1
#define SEND_GYRO_RAW_AVG 1
#define SEND_AHRS_STATE 1
#define SEND_AHRS_COV 1
@@ -82,6 +88,9 @@ static inline void main_event_task( void ) {
ImuUpdateMag();
#ifdef SEND_MAG
DOWNLINK_SEND_IMU_MAG(&imu_mag[AXIS_X], &imu_mag[AXIS_Y], &imu_mag[AXIS_Z]);
#endif
#ifdef SEND_MAG_RAW
DOWNLINK_SEND_IMU_MAG_RAW(&imu_mag_raw[AXIS_X], &imu_mag_raw[AXIS_Y], &imu_mag_raw[AXIS_Z]);
#endif
spi_cur_slave = SPI_SLAVE_MAX;
max1167_read();
@@ -94,11 +103,18 @@ static inline void main_event_task( void ) {
ImuUpdateGyros();
#ifdef SEND_GYRO
DOWNLINK_SEND_IMU_GYRO(&imu_gyro[AXIS_X], &imu_gyro[AXIS_Y], &imu_gyro[AXIS_Z]);
#endif
#ifdef SEND_GYRO_RAW
DOWNLINK_SEND_IMU_GYRO_RAW(&imu_gyro_raw[AXIS_X], &imu_gyro_raw[AXIS_Y], &imu_gyro_raw[AXIS_Z]);
#endif
ImuUpdateAccels();
#ifdef SEND_ACCEL
DOWNLINK_SEND_IMU_ACCEL(&imu_accel[AXIS_X], &imu_accel[AXIS_Y], &imu_accel[AXIS_Z]);
#endif
#ifdef SEND_ACCEL_RAW
DOWNLINK_SEND_IMU_ACCEL_RAW(&imu_accel_raw[AXIS_X], &imu_accel_raw[AXIS_Y], &imu_accel_raw[AXIS_Z]);
#endif
ahrs_task();
link_imu_send();
@@ -124,11 +140,22 @@ static inline void main_periodic_task( void ) {
}
static inline void ahrs_task( void ) {
/* discard first 100 measures */
if (ahrs_step == AHRS_STEP_UNINIT) {
static uint8_t init_count = 0;
init_count++;
if (init_count > 100) {
imu_detect_vehicle_still();
#ifdef SEND_GYRO_RAW_AVG
DOWNLINK_SEND_IMU_GYRO_RAW_AVG(&imu_vs_gyro_raw_avg[AXIS_X], &imu_vs_gyro_raw_avg[AXIS_Y], &imu_vs_gyro_raw_avg[AXIS_Z], \
&imu_vs_gyro_raw_var[AXIS_X], &imu_vs_gyro_raw_var[AXIS_Y], &imu_vs_gyro_raw_var[AXIS_Z]);
#endif
#ifdef SEND_ACCEL_RAW_AVG
DOWNLINK_SEND_IMU_ACCEL_RAW_AVG(&imu_vs_accel_raw_avg[AXIS_X], &imu_vs_accel_raw_avg[AXIS_Y], &imu_vs_accel_raw_avg[AXIS_Z], \
&imu_vs_accel_raw_var[AXIS_X], &imu_vs_accel_raw_var[AXIS_Y], &imu_vs_accel_raw_var[AXIS_Z]);
#endif
#ifdef SEND_MAG_RAW_AVG
DOWNLINK_SEND_IMU_MAG_RAW_AVG(&imu_vs_mag_raw_avg[AXIS_X], &imu_vs_mag_raw_avg[AXIS_Y], &imu_vs_mag_raw_avg[AXIS_Z], \
&imu_vs_mag_raw_var[AXIS_X], &imu_vs_mag_raw_var[AXIS_Y], &imu_vs_mag_raw_var[AXIS_Z]);
#endif
if (imu_vehicle_still) {
ahrs_step = AHRS_STEP_ROLL;
afe_init(imu_mag, imu_accel, imu_gyro);
}