diff --git a/conf/airframes/example_twog_analogimu.xml b/conf/airframes/example_twog_analogimu.xml index d6d68eee2f..391bd21495 100644 --- a/conf/airframes/example_twog_analogimu.xml +++ b/conf/airframes/example_twog_analogimu.xml @@ -37,29 +37,29 @@
- - - + + + - + - + - - - + + +
diff --git a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile index b89f694595..27d387b3ad 100644 --- a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile +++ b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile @@ -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 diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu.c b/sw/airborne/subsystems/ahrs/dcm/analogimu.c index 9a8f151b22..623a074a89 100644 --- a/sw/airborne/subsystems/ahrs/dcm/analogimu.c +++ b/sw/airborne/subsystems/ahrs/dcm/analogimu.c @@ -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(); } diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu.h b/sw/airborne/subsystems/ahrs/dcm/analogimu.h index 828736c2aa..3fec995c3f 100644 --- a/sw/airborne/subsystems/ahrs/dcm/analogimu.h +++ b/sw/airborne/subsystems/ahrs/dcm/analogimu.h @@ -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 ); diff --git a/sw/airborne/subsystems/imu/imu_analog.c b/sw/airborne/subsystems/imu/imu_analog.c index 1d036942eb..0e9b0230e8 100644 --- a/sw/airborne/subsystems/imu/imu_analog.c +++ b/sw/airborne/subsystems/imu/imu_analog.c @@ -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;