Hi Alex,
first have a look to the dokowki
:http://paparazzi.enac.fr/wiki/STM32F4_Discovery
For the connections look to the photos in the appendix of Chris mail 08
25 2013.
I use the master version of november 2013.
You have to exchange this file: imu_drotek_10dof_v2.c in .../subsysems/imu
I made changes to work with 512 Hz.
/*
* Copyright (C) 2013 Felix Ruess <address@hidden>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/imu/imu_drotek_10dof_v2.c
*
* Driver for the Drotek 10DOF V2 IMU.
* MPU6050 + HMC5883 + MS5611
* Reading the baro is not part of the IMU driver.
*
* By default the axes orientation should be as printed on the pcb,
* meaning z-axis pointing down if ICs are facing down.
* The orientation can be switched so that the IMU can be mounted ICs
facing up
* by defining IMU_DROTEK_2_ORIENTATION_IC_UP.
*/
#include "subsystems/imu.h"
#include "mcu_periph/i2c.h"
#if !defined DROTEK_2_LOWPASS_FILTER && !defined DROTEK_2_SMPLRT_DIV
#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
* Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
*/
#define DROTEK_2_LOWPASS_FILTER MPU60X0_DLPF_42HZ
#define DROTEK_2_SMPLRT_DIV 9
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#elif PERIODIC_FREQUENCY == 512
/* Accelerometer: Bandwidth 260Hz, Delay 0ms
* Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
*/
#define DROTEK_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define DROTEK_2_SMPLRT_DIV 3
PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
#endif
#endif
PRINT_CONFIG_VAR(DROTEK_2_SMPLRT_DIV)
PRINT_CONFIG_VAR(DROTEK_2_LOWPASS_FILTER)
#ifndef DROTEK_2_GYRO_RANGE
#define DROTEK_2_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
PRINT_CONFIG_VAR(DROTEK_2_GYRO_RANGE)
#ifndef DROTEK_2_ACCEL_RANGE
#define DROTEK_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
PRINT_CONFIG_VAR(DROTEK_2_ACCEL_RANGE)
#ifndef DROTEK_2_MPU_I2C_ADDR
#define DROTEK_2_MPU_I2C_ADDR MPU60X0_ADDR_ALT
#endif
PRINT_CONFIG_VAR(DROTEK_2_MPU_I2C_ADDR)
#ifndef DROTEK_2_HMC_I2C_ADDR
#define DROTEK_2_HMC_I2C_ADDR HMC58XX_ADDR
#endif
PRINT_CONFIG_VAR(DROTEK_2_HMC_I2C_ADDR)
struct ImuDrotek2 imu_drotek2;
void imu_impl_init(void)
{
/* MPU-60X0 */
mpu60x0_i2c_init(&imu_drotek2.mpu, &(DROTEK_2_I2C_DEV),
DROTEK_2_MPU_I2C_ADDR);
// change the default configuration
imu_drotek2.mpu.config.smplrt_div = DROTEK_2_SMPLRT_DIV;
imu_drotek2.mpu.config.dlpf_cfg = DROTEK_2_LOWPASS_FILTER;
imu_drotek2.mpu.config.gyro_range = DROTEK_2_GYRO_RANGE;
imu_drotek2.mpu.config.accel_range = DROTEK_2_ACCEL_RANGE;
/* HMC5883 magnetometer */
hmc58xx_init(&imu_drotek2.hmc, &(DROTEK_2_I2C_DEV), DROTEK_2_HMC_I2C_ADDR);
/* mag is declared as slave to call the configure function,
* regardless if it is an actual MPU slave or passthrough
*/
imu_drotek2.mpu.config.nb_slaves = 1;
/* set callback function to configure mag */
imu_drotek2.mpu.config.slaves[0].configure =
&imu_drotek2_configure_mag_slave;
// use hmc mag via passthrough
imu_drotek2.mpu.config.i2c_bypass = TRUE;
imu_drotek2.gyro_valid = FALSE;
imu_drotek2.accel_valid = FALSE;
imu_drotek2.mag_valid = FALSE;
}
void imu_periodic(void)
{
// Start reading the latest gyroscope data
mpu60x0_i2c_periodic(&imu_drotek2.mpu);
// Read HMC58XX at ~50Hz (main loop for rotorcraft: 512Hz)
if (imu_drotek2.mpu.config.initialized) {
RunOnceEvery(10, hmc58xx_read(&imu_drotek2.hmc));
}
}
void imu_drotek2_event(void)
{
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_drotek2.mpu);
if (imu_drotek2.mpu.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
/* change orientation, so if ICs face up, z-axis is down */
imu.gyro_unscaled.p = imu_drotek2.mpu.data_rates.rates.p;
imu.gyro_unscaled.q = -imu_drotek2.mpu.data_rates.rates.q;
imu.gyro_unscaled.r = -imu_drotek2.mpu.data_rates.rates.r;
imu.accel_unscaled.x = imu_drotek2.mpu.data_accel.vect.x;
imu.accel_unscaled.y = -imu_drotek2.mpu.data_accel.vect.y;
imu.accel_unscaled.z = -imu_drotek2.mpu.data_accel.vect.z;
#else
/* default orientation as should be printed on the pcb, z-down, ICs
down */
RATES_COPY(imu.gyro_unscaled, imu_drotek2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_drotek2.mpu.data_accel.vect);
#endif
imu_drotek2.mpu.data_available = FALSE;
imu_drotek2.gyro_valid = TRUE;
imu_drotek2.accel_valid = TRUE;
}
/* HMC58XX event task */
hmc58xx_event(&imu_drotek2.hmc);
if (imu_drotek2.hmc.data_available) {
#if IMU_DROTEK_2_ORIENTATION_IC_UP
imu.mag_unscaled.x = imu_drotek2.hmc.data.vect.x;
imu.mag_unscaled.y = -imu_drotek2.hmc.data.vect.y;
imu.mag_unscaled.z = -imu_drotek2.hmc.data.vect.z;
#else
VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect);
#endif
imu_drotek2.hmc.data_available = FALSE;
imu_drotek2.mag_valid = TRUE;
}
}
/** callback function to configure hmc5883 mag
* @return TRUE if mag configuration finished
*/
bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set
__attribute__ ((unused)), void* mpu __attribute__ ((unused)))
{
hmc58xx_start_configure(&imu_drotek2.hmc);
if (imu_drotek2.hmc.initialized)
return TRUE;
else
return FALSE;
}
****************************************************************
the airframe file is:
<!DOCTYPE airframe SYSTEM "/airframe.dtd">
<!-- Discovery F4 for multi rotor, with DROTEK 10Dof IMU -->
<airframe name="HB_DiscoveryQuad">
<firmware name="rotorcraft">
<target name="ap" board="stm32f4_discovery">
<subsystem name="radio_control" type="spektrum">
<define name="RADIO_MODE" value="RADIO_AUX1"/>
</subsystem>
<define name="USE_I2C1"/>
</target>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="pwm">
<!--define name="SERVO_HZ" value="400"/-->
</subsystem>
<subsystem name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B57600"/>
<configure name="MODEM_PORT" value="UART2"/>
</subsystem>
<subsystem name="imu" type="drotek_10dof_v2">
<!--
<configure name="PERIODIC_FREQUENCY" value="512"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
<configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
-->
<!-- <subsystem name="imu" type="aspirin_v2.1">
<configure name="ASPIRIN_2_SPI_DEV" value="spi1"/>
<configure name="ASPIRIN_2_SPI_SLAVE_IDX" value="SPI_SLAVE0"/> -->
</subsystem>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="30"/>
</subsystem>
<subsystem name="ins"/>
</firmware>
<firmware name="test_progs">
<target name="test_lis302dl" board="stm32f4_discovery">
<define name="STM32F4_DISCOVERY_SPI1_FOR_LIS302"/>
</target>
</firmware>
<!--#######################################################################################-->
<!--################################ MODULES
#################################-->
<!--#######################################################################################-->
<modules main_freq="512">
<!-- baro_ms5611_i2c Alternate address -->
<load name="baro_ms5611_i2c.xml">
<define name="MS5611_I2C_DEV" value="i2c2" />
<define name="MS5611_SLAVE_ADDR=0xEE"/>
<define name="MS5611_SEND_BARO_ALTITUDE"/>
<!-- olri
<define name="SENSOR_SYNC_SEND"/>
olri -->
</load>
<load name="gps_ubx_ucenter.xml"/>
</modules>
<!--
<modules main_freq="512">
<!-- baro_ms5611_i2c Alternate address -->
<!--<load name="baro_ms5611_i2c.xml">
<define name="MS5611_I2C_DEV" value="i2c2" />
<define name="MS5611_SLAVE_ADDR=0xEE"/>
<define name="MS5611_SEND_BARO_ALTITUDE"/> -->
<!-- olri
<define name="SENSOR_SYNC_SEND"/>
olri -->
<!-- </load>
<load name="gps_ubx_ucenter.xml"/>
<load name="baro_ets.xml">
<define name="BARO_ETS_SCALE" value="40.0"/>
<define name="BARO_ETS_ALT_SCALE" value="0.3"/>
<define name="BARO_ETS_SYNC_SEND"/>
<define name="BARO_ETS_I2C_DEV" value="i2c1"/>
</load>
</modules>
-->
<!--#######################################################################################-->
<!--############################# SERVOS AND CONTROLS
#################################-->
<!--#######################################################################################-->
<!-- commands section -->
<servos driver="Pwm">
<servo name="FRONT" no="0" min="1000" neutral="1100" max="1900"/>
<servo name="BACK" no="1" min="1000" neutral="1100" max="1900"/>
<servo name="RIGHT" no="2" min="1000" neutral="1100" max="1900"/>
<servo name="LEFT" no="3" min="1000" neutral="1100" max="1900"/>
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="256"/>
<!-- front/back turning CW, right/left CCW -->
<define name="ROLL_COEF" value="{ 0, 0, -256, 256 }"/>
<define name="PITCH_COEF" value="{ 256, -256, 0, 0 }"/>
<define name="YAW_COEF" value="{ -256, -256, 256, 256 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="FRONT" value="motor_mixing.commands[SERVO_FRONT]"/>
<set servo="BACK" value="motor_mixing.commands[SERVO_BACK]"/>
<set servo="RIGHT" value="motor_mixing.commands[SERVO_RIGHT]"/>
<set servo="LEFT" value="motor_mixing.commands[SERVO_LEFT]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_X_NEUTRAL" value="-240"/>
<define name="ACCEL_Y_NEUTRAL" value="-700"/>
<define name="ACCEL_Z_NEUTRAL" value="-40"/>
<define name="GYRO_P_NEUTRAL" value="5" />
<define name="GYRO_Q_NEUTRAL" value="25" />
<define name="GYRO_R_NEUTRAL" value="12" />
<!-- SENS = 2048 LSB/g / 9.81 ms2/g = 208.766564729 LSB/ms2 / 10bit
FRAC: 1024 / 208.7665 -->
<!-- Found with calibrate.py script -->
<define name="ACCEL_X_NEUTRAL" value="8"/>
<define name="ACCEL_Y_NEUTRAL" value="25"/>
<define name="ACCEL_Z_NEUTRAL" value="-55"/>
<define name="ACCEL_X_SENS" value="2.46169818688" integer="16"/>
<define name="ACCEL_Y_SENS" value="2.4400141151" integer="16"/>
<define name="ACCEL_Z_SENS" value="2.42446843209" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="9.7" unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="-2.4" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
<!-- SENS=16.4 LSB/(deg/sec)*57.295779513 deg/rad=939.65078
LSB/rad/sec / 12bit FRAC: 4096/939.65 -->
<!--<define name="GYRO_P_SENS" value="4.359" integer="16"/>
<define name="GYRO_Q_SENS" value="4.359" integer="16"/>
<define name="GYRO_R_SENS" value="4.359" integer="16"/>-->
<define name="AGYRO_P_SENS" value="2.17953" integer="16"/>
<define name="AGYRO_Q_SENS" value="2.17953" integer="16"/>
<define name="AGYRO_R_SENS" value="2.17953" integer="16"/>
<!-- DCM decouble Motions
<define name="GYRO_P_Q" value="0."/>
<define name="GYRO_P_R" value="0."/>
<define name="GYRO_Q_P" value="0."/>
<define name="GYRO_Q_R" value="0."/>
<define name="GYRO_R_P" value="0."/>
<define name="GYRO_R_Q" value="0."/> -->
<!-- replace this with your own calibration -->
<define name="MAG_X_NEUTRAL" value="-179"/>
<define name="MAG_Y_NEUTRAL" value="-21"/>
<define name="MAG_Z_NEUTRAL" value="79"/>
<define name="MAG_X_SENS" value="4.17334785618" integer="16"/>
<define name="MAG_Y_SENS" value="3.98885954135" integer="16"/>
<define name="MAG_Z_SENS" value="4.40442339014" integer="16"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
</section>
<section name="INS" prefix="INS_">
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" value="45." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="400." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="1000"/>
<define name="PHI_DGAIN" value="400"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="1000"/>
<define name="THETA_DGAIN" value="400"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="500"/>
<define name="PSI_DGAIN" value="300"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="300"/>
<define name="THETA_DDGAIN" value="300"/>
<define name="PSI_DDGAIN" value="300"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="150"/>
<define name="HOVER_KD" value="80"/>
<define name="HOVER_KI" value="20"/>
<!--define name="NOMINAL_HOVER_THROTTLE" value="0.5"/-->
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="100"/>
<define name="IGAIN" value="20"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
have fun
Heinrich
Alexandr Arhutich schrieb:
Hi Heinrich,
What airframe, flight plan, settings, radio & telemetry it's
necessary to take to build project for multirotor by using
stm32f4discovery + (mpu6050+hmc5883+ms5611)...
All that stuff in master branch? If not could you share
necessary stuff pls...
thx-
Alex
_______________________________________________
Paparazzi-devel mailing list
address@hidden
https://lists.nongnu.org/mailman/listinfo/paparazzi-devel
|