Skip to content

Commit

Permalink
[conf] updated walkera genius v2 (pid) (#1742)
Browse files Browse the repository at this point in the history
  • Loading branch information
@bartslinger @flixr
bartslinger authored and flixr committed Jun 17, 2016
1 parent 19a0ad3 commit 94826c4
Show file tree
Hide file tree
Showing 2 changed files with 207 additions and 138 deletions.
37 changes: 37 additions & 0 deletions conf/airframes/TUDELFT/calibrations/walkera_2016_03_16.xml
View file
@@ -0,0 +1,37 @@
<airframe>
<section name="IMU" prefix="IMU_">

<!-- From Delft MAVlab, quick calibration -->
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<!--define name="ACCEL_X_NEUTRAL" value="62"/>
<define name="ACCEL_Y_NEUTRAL" value="16"/>
<define name="ACCEL_Z_NEUTRAL" value="-191"/>
<define name="ACCEL_X_SENS" value="4.86205019187" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.88606377201" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.84406734807" integer="16"/-->


<!-- From delft MAVLab 16/3 in t lab-->
<define name="MAG_X_NEUTRAL" value="81"/>
<define name="MAG_Y_NEUTRAL" value="-214"/>
<define name="MAG_Z_NEUTRAL" value="-75"/>
<define name="MAG_X_SENS" value="3.4123397367" integer="16"/>
<define name="MAG_Y_SENS" value="3.5176940188" integer="16"/>
<define name="MAG_Z_SENS" value="3.93401804914" integer="16"/>

<!-- Current correction -->
<!--define name= "MAG_X_CURRENT_COEF" value="-0.00826591496471"/>
<define name= "MAG_Y_CURRENT_COEF" value="0.0504179871004"/>
<define name= "MAG_Z_CURRENT_COEF" value="0.000356565353913"/-->
</section>

<section name="AHRS" prefix="AHRS_">
<!-- (0.390835, 0.00556658, 0.920444) 16mrt2016 delft -->
<define name="H_X" value=" 0.390835"/>
<define name="H_Y" value=" 0.00556658"/>
<define name="H_Z" value=" 0.920444"/>
</section>

</airframe>
308 changes: 170 additions & 138 deletions conf/airframes/TUDELFT/tudelft_walkera_genius_v2.xml
View file
Expand Up @@ -7,108 +7,80 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
-->

<airframe name="walkera_genius_v2">
<firmware name="rotorcraft">
<target name="ap" board="lisa_s_1.0">
<!--define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="1"/-->
<configure name="USE_MAGNETOMETER" value="0"/>
<define name="AHRS_USE_GPS_HEADING" value="1"/>
<define name="AHRS_GRAVITY_HEURISTIC_FACTOR" value="0"/>

<servos driver="Pwm">
<servo name="CIC_FRONT" no="4" min="1000" neutral="1400" max="1700"/>
<servo name="CIC_RIGHT" no="5" min="1150" neutral="1450" max="1850"/>
<servo name="CIC_LEFT" no="0" min="1000" neutral="1400" max="1700"/>
<servo name="TAIL" no="2" min="0" neutral="0" max="500"/>
<servo name="GAS" no="3" min="1890" neutral="1890" max="1430"/>
</servos>
<subsystem name="radio_control" type="ppm">
<configure name="RADIO_CONTROL_PPM_PIN" value="SUPERBIT_RST"/>
</subsystem>

<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<!--configure name="USE_BARO_BOARD" value="0"/-->
<define name="RADIO_KILL_SWITCH" value="RADIO_FLAP"/>

<command_laws>
<let var="hoverpower" value=".85*MAX_PPRZ"/>
<let var="hoverstick" value=".35*MAX_PPRZ"/>
<let var="halfway" value="(@THRUST >= ($hoverstick) ? 1 : 0)"/>
<let var="collective" value="@THRUST * 0.1258 + (MAX_PPRZ*0.2013)"/>
<!--let var="collective" value="@PITCH"/-->
<let var="gas" value="($hoverpower) + $halfway * (((@THRUST) - ($hoverstick)) * (MAX_PPRZ - ($hoverpower))) / (MAX_PPRZ - ($hoverstick)) + (1 - $halfway) * ((@THRUST - ($hoverstick)) * ($hoverpower)) / ($hoverstick)"/>

<set servo="CIC_LEFT" value="((@PITCH*1.75*0.5*0.5)-(@ROLL*1.75*0.5*0.73))-($collective)"/>
<set servo="CIC_RIGHT" value="((-@PITCH*1.75*0.5*0.5)-(@ROLL*1.75*0.5*0.73))+($collective)"/>
<set servo="CIC_FRONT" value="-@PITCH*1.75*0.5-($collective)"/>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
</target>

<!--set servo="CIC_LEFT" value="-$collective"/>
<set servo="CIC_RIGHT" value="$collective"/>
<set servo="CIC_FRONT" value="-$collective"/-->

<set servo="GAS" value="$gas"/>
<set servo="TAIL" value="@YAW + $halfway * ((MAX_PPRZ*0.4) + @THRUST*0.2037) + (1 - $halfway) * (@THRUST * 1.4872) "/>
<!--set servo="TAIL" value="0"/-->

<!--set servo="GAS" value="$gas"/>
<set servo="TAIL" value="@YAW+0.4*$gas"/-->

</command_laws>
<subsystem name="actuators" type="pwm">
<define name="TIM5_SERVO_HZ" value="300"/>
<define name="SERVO_HZ" value="300"/>
<define name="USE_SERVOS_1AND2"/>
</subsystem>

<subsystem name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/>
<configure name="MODEM_PORT" value="UART1"/>
</subsystem>
<subsystem name="imu" type="lisa_s_v1.0">
<define name="LISA_S_UPSIDE_DOWN"/> <!-- wtf hij zit gewoon rechtop maargoed -->
</subsystem>
<module name="gps" type="optitrack"/>
<subsystem name="stabilization" type="int_quat"/> <!-- int_quat / heli_indi -->
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins"/>
</firmware>

<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="184." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="-4.5" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0" unit="deg"/>

<!-- From Delft MAVlab, quick calibration -->
<define name="ACCEL_X_NEUTRAL" value="0"/>
<define name="ACCEL_Y_NEUTRAL" value="0"/>
<define name="ACCEL_Z_NEUTRAL" value="0"/>
<!--define name="ACCEL_X_NEUTRAL" value="62"/>
<define name="ACCEL_Y_NEUTRAL" value="16"/>
<define name="ACCEL_Z_NEUTRAL" value="-191"/>
<define name="ACCEL_X_SENS" value="4.86205019187" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.88606377201" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.84406734807" integer="16"/-->


<!-- From delft MAVLab -->
<define name="MAG_X_NEUTRAL" value="396"/>
<define name="MAG_Y_NEUTRAL" value="-260"/>
<define name="MAG_Z_NEUTRAL" value="-91"/>
<define name="MAG_X_SENS" value="3.71635905504" integer="16"/>
<define name="MAG_Y_SENS" value="3.5526454063" integer="16"/>
<define name="MAG_Z_SENS" value="3.52339566014" integer="16"/>

<!-- Current correction -->
<define name="MAG_X_CURRENT_COEF" value="-0.390312445109"/>
<define name="MAG_Y_CURRENT_COEF" value="-0.068976232866"/>
<define name="MAG_Z_CURRENT_COEF" value="-0.181415585657"/>
<define name="BODY_TO_IMU_PHI" value="1." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="4.0" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0.0" unit="deg"/>
</section>

<section name="AUTOPILOT">
<define name="MODE_AUTO2" value="AP_MODE_RC_DIRECT"/> <!--AP_MODE_ATTITUDE_DIRECT-->
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_MANUAL" value="AP_MODE_KILL"/> <!--AP_MODE_ATTITUDE_Z_HOLD-->
</section>

<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="3700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="3700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="3.2" unit="V"/>
<define name="LOW_BAT_LEVEL" value="3.5" unit="V"/>
<define name="MAX_BAT_LEVEL" value="4.2" unit="V"/>
</section>
</section>

<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="360" unit="deg/s"/>
<define name="SP_MAX_Q" value="360" unit="deg/s"/>
<define name="SP_MAX_R" value="360" unit="deg/s"/>

<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
</section>

<!-- This section contains gains for PID -->
<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="STEADY_STATE_ROLL" value="4.5"/>
<define name="STEADY_STATE_PITCH" value="0"/>
<define name="DEADBAND_R" value="250"/>
<define name="DEADBAND_A" value="250"/>

<!-- reference -->
<!-- reference-->
<define name="REF_OMEGA_P" value="3000" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="300." unit="deg/s"/>
Expand Down Expand Up @@ -147,75 +119,135 @@
<define name="PSI_AGAIN" value="0"/>
</section>

<section name="INS" prefix="INS_">
<section name="AHRS" prefix="AHRS_">
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</section>

<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="281"/>
<define name="HOVER_KD" value="66"/>
<define name="HOVER_KI" value="0"/>
<define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value="0.9"/>
<section name="RPM_SENSOR" prefix="RPM_SENSOR_">
<define name="PULSES_PER_ROTATION" value="6"/>
</section>

<section name="AHRS" prefix="AHRS_">
<define name="H_X" value=" 0.47577"/>
<define name="H_Y" value=" 0.11811"/>
<define name="H_Z" value=" 0.87161"/>
<define name="USE_RPM_SENSOR_NOTCH" value="1"/>
<define name="NOTCH_FILTER_BANDWIDTH" value="10.0"/>
<!-- magneto calibration -->
<include href="conf/airframes/TUDELFT/calibrations/walkera_2016_03_16.xml"/>

<heli_curves>
<curve throttle="0,4800,7200,8400,9600" collective="-1500,-1500,0,3000,3000"/>
</heli_curves>

<servos driver="Pwm">
<servo name="CIC_FRONT" no="0" min="1000" neutral="1400" max="1700"/>
<servo name="CIC_RIGHT" no="5" min="1150" neutral="1450" max="1850"/>
<servo name="CIC_LEFT" no="4" min="1000" neutral="1400" max="1700"/>
<servo name="TAIL" no="1" min="0" neutral="0" max="3500"/>
<servo name="GAS" no="2" min="1000" neutral="1000" max="1890"/>
</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"/>
<axis name="FMODE" failsafe_value="-9600"/>
</commands>

<section name="MIXING" prefix="SW_MIXING_">
<!-- front left (CW), front right (CCW), back right (CW), back left (CCW) -->
<define name="TYPE" value="H120"/>
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_COLL" value="0"/>
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="50"/>
<define name="IGAIN" value="0"/>
</section>
<command_laws>
<!-- fly with fixed rpm full throttle -->
<let var="hoverstick" value=".1*MAX_PPRZ"/>
<let var="halfway" value="(@THRUST >= ($hoverstick) ? 1 : 0)"/>

<section name="RPM_SENSOR" prefix="RPM_SENSOR_">
<define name="PULSES_PER_ROTATION" value="6"/>
<call fun="throttle_curve_run(autopilot_motors_on, values)"/>
<call fun="swashplate_mixing_run(values)"/>
<set servo="CIC_FRONT" value="-swashplate_mixing.commands[SW_FRONT]"/>
<set servo="CIC_LEFT" value="-swashplate_mixing.commands[SW_LEFTBACK]"/>
<set servo="CIC_RIGHT" value="swashplate_mixing.commands[SW_RIGHTBACK]"/>
<set servo="TAIL" value="@YAW+0.4*@THRUST"/> <!-- +0.4*@THRUST met pid -->
<set servo="GAS" value="$halfway * MAX_PPRZ"/>


<!--set servo="CIC_LEFT" value="0"/>
<set servo="CIC_RIGHT" value="0"/>
<set servo="CIC_FRONT" value="0"/>
<set servo="GAS" value="@THRUST"/>
<set servo="TAIL" value="0"/-->

</command_laws>

<section name="AUTOPILOT">
<define name="MODE_AUTO2" value="AP_MODE_NAV"/> <!--AP_MODE_ATTITUDE_DIRECT-->
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD"/> <!--AP_MODE_HELI_INDI_4DOF-->
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/> <!--AP_MODE_ATTITUDE_Z_HOLD-->
</section>

<modules main_freq="512">
<module name="gps" type="ubx_ucenter"/>
<module name="send_imu_mag_current"/>
<module name="logger_sd_spi_direct">
<configure name="LOGGER_CONTROL_SWITCH" value="RADIO_AUX2"/>
<configure name="LOGGER_LED" value="3"/>
</module>
<!--module name="sys_mon"/-->
</modules>
<section name="STABILIZATION_ATTITUDE_HELI_INDI" prefix="STABILIZATION_ATTITUDE_HELI_INDI_">
<define name="ROLL_P" value="12"/><!--20 3052 -->
<define name="PITCH_P" value="8"/><!--20 3052 -->
<define name="YAW_P" value="10"/>
<define name="YAW_D" value="30"/>
<define name="USE_FAST_DYN_FILTERS" value="1" />
<define name="FAST_DYN_ROLL_BW" value="68" />
<define name="FAST_DYN_PITCH_BW" value="17" />
</section>

<firmware name="rotorcraft">
<target name="ap" board="lisa_s_1.0">
<!--module name="radio_control" type="superbitrf_rc"-->
<!--<define name="RADIO_TRANSMITTER_ID" value="2008496626"/>--> <!-- TUDelft Dx6i: TX 4 -->
<!--<define name="RADIO_TRANSMITTER_CHAN" value="6"/>-->
<!--<define name="RADIO_TRANSMITTER_PROTOCOL" value="0x01"/>-->
<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="270." unit="deg/s"/>
</section>

<module name="radio_control" type="ppm">
<configure name="RADIO_CONTROL_PPM_PIN" value="SERVO6"/>
</module>
<section name="INS" prefix="INS_">
<!-- Use GPS altitude measurments and set the R gain -->
<define name="USE_GPS_ALT" value="1"/>
<define name="VFF_R_GPS" value="0.01"/>
</section>

<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
</target>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="364"/>
<define name="HOVER_KD" value="362"/>
<define name="HOVER_KI" value="0"/>
<define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value="0.9"/>
</section>

<section name="AHRS" prefix="AHRS_">
<define name="HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
<define name="USE_RPM_SENSOR_NOTCH" value="0"/>
<define name="NOTCH_FILTER_BANDWIDTH" value="10"/>
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="165"/> <!-- 50/165 -->
<define name="DGAIN" value="185"/> <!-- 50/185 -->
<define name="IGAIN" value="31"/> <!-- 0/31 -->
</section>

<modules main_freq="512">
<load name="pwm_meas.xml"/>
<load name="rpm_sensor.xml">
<define name="RPM_PULSE_PER_RND" value="18"/>
</load>
<load name="send_imu_mag_current.xml"/>
<load name="logger_sd_spi_direct.xml">
<configure name="LOGGER_CONTROL_SWITCH" value="RADIO_AUX2"/>
<configure name="LOGGER_LED" value="3"/>
</load>
<load name="heli_swashplate_mixing.xml"/>
<load name="heli_throttle_curve.xml"/>
<!--load name="roll_disturbance.xml"/-->
<!-- DISABLED MODULES
<load name="gps_ubx_ucenter.xml"/>
<load name="adc_expansion_uart.xml"/>
<load name="air_data.xml"/>
<load name="sys_mon.xml"/>
-->
</modules>

<module name="actuators" type="pwm">
<define name="TIM5_SERVO_HZ" value="2000"/>
<define name="SERVO_HZ" value="300"/>
<define name="USE_SERVOS_1AND2"/>
</module>

<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/>
<configure name="MODEM_PORT" value="UART1"/>
</module>
<!--module name="telemetry" type="superbitrf" /-->
<module name="imu" type="lisa_s_v1.0"/>
<module name="gps" type="ublox"/>
<module name="stabilization" type="int_quat"/>
<!--module name="rpm_sensor" type="eagletree"/-->
<module name="ahrs" type="int_cmpl_quat"/>
<module name="ins"/>
</firmware>
</airframe>

0 comments on commit 94826c4

Please sign in to comment.