Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[boards] add driver for KroozSD BigRotorcraftEdition
- IMU with memsic MXR9500 accelerometer - set mag correct freq to 75Hz for krooz_sd_x closes #586
- Loading branch information
Showing
10 changed files
with
771 additions
and
115 deletions.
There are no files selected for viewing
241 changes: 241 additions & 0 deletions
241
conf/airframes/examples/krooz_sd/krooz_sd_bre_hexa_mkk.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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="{"front_motor", "back_motor", "right_motor", "left_motor"}"/> | ||
<define name="JSBSIM_INIT" value=""reset00""/> | ||
<define name="SENSORS_PARAMS" value=""nps_sensors_params_default.h""/> | ||
</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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.