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 29 Packages Projects Releases Wiki Activity

removed imu code from ahrs

Browse Source
This commit is contained in:
Christophe De Wagter
2010-12-09 02:21:04 +01:00
parent 897f59db46
commit ead6da25c9
5 changed files with 51 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/example_twog_analogimu.xml
+12 -12
View File
@@ -37,29 +37,29 @@
<section name="IMU" prefix="IMU_">
<!--
<define name="GYRO_P_CHAN" value="1"/>
<define name="GYRO_Q_CHAN" value="0"/>
<define name="GYRO_P_CHAN" value="0"/>
<define name="GYRO_Q_CHAN" value="1"/>
<define name="GYRO_R_CHAN" value="2"/>
-->
<define name="GYRO_P_NEUTRAL" value="31948"/>
<define name="GYRO_Q_NEUTRAL" value="31834"/>
<define name="GYRO_R_NEUTRAL" value="32687"/>
<define name="GYRO_P_NEUTRAL" value="512"/>
<define name="GYRO_Q_NEUTRAL" value="512"/>
<define name="GYRO_R_NEUTRAL" value="512"/>
<define name="GYRO_P_SENS" value="0.017f" integer="16"/>
<define name="GYRO_Q_SENS" value="0.017f" integer="16"/>
<define name="GYRO_R_SENS" value="0.017f" integer="16"/>
<define name="GYRO_R_SENS" value="-0.017f" integer="16"/>
<!--
<define name="ACCEL_X_CHAN" value="3"/>
<define name="ACCEL_Y_CHAN" value="5"/>
<define name="ACCEL_Z_CHAN" value="6"/>
<define name="ACCEL_Y_CHAN" value="4"/>
<define name="ACCEL_Z_CHAN" value="5"/>
-->
<define name="ACCEL_X_SENS" value="0.1f" integer="16"/>
<define name="ACCEL_Y_SENS" value="0.1f" integer="16"/>
<define name="ACCEL_Y_SENS" value="-0.1f" integer="16"/>
<define name="ACCEL_Z_SENS" value="0.1f" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="32771"/>
<define name="ACCEL_Y_NEUTRAL" value="32895"/>
<define name="ACCEL_Z_NEUTRAL" value="32148"/>
<define name="ACCEL_X_NEUTRAL" value="512"/>
<define name="ACCEL_Y_NEUTRAL" value="512"/>
<define name="ACCEL_Z_NEUTRAL" value="512"/>
</section>
conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
+2 -2
View File
@@ -4,9 +4,9 @@
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DANALOG_IMU -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -DUSE_ADC_4 -DUSE_ADC_5 -DUSE_ADC_6 -DUSE_ADC_7
ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm//dcm.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm//arduimu.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm//matrix.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm//vector.c
ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/dcm.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm/arduimu.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm/matrix.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm//vector.c
ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm//analogimu.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm//analogimu_util.c
ap.srcs += $(SRC_FIXEDWING)/subsystems/ahrs/dcm/analogimu.c $(SRC_FIXEDWING)/subsystems/imu/imu_analog.c $(SRC_FIXEDWING)/subsystems/ahrs/dcm/analogimu_util.c
endif
# since there is currently no SITL sim for the Analog IMU, we use the infrared sim
sw/airborne/subsystems/ahrs/dcm/analogimu.c
+31 -40
View File
@@ -28,32 +28,29 @@
*/
#if ! (defined SITL || defined HITL)
// Actual Inertial Measurements
#include "subsystems/imu/imu_analog.h"
// AHRS attitude computations
#include "led.h"
#include "mcu_periph/adc.h"
#include "mcu_periph/uart.h"
//#include "downlink.h"
#include "estimator.h"
//#include "ap_downlink.h"
#include "estimator.h"
#include "sys_time.h"
#include "dcm.h"
#include "analogimu_util.h"
#include "analogimu.h"
#endif
#define NB_ADC 8
#define ADC_NB_SAMPLES 16
// variables
uint16_t analog_imu_offset[NB_ADC] = {0,};
static struct adc_buf buf_adc[NB_ADC];
uint16_t analog_imu_offset[NB_ANALOG_IMU_ADC] = {0,};
int adc_average[16] = { 0 };
// remotely settable
float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT);
float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
@@ -66,9 +63,9 @@ float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
* \return accel[ACC_X], accel[ACC_Y], accel[ACC_Z]
*/
void accel2ms2( void ) {
accel[ACC_X] = (float)(adc_average[ADC_ACCX]) * IMU_ACCEL_X_SENS;
accel[ACC_Y] = (float)(-adc_average[ADC_ACCY]) * IMU_ACCEL_Y_SENS;
accel[ACC_Z] = (float)(adc_average[ADC_ACCZ]) * IMU_ACCEL_Z_SENS;
accel[ACC_X] = (float)(adc_average[3]) * IMU_ACCEL_X_SENS;
accel[ACC_Y] = (float)(adc_average[4]) * IMU_ACCEL_Y_SENS;
accel[ACC_Z] = (float)(adc_average[5]) * IMU_ACCEL_Z_SENS;
}
/**
* gyro2rads():
@@ -77,32 +74,27 @@ void accel2ms2( void ) {
*/
void gyro2rads( void ) {
/** 150 grad/sec 10Bit, 3,3Volt, 1rad = 2Pi/1024 => Pi/512 */
gyro[G_ROLL] = (float)(adc_average[ADC_ROLL]) * IMU_GYRO_P_SENS;
gyro[G_PITCH] = (float)(adc_average[ADC_PITCH]) * IMU_GYRO_Q_SENS;
gyro[G_YAW] = (float)(-adc_average[ADC_YAW]) * IMU_GYRO_R_SENS;
gyro[G_ROLL] = (float)(adc_average[0]) * IMU_GYRO_P_SENS;
gyro[G_PITCH] = (float)(adc_average[1]) * IMU_GYRO_Q_SENS;
gyro[G_YAW] = (float)(adc_average[2]) * IMU_GYRO_R_SENS;
}
void analog_imu_init( void ) {
adc_buf_channel(ADC_CHANNEL_GYRO_P, &buf_adc[0], ADC_NB_SAMPLES);
adc_buf_channel(ADC_CHANNEL_GYRO_Q, &buf_adc[1], ADC_NB_SAMPLES);
adc_buf_channel(ADC_CHANNEL_GYRO_R, &buf_adc[2], ADC_NB_SAMPLES);
adc_buf_channel(ADC_CHANNEL_ACCEL_X, &buf_adc[5], ADC_NB_SAMPLES);
adc_buf_channel(ADC_CHANNEL_ACCEL_Y, &buf_adc[6], ADC_NB_SAMPLES);
adc_buf_channel(ADC_CHANNEL_ACCEL_Z, &buf_adc[7], ADC_NB_SAMPLES);
#if NB_ADC != 8
#error "8 ADCs expected !"
#endif
imu_impl_init();
}
void analog_imu_offset_set( void ) {
uint8_t i;
for(i = 0; i < NB_ADC - 1; i++) {
analog_raw[i] = buf_adc[i].sum / ADC_NB_SAMPLES;
analog_imu_offset[i] = analog_raw[i];
// read IMU
imu_periodic();
for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
analog_imu_offset[i] = analog_imu_values[i];
}
analog_imu_offset[7] = analog_raw[7] + (9.81f / IMU_ACCEL_Z_SENS);
// Z channel should read
analog_imu_offset[5] += (9.81f / IMU_ACCEL_Z_SENS);
}
/**
* analog_imu_update():
@@ -110,15 +102,14 @@ void analog_imu_offset_set( void ) {
void analog_imu_update( void ) {
uint8_t i;
for(i = 0; i < NB_ADC; i++) {
analog_raw[i] = buf_adc[i].sum / ADC_NB_SAMPLES;
// read IMU
imu_periodic();
for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
adc_average[i] -= analog_imu_offset[i];
}
adc_average[ADC_ROLL] = analog_raw[0] - analog_imu_offset[0];
adc_average[ADC_PITCH] = analog_raw[1] - analog_imu_offset[1];
adc_average[ADC_YAW] = analog_raw[2] - analog_imu_offset[2];
adc_average[ADC_ACCX] = analog_raw[5] - analog_imu_offset[5];
adc_average[ADC_ACCY] = analog_raw[6] - analog_imu_offset[6];
adc_average[ADC_ACCZ] = analog_raw[7] - analog_imu_offset[7];
accel2ms2();
gyro2rads();
}
sw/airborne/subsystems/ahrs/dcm/analogimu.h
-12
View File
@@ -39,18 +39,6 @@ extern float imu_roll_neutral;
extern float imu_pitch_neutral;
// ADC0 Slots
#define ADC_YAW 1
#define ADC_PITCH 2
#define ADC_ROLL 3
#define ADC_OFF 4
// ADC1 Slots
#define ADC_ACCX (3+8)
#define ADC_ACCY (4+8)
#define ADC_ACCZ (5+8)
#define ADC_VBAT (6+8)
#define ADC_ALT (7+8)
//functions
void analog_imu_init( void );
sw/airborne/subsystems/imu/imu_analog.c
+6 -3
View File
@@ -21,13 +21,16 @@
* Boston, MA 02111-1307, USA.
*/
#include "subsystems/imu.h"
#include "imu_analog.h"
#include "generated/airframe.h"
#include "mcu_periph/adc.h"
#include "subsystems/imu.h"
#include "mcu_periph/uart.h"
volatile bool_t analog_imu_available;
uint16_t analog_imu_values[NB_ANALOG_IMU_ADC];
static struct adc_buf analog_imu_adc_buf[NB_ADC];
static struct adc_buf analog_imu_adc_buf[NB_ANALOG_IMU_ADC];
void imu_impl_init(void) {
@@ -45,7 +48,7 @@ void imu_impl_init(void) {
void imu_periodic(void) {
uint8_t i;
for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
analog_imu_values[i] = analog_imu_adc_buf[i].sum / analog_imu_adc_buf[i].nb_samples;
analog_imu_values[i] = analog_imu_adc_buf[i].sum / analog_imu_adc_buf[i].av_nb_sample;
}
analog_imu_available = TRUE;