|
From: | Prof. Dr.-Ing. Heinrich Warmers |
Subject: | Re: [Paparazzi-devel] Temperature compensation of rate sensors solved |
Date: | Thu, 14 Mar 2013 23:29:46 +0100 |
User-agent: | Mozilla/5.0 (Windows; U; Windows NT 5.0; de-DE; rv:1.4) Gecko/20030619 Netscape/7.1 (ax) |
Hi, i have implemented and tested the temperature compensation. It works perfect. I think it was the temperature drift of the ADX355 (accelerator sensor) Since my current Version is 4.2 it will need some time to make a pull request. I use the MBMINI Autopilot. This can also very easy integrated in the MPU60X0 drivers. Therfore the changes i made: aircraft.xml <define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/> --> <target name="ap" board="hbmini_2.0"> <define name="CS_74138" value="1"/> <define name="IMU_HBMINI_I_VERSION_1_0" value="1"/> <define name="BMP_I2C_DEV" value="i2c1"/> <define name="SENSOR_SYNC_SEND" value="1"/> <define name="FAILSAFE_GROUND_DETECT"/> <define name="USE_GPS_ACC4R"/> <define name="CRITIC_BAT_LEVEL" value="9.3"/> <define name="LOW_BAT_LEVEL" value="9.7"/> <define name="BAT_CHECKER_LED" value="3"/> <define name="INVERT_BAT_LED" value="1"/> <define name="USE_LED_6" value="1"/> <define name="BAT_CHECKER_DELAY" value="2"/> <define name="HBminiBMP" value="1"/> new <define name="DXA" value="-0.0785"/> <define name="DYA" value="-0.0338"/> <define name="DZA" value="-0.171"/> <define name="DXG" value="0.1301"/> <define name="DYG" value="-0.1592"/> <define name="DZG" value="-0.0"/> <define name="TREF" value="19420"/> ********************************************************************************************* /sw/airborne/subsystems/imu.h line 56 ****************************************************************************** /** abstract IMU interface providing floating point interface */ struct ImuFloat { struct FloatRates gyro; float temp; struct FloatVect3 accel; **************************************** line 103 RATES_COPY(_imu.gyro_prev, _imu.gyro); \ _imu.gyro.p = ((_imu.gyro_unscaled.p +(TREF-imu.temp)*DXG - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \ _imu.gyro.q = ((_imu.gyro_unscaled.q +(TREF-imu.temp)*DYG- _imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \ _imu.gyro.r = ((_imu.gyro_unscaled.r +(TREF-imu.temp)*DZG - _imu.gyro_neutral.r)*IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM)/IMU_GYRO_R_SENS_DEN; \ } #endif #ifndef ImuScaleAccel #define ImuScaleAccel(_imu) { \ VECT3_COPY(_imu.accel_prev, _imu.accel); \ _imu.accel.x = ((_imu.accel_unscaled.x +(TREF-imu.temp)*DXA - _imu.accel_neutral.x)*IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN; \ _imu.accel.y = ((_imu.accel_unscaled.y +(TREF-imu.temp)*DYA - _imu.accel_neutral.y)*IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM)/IMU_ACCEL_Y_SENS_DEN; \ _imu.accel.z = ((_imu.accel_unscaled.z +(TREF-imu.temp)*DZA - _imu.accel_neutral.z)*IMU_ACCEL_Z_SIGN*IMU_ACCEL_Z_SENS_NUM)/IMU_ACCEL_Z_SENS_DEN; \ } **************************************************************************************+ /sw/airborne/subsystems/imu/imu_b2.h line 39 ************************************************************************************* #ifdef IMU_HBMINI_I_VERSION_1_0 #if !defined IMU_GYRO_P_CHAN & !defined IMU_GYRO_Q_CHAN & !defined IMU_GYRO_R_CHAN #define IMU_GYRO_P_CHAN 3 #define IMU_GYRO_Q_CHAN 4 #define IMU_GYRO_R_CHAN 5 #define IMU_GYRO_TEMP_CHAN 6 #endif line 237 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ if (max1168_status == STA_MAX1168_DATA_AVAILABLE) { \ imu.gyro_unscaled.p = max1168_values[IMU_GYRO_P_CHAN]; \ imu.gyro_unscaled.q = max1168_values[IMU_GYRO_Q_CHAN]; \ imu.gyro_unscaled.r = max1168_values[IMU_GYRO_R_CHAN]; \ imu.temp = max1168_values[IMU_GYRO_TEMP_CHAN]; \ imu.accel_unscaled.x = max1168_values[IMU_ACCEL_X_CHAN]; \ imu.accel_unscaled.y = max1168_values[IMU_ACCEL_Y_CHAN]; \ imu.accel_unscaled.z = max1168_values[IMU_ACCEL_Z_CHAN]; \ max1168_status = STA_MAX1168_IDLE; \ _gyro_handler(); \ _accel_handler(); \ } \ ImuMagEvent(_mag_handler); \ } ********************************************************************************************************* /sw/airborne/fimwares/rotorcraft/telemetry.h line 157 to see the temperature value /* &imu.gyro_unscaled.r); \ */ #define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) { \ DOWNLINK_SEND_IMU_GYRO_RAW(_trans, _dev, \ &imu.gyro_unscaled.p, \ &imu.gyro_unscaled.q, \ &imu.temp); \ } *********************************************************************************************************** Felix Ruess schrieb: Hi, |
[Prev in Thread] | Current Thread | [Next in Thread] |