Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

[boards] add driver for KroozSD BigRotorcraftEdition

Browse Source 
- IMU with memsic MXR9500 accelerometer
- set mag correct freq to 75Hz for krooz_sd_x

closes #586
This commit is contained in:
softsr
2013-11-11 14:55:28 -08:00
committed by Felix Ruess
parent 3804823110
commit 8a782ca3e2
10 changed files with 771 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/krooz_sd/krooz_sd_bre_hexa_mkk.xml
+241
View File
@@ -0,0 +1,241 @@
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
<airframe name="Hexarotor KroozSD Big Rotorcraft Edition Mkk">
<firmware name="rotorcraft">
<target name="ap" board="krooz_sd"/>
<subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="mkk">
<define name="I2C_TRANSACTION_QUEUE_LEN" value="10"/>
<define name="I2C1_CLOCK_SPEED" value="42000"/>
</subsystem>
<subsystem name="stabilization" type="int_euler"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="imu" type="krooz_sd_memsic"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="hff"/>
<define name="NO_RC_THRUST_LIMIT"/>
<define name="FAILSAFE_GROUND_DETECT" value="1"/>
<define name="THRESHOLD_GROUND_DETECT" value="12."/>
<define name="USE_GPS_ACC4R" value="1"/>
<define name="HFF_R_POS_MIN" value="2.5"/>
<define name="HFF_R_SPEED_MIN" value="2."/>
<define name="VF_FLOAT_MEAS_NOISE" value="2."/>
<define name="USE_ADC_2" value="1"/>
</firmware>
<modules main_freq="512">
<load name="gps_ubx_ucenter.xml"/>
<load name="geo_mag.xml"/>
<load name="osd_max7456.xml"/>
</modules>
<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="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
<define name="NB" value="6"/>
<define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 , 0x5A , 0x5C }"/>
</section>
<servos driver="Mkk">
<servo name="FF" no="0" min="0" neutral="3" max="210"/>
<servo name="FR" no="1" min="0" neutral="3" max="210"/>
<servo name="DR" no="2" min="0" neutral="3" max="210"/>
<servo name="DD" no="3" min="0" neutral="3" max="210"/>
<servo name="DL" no="4" min="0" neutral="3" max="210"/>
<servo name="FL" no="5" min="0" neutral="3" max="210"/>
</servos>
<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="6"/>
<define name="SCALE" value="256"/>
<define name="ROLL_COEF" value="{ 0, -256, -256, 0, 256, 256 }"/>
<define name="PITCH_COEF" value="{ 256, 128, -128, -256, -128, 128 }"/>
<define name="YAW_COEF" value="{ -256, 249, -249, 256, -249, 249 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256, 256, 256 }"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="FF" value="motor_mixing.commands[SERVO_FF]"/>
<set servo="FR" value="motor_mixing.commands[SERVO_FR]"/>
<set servo="DR" value="motor_mixing.commands[SERVO_DR]"/>
<set servo="DD" value="motor_mixing.commands[SERVO_DD]"/>
<set servo="DL" value="motor_mixing.commands[SERVO_DL]"/>
<set servo="FL" value="motor_mixing.commands[SERVO_FL]"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<!-- replace this with your own calibration -->
<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_SENS" value="0.5454" integer="16"/>
<define name="GYRO_Q_SENS" value="0.5454" integer="16"/>
<define name="GYRO_R_SENS" value="0.5454" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="-48"/>
<define name="ACCEL_Y_NEUTRAL" value="86"/>
<define name="ACCEL_Z_NEUTRAL" value="-479"/>
<define name="ACCEL_X_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Y_SENS" value="0.6131" integer="16"/>
<define name="ACCEL_Z_SENS" value="0.6131" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-282"/>
<define name="MAG_Y_NEUTRAL" value="-78"/>
<define name="MAG_Z_NEUTRAL" value="109"/>
<define name="MAG_X_SENS" value="3.33430456557" integer="16"/>
<define name="MAG_Y_SENS" value="3.53180739126" integer="16"/>
<define name="MAG_Z_SENS" value="3.68297143457" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="GRAVITY_HEURISTIC_FACTOR" value="30"/>
<define name="H_X" value="0.3586845"/>
<define name="H_Y" value="0.0168651"/>
<define name="H_Z" value="0.933303"/>
</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="50." 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="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="180." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="180." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_R" value="400" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="70." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(360.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="1500"/>
<define name="PHI_DGAIN" value="600"/>
<define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="1500"/>
<define name="THETA_DGAIN" value="600"/>
<define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="4000"/>
<define name="PSI_DGAIN" value="1500"/>
<define name="PSI_IGAIN" value="300"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="300"/>
<define name="THETA_DDGAIN" value="300"/>
<define name="PSI_DDGAIN" value="2000"/>
</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="REF_MIN_ZDD" value="-0.2*9.81"/> <!-- max descent acceleration in m/s2 -->
<define name="REF_MAX_ZDD" value="0.2*9.81"/> <!-- max climb acceleration in m/s2 -->
<define name="REF_MIN_ZD" value="-3."/> <!-- max descent speed in m/s -->
<define name="REF_MAX_ZD" value="3."/> <!-- max climb speed in m/s -->
<define name="ADAPT_NOISE_FACTOR" value="0.7"/>
<define name="HOVER_KP" value="130"/>
<define name="HOVER_KD" value="150"/>
<define name="HOVER_KI" value="10"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="USE_REF" value="1"/>
<define name="USE_SPEED_REF" value="1"/> <!-- using RC to control horizontal setpoint -->
<define name="MAX_BANK" value="27" unit="deg"/>
<define name="REF_MAX_ACCEL" value="7."/> <!-- max reference horizontal acceleration in m/s2 -->
<define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s -->
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="120"/>
<define name="IGAIN" value="10"/>
<define name="AGAIN" value="10"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
</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="CRITIC_BAT_LEVEL" value="14.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="14.3" unit="V"/>
<define name="MAX_BAT_LEVEL" value="16.8" unit="V"/>
<!--uncomment next two lines together with a corresponding USE_ADC_2 define on the top of this file to use a current sensor
(more about is here http://paparazzi.enac.fr/wiki/Sensors/Current) -->
<!--
<define name="ADC_CHANNEL_CURRENT" value="ADC_2"/>
<define name="MilliAmpereOfAdc(adc)" value="Max(0,(3100 - adc)*20)"/>
-->
</section>
</airframe>
conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
+39 -40
View File
@@ -5,11 +5,6 @@
<firmware name="rotorcraft"> <firmware name="rotorcraft">
<target name="ap" board="krooz_sd"/> <target name="ap" board="krooz_sd"/>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>
<subsystem name="radio_control" type="ppm"/> <subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/> <subsystem name="telemetry" type="transparent"/>
<subsystem name="motor_mixing"/> <subsystem name="motor_mixing"/>
@@ -24,15 +19,19 @@
<subsystem name="ins" type="hff"/> <subsystem name="ins" type="hff"/>
<define name="NO_RC_THRUST_LIMIT"/> <define name="NO_RC_THRUST_LIMIT"/>
<define name="USE_RC_FP_BLOCK_SWITCHING"/> <define name="FAILSAFE_GROUND_DETECT" value="1"/>
<define name="USE_ATTITUDE_REF" value="0"/> <define name="THRESHOLD_GROUND_DETECT" value="12."/>
<define name="USE_GPS_ACC4R" value="1"/>
<define name="HFF_R_POS_MIN" value="2.5"/>
<define name="HFF_R_SPEED_MIN" value="2."/>
<define name="VF_FLOAT_MEAS_NOISE" value="2."/>
<define name="USE_ADC_2" value="1"/>
</firmware> </firmware>
<modules main_freq="512"> <modules main_freq="512">
<!--<load name="gps_ubx_ucenter.xml"/> <load name="gps_ubx_ucenter.xml"/>
<load name="sys_mon.xml"/>
<load name="geo_mag.xml"/> <load name="geo_mag.xml"/>
<load name="osd_max7456.xml"/>--> <load name="osd_max7456.xml"/>
</modules> </modules>
<commands> <commands>
@@ -60,9 +59,6 @@
<define name="TRIM_ROLL" value="0"/> <define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/> <define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/> <define name="TRIM_YAW" value="0"/>
<define name="TRIM_A" value="0"/>
<define name="TRIM_E" value="0"/>
<define name="TRIM_R" value="0"/>
<define name="NB_MOTOR" value="6"/> <define name="NB_MOTOR" value="6"/>
<define name="SCALE" value="256"/> <define name="SCALE" value="256"/>
<define name="ROLL_COEF" value="{ 0, -256, -256, 0, 256, 256 }"/> <define name="ROLL_COEF" value="{ 0, -256, -256, 0, 256, 256 }"/>
@@ -111,7 +107,6 @@
</section> </section>
<section name="AHRS" prefix="AHRS_"> <section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_LOW_PASS_RATES" value="1"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="30"/> <define name="GRAVITY_HEURISTIC_FACTOR" value="30"/>
<define name="H_X" value="0.3586845"/> <define name="H_X" value="0.3586845"/>
<define name="H_Y" value="0.0168651"/> <define name="H_Y" value="0.0168651"/>
@@ -151,35 +146,35 @@
<!-- setpoints --> <!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg"/> <define name="SP_MAX_PHI" value="45." unit="deg"/>
<define name="SP_MAX_THETA" 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="SP_MAX_R" value="50." unit="deg/s"/>
<define name="DEADBAND_A" value="0"/> <define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/> <define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/> <define name="DEADBAND_R" value="250"/>
<!-- reference --> <!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/> <define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/> <define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="100." unit="deg/s"/> <define name="REF_MAX_P" value="180." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(2000.)"/> <define name="REF_MAX_PDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/> <define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/> <define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="100." unit="deg/s"/> <define name="REF_MAX_Q" value="180." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(2000.)"/> <define name="REF_MAX_QDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/> <define name="REF_OMEGA_R" value="400" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/> <define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/> <define name="REF_MAX_R" value="70." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(180.)"/> <define name="REF_MAX_RDOT" value="RadOfDeg(360.)"/>
<!-- feedback --> <!-- feedback -->
<define name="PHI_PGAIN" value="850"/> <define name="PHI_PGAIN" value="1500"/>
<define name="PHI_DGAIN" value="400"/> <define name="PHI_DGAIN" value="600"/>
<define name="PHI_IGAIN" value="100"/> <define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="850"/> <define name="THETA_PGAIN" value="1500"/>
<define name="THETA_DGAIN" value="400"/> <define name="THETA_DGAIN" value="600"/>
<define name="THETA_IGAIN" value="100"/> <define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="4000"/> <define name="PSI_PGAIN" value="4000"/>
<define name="PSI_DGAIN" value="1500"/> <define name="PSI_DGAIN" value="1500"/>
@@ -197,15 +192,14 @@
<define name="MIN_ERR_ZD" value="SPEED_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_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/> <define name="MAX_SUM_ERR" value="2000000"/>
<define name="TILT_COEFF" value="0.35"/> <define name="REF_MIN_ZDD" value="-0.2*9.81"/> <!-- max descent acceleration in m/s2 -->
<define name="REF_MIN_ZDD" value="-0.2*9.81"/> <!-- max descent acceleration --> <define name="REF_MAX_ZDD" value="0.2*9.81"/> <!-- max climb acceleration in m/s2 -->
<define name="REF_MAX_ZDD" value="0.2*9.81"/> <!-- max climb acceleration --> <define name="REF_MIN_ZD" value="-3."/> <!-- max descent speed in m/s -->
<define name="REF_MIN_ZD" value="-3."/> <!-- max descent speed --> <define name="REF_MAX_ZD" value="3."/> <!-- max climb speed in m/s -->
<define name="REF_MAX_ZD" value="3."/> <!-- max climb speed --> <define name="ADAPT_NOISE_FACTOR" value="0.7"/>
<define name="HOVER_KP" value="120"/> <define name="HOVER_KP" value="130"/>
<define name="HOVER_KD" value="80"/> <define name="HOVER_KD" value="150"/>
<define name="HOVER_KI" value="20"/> <define name="HOVER_KI" value="10"/>
<define name="RC_DESCENT_COEF" value ="200"/>
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
@@ -214,7 +208,6 @@
<define name="MAX_BANK" value="27" unit="deg"/> <define name="MAX_BANK" value="27" unit="deg"/>
<define name="REF_MAX_ACCEL" value="7."/> <!-- max reference horizontal acceleration in m/s2 --> <define name="REF_MAX_ACCEL" value="7."/> <!-- max reference horizontal acceleration in m/s2 -->
<define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s --> <define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s -->
<define name="RC_SPEED_DEAD_BAND" value="200000"/>
<define name="PGAIN" value="100"/> <define name="PGAIN" value="100"/>
<define name="DGAIN" value="120"/> <define name="DGAIN" value="120"/>
<define name="IGAIN" value="10"/> <define name="IGAIN" value="10"/>
@@ -237,6 +230,12 @@
<define name="CRITIC_BAT_LEVEL" value="14.0" unit="V"/> <define name="CRITIC_BAT_LEVEL" value="14.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="14.3" unit="V"/> <define name="LOW_BAT_LEVEL" value="14.3" unit="V"/>
<define name="MAX_BAT_LEVEL" value="16.8" unit="V"/> <define name="MAX_BAT_LEVEL" value="16.8" unit="V"/>
<!--uncomment next two lines together with a corresponding USE_ADC_2 define on the top of this file to use a current sensor
(more about is here http://paparazzi.enac.fr/wiki/Sensors/Current) -->
<!--
<define name="ADC_CHANNEL_CURRENT" value="ADC_2"/>
<define name="MilliAmpereOfAdc(adc)" value="Max(0,(3100 - adc)*20)"/>
-->
</section> </section>
</airframe> </airframe>
conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
+42 -41
View File
@@ -5,11 +5,6 @@
<firmware name="rotorcraft"> <firmware name="rotorcraft">
<target name="ap" board="krooz_sd"/> <target name="ap" board="krooz_sd"/>
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target>
<subsystem name="radio_control" type="ppm"/> <subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/> <subsystem name="telemetry" type="transparent"/>
<subsystem name="motor_mixing"/> <subsystem name="motor_mixing"/>
@@ -24,16 +19,19 @@
<subsystem name="ins" type="hff"/> <subsystem name="ins" type="hff"/>
<define name="NO_RC_THRUST_LIMIT"/> <define name="NO_RC_THRUST_LIMIT"/>
<define name="USE_RC_FP_BLOCK_SWITCHING"/> <define name="FAILSAFE_GROUND_DETECT" value="1"/>
<define name="USE_ATTITUDE_REF" value="0"/> <define name="THRESHOLD_GROUND_DETECT" value="12."/>
<define name="KILL_ON_GROUND_DETECT" value="TRUE"/> <define name="USE_GPS_ACC4R" value="1"/>
<define name="HFF_R_POS_MIN" value="2.5"/>
<define name="HFF_R_SPEED_MIN" value="2."/>
<define name="VF_FLOAT_MEAS_NOISE" value="2."/>
<define name="USE_ADC_2" value="1"/>
</firmware> </firmware>
<modules main_freq="512"> <modules main_freq="512">
<!--<load name="gps_ubx_ucenter.xml"/> <load name="gps_ubx_ucenter.xml"/>
<load name="sys_mon.xml"/>
<load name="geo_mag.xml"/> <load name="geo_mag.xml"/>
<load name="osd_max7456.xml"/>--> <load name="osd_max7456.xml"/>
</modules> </modules>
<commands> <commands>
@@ -114,7 +112,6 @@
</section> </section>
<section name="AHRS" prefix="AHRS_"> <section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_LOW_PASS_RATES" value="1"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="30"/> <define name="GRAVITY_HEURISTIC_FACTOR" value="30"/>
<define name="H_X" value="0.3586845"/> <define name="H_X" value="0.3586845"/>
<define name="H_Y" value="0.0168651"/> <define name="H_Y" value="0.0168651"/>
@@ -160,29 +157,29 @@
<define name="DEADBAND_R" value="250"/> <define name="DEADBAND_R" value="250"/>
<!-- reference --> <!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s"/> <define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/> <define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="100." unit="deg/s"/> <define name="REF_MAX_P" value="180." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(2000.)"/> <define name="REF_MAX_PDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/> <define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/> <define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="100." unit="deg/s"/> <define name="REF_MAX_Q" value="180." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(2000.)"/> <define name="REF_MAX_QDOT" value="RadOfDeg(400.)"/>
<define name="REF_OMEGA_R" value="500" unit="deg/s"/> <define name="REF_OMEGA_R" value="400" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/> <define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="90." unit="deg/s"/> <define name="REF_MAX_R" value="70." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(180.)"/> <define name="REF_MAX_RDOT" value="RadOfDeg(360.)"/>
<!-- feedback --> <!-- feedback -->
<define name="PHI_PGAIN" value="850"/> <define name="PHI_PGAIN" value="1500"/>
<define name="PHI_DGAIN" value="400"/> <define name="PHI_DGAIN" value="600"/>
<define name="PHI_IGAIN" value="100"/> <define name="PHI_IGAIN" value="200"/>
<define name="THETA_PGAIN" value="850"/> <define name="THETA_PGAIN" value="1500"/>
<define name="THETA_DGAIN" value="400"/> <define name="THETA_DGAIN" value="600"/>
<define name="THETA_IGAIN" value="100"/> <define name="THETA_IGAIN" value="200"/>
<define name="PSI_PGAIN" value="4000"/> <define name="PSI_PGAIN" value="4000"/>
<define name="PSI_DGAIN" value="1500"/> <define name="PSI_DGAIN" value="1500"/>
@@ -200,28 +197,26 @@
<define name="MIN_ERR_ZD" value="SPEED_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_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/> <define name="MAX_SUM_ERR" value="2000000"/>
<define name="TILT_COEFF" value="0.35"/> <define name="REF_MIN_ZDD" value="-0.2*9.81"/> <!-- max descent acceleration in m/s2 -->
<define name="REF_MIN_ZDD" value="-0.2*9.81"/> <!-- max descent acceleration --> <define name="REF_MAX_ZDD" value="0.2*9.81"/> <!-- max climb acceleration in m/s2 -->
<define name="REF_MAX_ZDD" value="0.2*9.81"/> <!-- max climb acceleration --> <define name="REF_MIN_ZD" value="-3."/> <!-- max descent speed in m/s -->
<define name="REF_MIN_ZD" value="-3."/> <!-- max descent speed --> <define name="REF_MAX_ZD" value="3."/> <!-- max climb speed in m/s -->
<define name="REF_MAX_ZD" value="3."/> <!-- max climb speed --> <define name="ADAPT_NOISE_FACTOR" value="0.7"/>
<define name="HOVER_KP" value="120"/> <define name="HOVER_KP" value="130"/>
<define name="HOVER_KD" value="80"/> <define name="HOVER_KD" value="150"/>
<define name="HOVER_KI" value="20"/> <define name="HOVER_KI" value="10"/>
<define name="RC_DESCENT_COEF" value ="200"/>
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="USE_REF" value="1"/> <define name="USE_REF" value="1"/>
<define name="USE_SPEED_REF" value="1"/> <!-- using RC to control horizontal setpoint --> <define name="USE_SPEED_REF" value="1"/> <!-- using RC to control horizontal setpoint -->
<define name="MAX_BANK" value="27" unit="deg"/> <define name="MAX_BANK" value="27" unit="deg"/>
<define name="REF_MAX_ACCEL" value="1.2"/> <!-- max reference horizontal acceleration in m/s2 --> <define name="REF_MAX_ACCEL" value="7."/> <!-- max reference horizontal acceleration in m/s2 -->
<define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s --> <define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s -->
<define name="RC_SPEED_DEAD_BAND" value="200000"/>
<define name="PGAIN" value="100"/> <define name="PGAIN" value="100"/>
<define name="DGAIN" value="180"/> <define name="DGAIN" value="120"/>
<define name="IGAIN" value="3"/> <define name="IGAIN" value="10"/>
<define name="AGAIN" value="20"/> <define name="AGAIN" value="10"/>
</section> </section>
<section name="SIMULATOR" prefix="NPS_"> <section name="SIMULATOR" prefix="NPS_">
@@ -240,6 +235,12 @@
<define name="CRITIC_BAT_LEVEL" value="13.0" unit="V"/> <define name="CRITIC_BAT_LEVEL" value="13.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="13.2" unit="V"/> <define name="LOW_BAT_LEVEL" value="13.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="16.8" unit="V"/> <define name="MAX_BAT_LEVEL" value="16.8" unit="V"/>
<!--uncomment next two lines together with a corresponding USE_ADC_2 define on the top of this file to use a current sensor
(more about is here http://paparazzi.enac.fr/wiki/Sensors/Current) -->
<!--
<define name="ADC_CHANNEL_CURRENT" value="ADC_2"/>
<define name="MilliAmpereOfAdc(adc)" value="Max(0,(3100 - adc)*20)"/>
-->
</section> </section>
</airframe> </airframe>
conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
+23 -13
View File
@@ -24,12 +24,18 @@
<subsystem name="motor_mixing"/> <subsystem name="motor_mixing"/>
<define name="NO_RC_THRUST_LIMIT"/> <define name="NO_RC_THRUST_LIMIT"/>
<define name="FAILSAFE_GROUND_DETECT" value="1"/>
<define name="THRESHOLD_GROUND_DETECT" value="12."/>
<define name="USE_GPS_ACC4R" value="1"/>
<define name="HFF_R_POS_MIN" value="2.5"/>
<define name="HFF_R_SPEED_MIN" value="2."/>
<define name="VF_FLOAT_MEAS_NOISE" value="2."/>
<define name="USE_ADC_2" value="1"/>
</firmware> </firmware>
<modules main_freq="512"> <modules main_freq="512">
<!--<load name="gps_ubx_ucenter.xml"/> <load name="gps_ubx_ucenter.xml"/>
<load name="sys_mon.xml"/> <load name="geo_mag.xml"/>
<load name="geo_mag.xml"/>-->
<load name="osd_max7456.xml"/> <load name="osd_max7456.xml"/>
</modules> </modules>
@@ -102,9 +108,7 @@
</section> </section>
<section name="AHRS" prefix="AHRS_"> <section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_LOW_PASS_RATES" value="1"/>
<define name="GRAVITY_HEURISTIC_FACTOR" value="30"/> <define name="GRAVITY_HEURISTIC_FACTOR" value="30"/>
<define name="MAG_CORRECT_FREQUENCY" value="75"/>
<define name="H_X" value="0.3586845"/> <define name="H_X" value="0.3586845"/>
<define name="H_Y" value="0.0168651"/> <define name="H_Y" value="0.0168651"/>
<define name="H_Z" value="0.933303"/> <define name="H_Z" value="0.933303"/>
@@ -189,14 +193,14 @@
<define name="MIN_ERR_ZD" value="SPEED_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_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/> <define name="MAX_SUM_ERR" value="2000000"/>
<define name="REF_MIN_ZDD" value="-2.5"/> <!-- max climb acceleration --> <define name="REF_MIN_ZDD" value="-0.2*9.81"/> <!-- max descent acceleration in m/s2 -->
<define name="REF_MAX_ZDD" value=" 2.5"/> <!-- max descent acceleration --> <define name="REF_MAX_ZDD" value="0.2*9.81"/> <!-- max climb acceleration in m/s2 -->
<define name="REF_MIN_ZD" value="-3."/> <!-- max climb speed --> <define name="REF_MIN_ZD" value="-3."/> <!-- max descent speed in m/s -->
<define name="REF_MAX_ZD" value="3."/> <!-- max descent speed --> <define name="REF_MAX_ZD" value="3."/> <!-- max climb speed in m/s -->
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.35"/> <!-- initial takeoff throttle value --> <define name="ADAPT_NOISE_FACTOR" value="0.7"/>
<define name="HOVER_KP" value="130"/> <define name="HOVER_KP" value="130"/>
<define name="HOVER_KD" value="120"/> <define name="HOVER_KD" value="150"/>
<define name="HOVER_KI" value="20"/> <define name="HOVER_KI" value="10"/>
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
@@ -206,7 +210,7 @@
<define name="REF_MAX_ACCEL" value="7."/> <!-- max reference horizontal acceleration in m/s2 --> <define name="REF_MAX_ACCEL" value="7."/> <!-- max reference horizontal acceleration in m/s2 -->
<define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s --> <define name="REF_MAX_SPEED" value="2."/> <!-- max reference horizontal speed in m/s -->
<define name="PGAIN" value="100"/> <define name="PGAIN" value="100"/>
<define name="DGAIN" value="220"/> <define name="DGAIN" value="120"/>
<define name="IGAIN" value="10"/> <define name="IGAIN" value="10"/>
<define name="AGAIN" value="10"/> <define name="AGAIN" value="10"/>
</section> </section>
@@ -228,6 +232,12 @@
<define name="CRITIC_BAT_LEVEL" value="14.0" unit="V"/> <define name="CRITIC_BAT_LEVEL" value="14.0" unit="V"/>
<define name="LOW_BAT_LEVEL" value="14.2" unit="V"/> <define name="LOW_BAT_LEVEL" value="14.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="16.8" unit="V"/> <define name="MAX_BAT_LEVEL" value="16.8" unit="V"/>
<!--uncomment next two lines together with a corresponding USE_ADC_2 define on the top of this file to use a current sensor
(more about is here http://paparazzi.enac.fr/wiki/Sensors/Current) -->
<!--
<define name="ADC_CHANNEL_CURRENT" value="ADC_2"/>
<define name="MilliAmpereOfAdc(adc)" value="Max(0,(3100 - adc)*20)"/>
-->
</section> </section>
</airframe> </airframe>
conf/firmwares/subsystems/shared/imu_krooz_sd.makefile
+2
View File
@@ -20,8 +20,10 @@ IMU_KROOZ_SRCS += peripherals/hmc58xx.c
AHRS_PROPAGATE_FREQUENCY ?= 512 AHRS_PROPAGATE_FREQUENCY ?= 512
AHRS_CORRECT_FREQUENCY ?= 512 AHRS_CORRECT_FREQUENCY ?= 512
AHRS_MAG_CORRECT_FREQUENCY ?= 75
ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
ap.CFLAGS += -DAHRS_MAG_CORRECT_FREQUENCY=$(AHRS_MAG_CORRECT_FREQUENCY)
ap.CFLAGS += $(IMU_KROOZ_CFLAGS) ap.CFLAGS += $(IMU_KROOZ_CFLAGS)
ap.srcs += $(IMU_KROOZ_SRCS) ap.srcs += $(IMU_KROOZ_SRCS)
conf/firmwares/subsystems/shared/imu_krooz_sd_memsic.makefile
+39
View File
@@ -0,0 +1,39 @@
# Hey Emacs, this is a -*- makefile -*-
#
# KroozSD IMU
#
include $(CFG_SHARED)/spi_master.makefile
IMU_KROOZ_CFLAGS = -DUSE_IMU
IMU_KROOZ_CFLAGS += -DIMU_TYPE_H=\"boards/krooz/imu_krooz.h\"
IMU_KROOZ_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
$(SRC_BOARD)/imu_krooz_memsic.c \
$(SRC_ARCH)/subsystems/imu/imu_krooz_sd_arch.c
IMU_KROOZ_I2C_DEV=i2c2
IMU_KROOZ_SPI_DEV=spi2
IMU_KROOZ_CFLAGS += -DUSE_I2C -DUSE_I2C2 -DI2C2_CLOCK_SPEED=400000
IMU_KROOZ_CFLAGS += -DUSE_SPI -DUSE_SPI2 -DUSE_SPI_SLAVE1 -DUSE_SPI_SLAVE2 -DIMU_KROOZ_SPI_SLAVE_IDX=1
IMU_KROOZ_CFLAGS += -DIMU_KROOZ_I2C_DEV=$(IMU_KROOZ_I2C_DEV)
IMU_KROOZ_CFLAGS += -DIMU_KROOZ_SPI_DEV=$(IMU_KROOZ_SPI_DEV)
IMU_KROOZ_SRCS += peripherals/mpu60x0.c
IMU_KROOZ_SRCS += peripherals/mpu60x0_i2c.c
IMU_KROOZ_SRCS += peripherals/hmc58xx.c
AHRS_PROPAGATE_FREQUENCY ?= 512
AHRS_CORRECT_FREQUENCY ?= 512
AHRS_MAG_CORRECT_FREQUENCY ?= 75
ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
ap.CFLAGS += -DAHRS_MAG_CORRECT_FREQUENCY=$(AHRS_MAG_CORRECT_FREQUENCY)
ap.CFLAGS += $(IMU_KROOZ_CFLAGS)
ap.srcs += $(IMU_KROOZ_SRCS)
#
# NPS simulator
#
include $(CFG_SHARED)/imu_nps.makefile
sw/airborne/boards/krooz/imu_krooz.c
+1 -16
View File
@@ -59,10 +59,6 @@ PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE)
struct ImuKrooz imu_krooz; struct ImuKrooz imu_krooz;
#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
struct MedianFilter3Int median_gyro;
#endif
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
struct MedianFilter3Int median_accel; struct MedianFilter3Int median_accel;
#endif #endif
@@ -83,9 +79,6 @@ void imu_impl_init( void )
hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR); hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
// Init median filters // Init median filters
#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
InitMedianFilterRatesInt(median_gyro);
#endif
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
InitMedianFilterVect3Int(median_accel); InitMedianFilterVect3Int(median_accel);
#endif #endif
@@ -116,19 +109,11 @@ void imu_periodic( void )
if (imu_krooz.meas_nb) { if (imu_krooz.meas_nb) {
RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb); RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled);
#endif
VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb); VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif #endif
RATES_SMUL(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, IMU_KROOZ_GYRO_AVG_FILTER);
RATES_ADD(imu_krooz.gyro_filtered, imu.gyro_unscaled);
RATES_SDIV(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, (IMU_KROOZ_GYRO_AVG_FILTER + 1));
RATES_COPY(imu.gyro_unscaled, imu_krooz.gyro_filtered);
VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER); VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled); VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1)); VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
sw/airborne/boards/krooz/imu_krooz.h
+2 -5
View File
@@ -100,11 +100,8 @@
#define IMU_ACCEL_Z_NEUTRAL 0 #define IMU_ACCEL_Z_NEUTRAL 0
#endif #endif
#ifndef IMU_KROOZ_GYRO_AVG_FILTER
#define IMU_KROOZ_GYRO_AVG_FILTER 5
#endif
#ifndef IMU_KROOZ_ACCEL_AVG_FILTER #ifndef IMU_KROOZ_ACCEL_AVG_FILTER
#define IMU_KROOZ_ACCEL_AVG_FILTER 10 #define IMU_KROOZ_ACCEL_AVG_FILTER 15
#endif #endif
struct ImuKrooz { struct ImuKrooz {
@@ -119,7 +116,7 @@ struct ImuKrooz {
struct Int32Vect3 accel_sum; struct Int32Vect3 accel_sum;
volatile uint8_t meas_nb; volatile uint8_t meas_nb;
struct Int32Vect3 accel_filtered; struct Int32Vect3 accel_filtered;
struct Int32Rates gyro_filtered; int32_t temperature;
}; };
extern struct ImuKrooz imu_krooz; extern struct ImuKrooz imu_krooz;
sw/airborne/boards/krooz/imu_krooz_memsic.c
+226
View File
@@ -0,0 +1,226 @@
/*
* Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
*
* 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 boards/krooz/imu_krooz_memsic.c
*
* Driver for the IMU on the KroozSD Big Rotorcraft Edition board.
*
* Invensense MPU-6050
* Memsic MXR9500 with AD7689
* Honeywell HMC-5883
*/
#include <math.h>
#include "boards/krooz/imu_krooz_memsic.h"
#include "subsystems/imu/imu_krooz_sd_arch.h"
#include "mcu_periph/i2c.h"
#include "led.h"
#include "filters/median_filter.h"
#include "mcu_periph/sys_time.h"
#if !defined KROOZ_LOWPASS_FILTER && !defined KROOZ_SMPLRT_DIV
#define KROOZ_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define KROOZ_SMPLRT_DIV 1
#endif
PRINT_CONFIG_VAR(KROOZ_SMPLRT_DIV)
PRINT_CONFIG_VAR(KROOZ_LOWPASS_FILTER)
#ifndef KROOZ_GYRO_RANGE
#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
#endif
PRINT_CONFIG_VAR(KROOZ_GYRO_RANGE)
#ifndef KROOZ_ACCEL_RANGE
#define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G
#endif
PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE)
struct ImuKrooz imu_krooz;
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
struct MedianFilter3Int median_accel;
#endif
struct MedianFilter3Int median_mag;
static uint32_t ad7689_event_timer;
static uint8_t axis_cnt;
static uint8_t axis_nb;
void imu_impl_init( void )
{
/////////////////////////////////////////////////////////////////////
// MPU-60X0
mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR);
// change the default configuration
imu_krooz.mpu.config.smplrt_div = KROOZ_SMPLRT_DIV;
imu_krooz.mpu.config.dlpf_cfg = KROOZ_LOWPASS_FILTER;
imu_krooz.mpu.config.gyro_range = KROOZ_GYRO_RANGE;
imu_krooz.mpu.config.accel_range = KROOZ_ACCEL_RANGE;
imu_krooz.mpu.config.drdy_int_enable = TRUE;
hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
// Init median filters
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
InitMedianFilterVect3Int(median_accel);
#endif
InitMedianFilterVect3Int(median_mag);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.gyr_valid = FALSE;
imu_krooz.acc_valid = FALSE;
imu_krooz.mag_valid = FALSE;
imu_krooz.hmc_eoc = FALSE;
imu_krooz.mpu_eoc = FALSE;
imu_krooz.ad7689_trans.slave_idx = IMU_KROOZ_SPI_SLAVE_IDX;
imu_krooz.ad7689_trans.select = SPISelectUnselect;
imu_krooz.ad7689_trans.cpol = SPICpolIdleLow;
imu_krooz.ad7689_trans.cpha = SPICphaEdge1;
imu_krooz.ad7689_trans.dss = SPIDss8bit;
imu_krooz.ad7689_trans.bitorder = SPIMSBFirst;
imu_krooz.ad7689_trans.cdiv = SPIDiv16;
imu_krooz.ad7689_trans.output_length = sizeof(imu_krooz.ad7689_spi_tx_buffer);
imu_krooz.ad7689_trans.output_buf = (uint8_t*) imu_krooz.ad7689_spi_tx_buffer;
imu_krooz.ad7689_trans.input_length = sizeof(imu_krooz.ad7689_spi_rx_buffer);
imu_krooz.ad7689_trans.input_buf = (uint8_t*) imu_krooz.ad7689_spi_rx_buffer;
imu_krooz.ad7689_trans.before_cb = NULL;
imu_krooz.ad7689_trans.after_cb = NULL;
axis_cnt = 0;
axis_nb = 2;
imu_krooz_sd_arch_init();
}
void imu_periodic( void )
{
// Start reading the latest gyroscope data
if (!imu_krooz.mpu.config.initialized)
mpu60x0_i2c_start_configure(&imu_krooz.mpu);
if (!imu_krooz.hmc.initialized)
hmc58xx_start_configure(&imu_krooz.hmc);
if (imu_krooz.meas_nb) {
RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb,
imu_krooz.rates_sum.p / imu_krooz.meas_nb,
imu_krooz.rates_sum.r / imu_krooz.meas_nb);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.gyr_valid = TRUE;
}
if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) {
imu.accel_unscaled.x = 65536 - imu_krooz.accel_sum.x / imu_krooz.meas_nb_acc.x;
imu.accel_unscaled.y = 65536 - imu_krooz.accel_sum.y / imu_krooz.meas_nb_acc.y;
imu.accel_unscaled.z = imu_krooz.accel_sum.z / imu_krooz.meas_nb_acc.z;
#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);
INT_VECT3_ZERO(imu_krooz.accel_sum);
INT_VECT3_ZERO(imu_krooz.meas_nb_acc);
imu_krooz.acc_valid = TRUE;
}
RunOnceEvery(128,{axis_nb = 5;});
}
void imu_krooz_event( void )
{
if (imu_krooz.mpu_eoc) {
mpu60x0_i2c_read(&imu_krooz.mpu);
imu_krooz.mpu_eoc = FALSE;
}
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates);
imu_krooz.meas_nb++;
imu_krooz.mpu.data_available = FALSE;
}
if(SysTimeTimer(ad7689_event_timer) > 215) {
SysTimeTimerStart(ad7689_event_timer);
if(axis_cnt < axis_nb)
axis_cnt++;
else
axis_cnt = 0;
imu_krooz.ad7689_trans.output_buf[0] =
axis_cnt <= 2 ? 0xF0 | (axis_cnt << 1) : (axis_cnt >= 4 ? 0xF0 | ((axis_cnt - 3) << 1) : 0xB0);
imu_krooz.ad7689_trans.output_buf[1] = 0x44;
spi_submit(&(IMU_KROOZ_SPI_DEV), &imu_krooz.ad7689_trans);
}
if (imu_krooz.ad7689_trans.status == SPITransSuccess) {
imu_krooz.ad7689_trans.status = SPITransDone;
uint16_t buf = (imu_krooz.ad7689_trans.input_buf[0] << 8) | imu_krooz.ad7689_trans.input_buf[1];
switch(axis_cnt) {
case 0:
case 3:
imu_krooz.accel_sum.x += (int32_t)buf;
imu_krooz.meas_nb_acc.x++;
break;
case 1:
case 4:
imu_krooz.accel_sum.y += (int32_t)buf;
imu_krooz.meas_nb_acc.y++;
break;
case 2:
imu_krooz.accel_sum.z += (int32_t)buf;
imu_krooz.meas_nb_acc.z++;
break;
case 5:
imu_krooz.temperature = (imu_krooz.temperature * 4 + (int32_t)buf) / 5;
//imu.temperature = 33000 * imu_krooz.temp / 65536 - 2400;
axis_nb = 2;
break;
default:
axis_cnt = 0;
break;
}
}
if (imu_krooz.hmc_eoc) {
hmc58xx_read(&imu_krooz.hmc);
imu_krooz.hmc_eoc = FALSE;
}
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
imu_krooz.hmc.data_available = FALSE;
imu_krooz.mag_valid = TRUE;
}
}
sw/airborne/boards/krooz/imu_krooz_memsic.h
+156
View File
@@ -0,0 +1,156 @@
/*
* Copyright (C) 2013 Sergey Krukowski <softsr@yahoo.de>
*
* 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 boards/krooz/imu_krooz_memsic.h
*
* Driver for the IMU on the KroozSD Big Rotorcraft Edition board.
*
* Invensense MPU-6050
* Memsic MXR9500 with AD7689
* Honeywell HMC-5883
*/
#ifndef IMU_KROOZ_H
#define IMU_KROOZ_H
#include "std.h"
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "peripherals/mpu60x0_i2c.h"
#include "peripherals/hmc58xx.h"
#include "mcu_periph/spi.h"
// Default configuration
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
#define IMU_MAG_X_SIGN 1
#define IMU_MAG_Y_SIGN 1
#define IMU_MAG_Z_SIGN 1
#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU with 250 deg/s has 131.072 LSB/(deg/s)
* sens = 1/131.072 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/131.072 * pi/180 * 4096 = 0.5454
I*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 0.5454
#define IMU_GYRO_P_SENS_NUM 2727
#define IMU_GYRO_P_SENS_DEN 5000
#define IMU_GYRO_Q_SENS 0.5454
#define IMU_GYRO_Q_SENS_NUM 2727
#define IMU_GYRO_Q_SENS_DEN 5000
#define IMU_GYRO_R_SENS 0.5454
#define IMU_GYRO_R_SENS_NUM 2727
#define IMU_GYRO_R_SENS_DEN 5000
#endif
#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
#define IMU_GYRO_P_NEUTRAL 0
#define IMU_GYRO_Q_NEUTRAL 0
#define IMU_GYRO_R_NEUTRAL 0
#endif
/** default accel sensitivy using 16 bit AD7689 adc
* MXR9500 with 1.5g has 21845 LSB/g
* sens = 9.81 [m/s^2] / 21845 [LSB/g] * 2^INT32_ACCEL_FRAC = 0.6131
*/
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 0.9197
#define IMU_ACCEL_X_SENS_NUM 9197
#define IMU_ACCEL_X_SENS_DEN 10000
#define IMU_ACCEL_Y_SENS 0.9197
#define IMU_ACCEL_Y_SENS_NUM 9197
#define IMU_ACCEL_Y_SENS_DEN 10000
#define IMU_ACCEL_Z_SENS 0.9197
#define IMU_ACCEL_Z_SENS_NUM 9197
#define IMU_ACCEL_Z_SENS_DEN 10000
#endif
#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
#define IMU_ACCEL_X_NEUTRAL 32768
#define IMU_ACCEL_Y_NEUTRAL 32768
#define IMU_ACCEL_Z_NEUTRAL 32768
#endif
#ifndef IMU_KROOZ_ACCEL_AVG_FILTER
#define IMU_KROOZ_ACCEL_AVG_FILTER 15
#endif
struct ImuKrooz {
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
volatile bool_t mag_valid;
volatile bool_t mpu_eoc;
volatile bool_t hmc_eoc;
struct Mpu60x0_I2c mpu;
struct spi_transaction ad7689_trans;
volatile uint8_t ad7689_spi_tx_buffer[2];
volatile uint8_t ad7689_spi_rx_buffer[2];
struct Hmc58xx hmc;
struct Int32Rates rates_sum;
struct Int32Vect3 accel_sum;
volatile uint8_t meas_nb;
struct Uint8Vect3 meas_nb_acc;
struct Int32Vect3 accel_filtered;
int32_t temperature;
};
extern struct ImuKrooz imu_krooz;
/* must be defined in order to be IMU code: declared in imu.h
extern void imu_impl_init(void);
extern void imu_periodic(void);
*/
/* Own Extra Functions */
extern void imu_krooz_event( void );
extern void imu_krooz_downlink_raw( void );
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) {
imu_krooz_event();
if (imu_krooz.gyr_valid) {
imu_krooz.gyr_valid = FALSE;
_gyro_handler();
}
if (imu_krooz.acc_valid) {
imu_krooz.acc_valid = FALSE;
_accel_handler();
}
if (imu_krooz.mag_valid) {
imu_krooz.mag_valid = FALSE;
_mag_handler();
}
}
#endif // IMU_KROOZ_H