
<!--
     Applicable configuration:
     airframe="airframes/TUDelft/walkera_genius_v2.xml"
     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/control/stabilization_rate.xml"
-->

<airframe name="walkera_genius_v2">
  <description>
This is a Walkera Genius V2 frame equiped with Lisa/S 0.1 with brushless main rotor.
Using Lisa/S V1.0 board file as it is software compatible.
  </description>
  <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"/>

      <module name="radio_control" type="ppm">
        <configure name="RADIO_CONTROL_PPM_PIN" value="SUPERBIT_RST"/>
      </module>

      <!--configure name="USE_BARO_BOARD" value="0"/-->
      <define name="RADIO_KILL_SWITCH" value="RADIO_FLAP"/>

      <configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
      <define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
    </target>

    <module name="actuators" type="pwm">
      <define name="TIM5_SERVO_HZ" value="300"/>
      <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="imu" type="lisa_s_v1.0">
      <define name="LISA_S_UPSIDE_DOWN"/> <!-- wtf hij zit gewoon rechtop maargoed -->
    </module>
    <module name="gps" type="optitrack"/>
    <module name="stabilization" type="heli_indi"/> <!-- int_quat / heli_indi -->
    <module name="ahrs" type="int_cmpl_quat"/>
    <module name="ins"/>
  </firmware>

  <section name="IMU" prefix="IMU_">
    <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="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 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="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-->
    <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"/>
    <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>

    <define name="REF_OMEGA_Q" value="3000" unit="deg/s"/>
    <define name="REF_ZETA_Q" value="0.85"/>
    <define name="REF_MAX_Q" value="300." unit="deg/s"/>
    <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>

    <define name="REF_OMEGA_R" value="3000" unit="deg/s"/>
    <define name="REF_ZETA_R" value="0.85"/>
    <define name="REF_MAX_R" value="300." unit="deg/s"/>
    <define name="REF_MAX_RDOT" value="RadOfDeg(7000.)"/>

    <!-- feedback -->
    <define name="PHI_PGAIN" value="3052"/>
    <define name="PHI_DGAIN" value="108"/>
    <define name="PHI_IGAIN" value="0"/>

    <define name="THETA_PGAIN" value="3052"/>
    <define name="THETA_DGAIN" value="108"/>
    <define name="THETA_IGAIN" value="0"/>

    <define name="PSI_PGAIN" value="944"/>
    <define name="PSI_DGAIN" value="300"/>
    <define name="PSI_IGAIN" value="10"/>

    <!-- feedforward -->
    <define name="PHI_DDGAIN" value="0"/>
    <define name="THETA_DDGAIN" value="0"/>
    <define name="PSI_DDGAIN" value=" 300"/>

    <define name="PHI_AGAIN" value="0"/>
    <define name="THETA_AGAIN" value="0"/>
    <define name="PSI_AGAIN" value="0"/>
  </section>

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

  <section name="RPM_SENSOR" prefix="RPM_SENSOR_">
    <define name="PULSES_PER_ROTATION" value="6"/>
  </section>

  <!-- 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>

  <command_laws>
    <!-- fly with fixed rpm full throttle -->
    <let var="hoverstick" value=".1*MAX_PPRZ"/>
    <let var="halfway" value="(@THRUST >= ($hoverstick) ? 1 : 0)"/>

    <call fun="throttle_curve_run(autopilot_get_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 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>

  <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>

  <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>

  <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>

  <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>

</airframe>

