Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 09:58:23 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

PPZIMU calibrated and integrated with AHRS code

Browse Source
This commit is contained in:
Christophe De Wagter
2011-05-05 21:35:45 +02:00
parent c5dcaa4204
commit 737d8b1592
7 changed files with 74 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+37 -89
View File
@@ -7,11 +7,11 @@
<airframe name="Yapa v1">
<servos>
<servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
<servo name="AILERON_LEFT" no="2" min="1000" neutral="1500" max="2000"/>
<servo name="AILERON_RIGHT" no="6" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
<servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
<servo name="THROTTLE" no="4" min="1000" neutral="1000" max="2000"/>
<servo name="AILERON_LEFT" no="5" min="1000" neutral="1500" max="2000"/>
<servo name="AILERON_RIGHT" no="8" min="2000" neutral="1500" max="1000"/>
<servo name="ELEVATOR" no="1" min="2000" neutral="1500" max="1000"/>
<servo name="RUDDER" no="9" min="1100" neutral="1500" max="1900"/>
</servos>
<commands>
@@ -28,80 +28,30 @@
<set command="BRAKE" value="@YAW"/>
</rc_commands>
<section name="SERVO_MIXER_GAINS">
<define name="AILERON_RATE_UP" value="0.50f"/>
<define name="AILERON_RATE_DOWN" value="0.25f"/>
<define name="AILERON_RATE_UP_BRAKE" value="0.5f"/>
<define name="AILERON_RATE_DOWN_BRAKE" value="0.9f"/>
<define name="PITCH_GAIN" value="0.9f"/>
<define name="YAW_THRUST" value="0.0f"/>
<define name="BRAKE_AILEVON" value="-0.68f"/>
<define name="BRAKE_PITCH" value="0.0f"/>
<define name="MAX_BRAKE_RATE" value="150"/>
<define name="RATELIMIT(X,RATE)" value="( _rate_limited_value + Chop( (X) - _rate_limited_value, -(RATE), (RATE) )); \
int16_t _rate_limited_value = 0;"/>
</section>
<command_laws>
<!-- Brake Rate Limiter -->
<let var="brake_value" value="Chop(-@BRAKE, 0, MAX_PPRZ)"/>
<!--<let var="brake_value" value="RATELIMIT( $brake_value , MAX_BRAKE_RATE )"/>
<let var="test; \
static int16_t _var_brake_value = 0; \
_var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
int verwaarloos_deze_warning_CDW" value="0"/>
-->
<!-- Differential Aileron Depending on Brake Value -->
<let var="aileron_up_rate" value="(AILERON_RATE_UP * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_UP_BRAKE * $brake_value)"/>
<let var="aileron_down_rate" value="(AILERON_RATE_DOWN * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_DOWN_BRAKE * $brake_value)"/>
<let var="aileron_up" value="(@ROLL * (((float)$aileron_up_rate) / ((float)MAX_PPRZ)))"/>
<let var="aileron_down" value="(@ROLL * (((float)$aileron_down_rate) / ((float)MAX_PPRZ)))"/>
<let var="leftturn" value="(@ROLL >= 0? 1 : 0)"/>
<let var="rightturn" value="(1 - $leftturn)"/>
<set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON)"/>
<set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON)"/>
<!-- Differential Thurst -->
<set servo="AILERON_LEFT" value="@ROLL"/>
<set servo="AILERON_RIGHT" value="@ROLL"/>
<set servo="THROTTLE" value="@THROTTLE"/>
<!-- Pitch with Brake-Trim Function -->
<set servo="ELEVATOR" value="-@PITCH * PITCH_GAIN + BRAKE_PITCH * $brake_value"/>
<set servo="ELEVATOR" value="@PITCH"/>
</command_laws>
<!-- Local magnetic field -->
<section name="AHRS" prefix="AHRS_" >
<define name="H_X" value="0.51562740288882" />
<define name="H_Y" value="-0.05707735220832" />
<define name="H_Z" value="0.85490967783446" />
</section>
<!-- Local magnetic field -->
<section name="AHRS" prefix="AHRS_" >
<define name="H_X" value="0.51562740288882" />
<define name="H_Y" value="-0.05707735220832" />
<define name="H_Z" value="0.85490967783446" />
</section>
<section name="IMU" prefix="IMU_">
<!-- MAX1168 ADC CHANNELS
<define name="GYRO_P_CHAN" value="0"/>
<define name="GYRO_Q_CHAN" value="1"/>
<define name="GYRO_R_CHAN" value="2"/>
<define name="ACCEL_X_CHAN" value="4"/>
<define name="ACCEL_Y_CHAN" value="3"/>
<define name="ACCEL_Z_CHAN" value="5"/>
-->
<!-- Calibration Neutral -->
<define name="GYRO_P_NEUTRAL" value="0"/>
<define name="GYRO_Q_NEUTRAL" value="0"/>
<define name="GYRO_R_NEUTRAL" value="0"/>
<define name="GYRO_P_NEUTRAL" value="-7"/>
<define name="GYRO_Q_NEUTRAL" value="-77"/>
<define name="GYRO_R_NEUTRAL" value="-23"/>
<!-- SENS = 2mV/deg/sec * 57.6 deg/rad = 114.59 mV/rad/sec * 16 LSB/mV = 1833.465 LSB/rad/sec / 12bit FRAC -->
<define name="GYRO_P_SENS" value="2.234" integer="16"/>
<define name="GYRO_Q_SENS" value="2.234" integer="16"/>
<define name="GYRO_R_SENS" value="2.234" integer="16"/>
<!-- SENS = 14.375 LSB/(deg/sec) * 57.6 deg/rad = 828 LSB/rad/sec / 12bit FRAC: 4096 / 828 -->
<define name="GYRO_P_SENS" value="4.947" integer="16"/>
<define name="GYRO_Q_SENS" value="4.947" integer="16"/>
<define name="GYRO_R_SENS" value="4.947" integer="16"/>
<define name="GYRO_P_Q" value="0."/>
<define name="GYRO_P_R" value="0"/>
@@ -110,21 +60,21 @@
<define name="GYRO_R_P" value="0."/>
<define name="GYRO_R_Q" value="0."/>
<define name="GYRO_P_SIGN" value="1"/>
<define name="GYRO_Q_SIGN" value="-1"/>
<define name="GYRO_R_SIGN" value="-1"/>
<define name="GYRO_P_SIGN" value="-1"/>
<define name="GYRO_Q_SIGN" value="1"/>
<define name="GYRO_R_SIGN" value="1"/>
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_X_NEUTRAL" value="12"/>
<define name="ACCEL_Y_NEUTRAL" value="2"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<!-- SENS = 660mV/g / 9.81 ms2/g = 67.27 mV/ms2 * 16 LSB/mV = 1076 LSB/ms2 / 10bit FRAC -->
<define name="ACCEL_X_SENS" value="0.9372" integer="16"/>
<define name="ACCEL_Y_SENS" value="0.9346" integer="16"/>
<define name="ACCEL_Z_SENS" value="0.9178" integer="16"/>
<!-- SENS = 256 LSB/g @ 2.5V [X&Y: 265 LSB/g @ 3.3V] / 9.81 ms2/g = 26.095 LSB/ms2 / 10bit FRAC: 1024 / 26.095 for z and 1024 / 27.01 for X&Y -->
<define name="ACCEL_X_SENS" value="37.9" integer="16"/>
<define name="ACCEL_Y_SENS" value="37.9" integer="16"/>
<define name="ACCEL_Z_SENS" value="39.24" integer="16"/>
<define name="ACCEL_X_SIGN" value="1"/>
<define name="ACCEL_Y_SIGN" value="1"/>
<define name="ACCEL_Y_SIGN" value="-1"/>
<define name="ACCEL_Z_SIGN" value="1"/>
<define name="MAG_X_NEUTRAL" value="0"/>
@@ -135,8 +85,6 @@
<define name="MAG_Y_SENS" value="1" integer="16"/>
<define name="MAG_Z_SENS" value="1" integer="16"/>
<!-- <define name="MAG_45_HACK" value="1"/> -->
<define name="BODY_TO_IMU_PHI" value="0"/>
<define name="BODY_TO_IMU_THETA" value="0"/>
<define name="BODY_TO_IMU_PSI" value="0"/>
@@ -270,14 +218,12 @@
<define name="WIND_INFO"/>
<define name="WIND_INFO_RET"/>
<!--
<configure name="PERIODIC_FREQUENCY" value="960"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="240"/>
<configure name="PERIODIC_FREQUENCY" value="60"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="60"/>
<configure name="AHRS_CORRECT_FREQUENCY" value="60"/>
<configure name="AHRS_ALIGNER_LED" value="1"/>
-->
<configure name="CPU_LED" value="2"/>
<configure name="AHRS_ALIGNER_LED" value="3"/>
<configure name="CPU_LED" value="3"/>
</target>
<target name="sim" board="pc"/>
@@ -313,6 +259,8 @@
ap.srcs += $(SRC_SUBSYSTEMS)/imu.c
ap.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446
ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\"
# -DOUTPUTMODE=2
</makefile>
</airframe>
conf/modules/ins_ppzuavimu.xml
+4 -1
View File
@@ -5,13 +5,16 @@
<header>
<file name="ins_ppzuavimu.h"/>
</header>
<init fun="ppzuavimu_module_init()"/>
<periodic fun="ppzuavimu_module_periodic()" freq="60"/>
<periodic fun="ppzuavimu_module_downlink_raw()" freq="5"/>
<event fun="ppzuavimu_module_event()"/>
<makefile>
<define name="USE_I2C" />
<file name="ins_ppzuavimu.c"/>
<define name="PPZUAVIMU_I2C_DEVICE" value="i2c0" />
<define name="USE_I2C" />
<define name="USE_I2C0" />
</makefile>
</module>
sw/airborne/firmwares/fixedwing/main_ap.c
+6 -6
View File
@@ -75,7 +75,7 @@
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ahrs/ahrs_float_dcm.h"
static inline void on_gyro_accel_event( void );
static inline void on_gyro_event( void );
static inline void on_accel_event( void );
static inline void on_mag_event( void );
#endif
@@ -398,7 +398,7 @@ static inline void attitude_loop( void ) {
v_ctl_throttle_slew();
ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint;
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#if defined MCU_SPI_LINK
@@ -608,7 +608,7 @@ void event_task_ap( void ) {
#endif
#ifdef USE_AHRS
ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event);
ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif // USE_AHRS
#ifdef USE_GPS
@@ -630,7 +630,7 @@ void event_task_ap( void ) {
}
modules_event_task();
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
if (new_ins_attitude > 0)
{
@@ -639,7 +639,7 @@ void event_task_ap( void ) {
new_ins_attitude = 0;
}
#endif
} /* event_task_ap() */
@@ -656,7 +656,7 @@ static inline void on_gps_solution( void ) {
static inline void on_accel_event( void ) {
}
static inline void on_gyro_accel_event( void ) {
static inline void on_gyro_event( void ) {
#ifdef AHRS_CPU_LED
LED_ON(AHRS_CPU_LED);
sw/airborne/modules/ins/ins_ppzuavimu.c
+21 -5
View File
@@ -106,10 +106,10 @@ void ppzuavimu_module_init( void )
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
while(ppzuavimu_adxl345.status == I2CTransPending);
/* Set range to 4g */
/* Set range to 16g but keeping full resolution of 3.9 mV/g */
ppzuavimu_adxl345.type = I2CTransTx;
ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT;
ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x01; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x03; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g
ppzuavimu_adxl345.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345);
while(ppzuavimu_adxl345.status == I2CTransPending);
@@ -162,10 +162,13 @@ void ppzuavimu_module_periodic( void )
ppzuavimu_hmc5843.len_w = 1;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843);
}
RunOnceEvery(6,DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z));
RunOnceEvery(6,DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z));
RunOnceEvery(6,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
void ppzuavimu_module_downlink_raw( void )
{
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z);
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z);
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z);
}
void ppzuavimu_module_event( void )
@@ -176,7 +179,15 @@ void ppzuavimu_module_event( void )
gyr_x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]);
gyr_y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]);
gyr_z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]);
#ifdef ASPIRIN_IMU
RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z);
#else
RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z);
#endif
gyr_valid = TRUE;
ppzuavimu_itg3200.status = I2CTransDone;
}
// If the adxl345 I2C transaction has succeeded: convert the data
@@ -185,7 +196,11 @@ void ppzuavimu_module_event( void )
acc_x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]);
acc_y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]);
acc_z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]);
VECT3_ASSIGN(imu.accel_unscaled, acc_x, acc_y, acc_z);
acc_valid = TRUE;
ppzuavimu_adxl345.status = I2CTransDone;
}
// If the hmc5843 I2C transaction has succeeded: convert the data
@@ -195,5 +210,6 @@ void ppzuavimu_module_event( void )
mag_y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]);
mag_z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]);
mag_valid = TRUE;
ppzuavimu_hmc5843.status = I2CTransDone;
}
}
sw/airborne/modules/ins/ins_ppzuavimu.h
+2 -9
View File
@@ -24,10 +24,6 @@
#include "std.h"
#include "subsystems/imu.h"
extern int32_t mag_x, mag_y, mag_z;
extern int32_t gyr_x, gyr_y, gyr_z;
extern int32_t acc_x, acc_y, acc_z;
extern volatile bool_t gyr_valid;
extern volatile bool_t acc_valid;
extern volatile bool_t mag_valid;
@@ -36,11 +32,9 @@ extern volatile bool_t mag_valid;
extern void ppzuavimu_module_init( void );
extern void ppzuavimu_module_periodic( void );
extern void ppzuavimu_module_event( void );
extern void ppzuavimu_module_downlink_raw( void );
extern volatile bool_t analog_imu_available;
extern int imu_overrun;
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
if (gyr_valid) { \
gyr_valid = FALSE; \
@@ -54,8 +48,7 @@ extern int imu_overrun;
mag_valid = FALSE; \
_mag_handler(); \
} \
}
}
#endif // PPZUAVIMU_H
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+2 -2
View File
@@ -113,7 +113,7 @@ void ahrs_update_fw_estimator( void )
estimator_psi = ahrs_float.ltp_to_imu_euler.psi;
estimator_p = Omega_Vector[0];
/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel,
&(DCM_Matrix[0][0]),
&(DCM_Matrix[0][1]),
@@ -128,7 +128,7 @@ void ahrs_update_fw_estimator( void )
&(DCM_Matrix[2][2])
));
*/
}
sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+2
View File
@@ -53,7 +53,9 @@ void ahrs_update_fw_estimator(void);
#define GRAVITY 9.81
#ifndef OUTPUTMODE
#define OUTPUTMODE 1
#endif
// Mode 0 = DCM integration without Ki gyro bias
// Mode 1 = DCM integration with Kp and Ki
// Mode 2 = direct accelerometer -> euler