<!DOCTYPE airframe SYSTEM "../airframe.dtd">

<!-- this is a quadrotor frame equiped with
* Autopilot:   3dr Pixhawk 2.4
* IMU:         L3GD20 +  LSM303D + MPU6000 + external HMC58XX
* Actuators:   PWM motor controllers
* GPS:         Ublox                  http://wiki.paparazziuav.org/wiki/Subsystem/gps
* RC:          PPM
-->
<airframe name="iris_indi">

  <firmware name="rotorcraft">

    <target name="ap" board="px4fmu_2.4" />
      <configure name="PERIODIC_FREQUENCY" value="200"/>
      <configure name="TELEMETRY_FREQUENCY" value="60"/>

    <comment>
      amount of time it take for the bat to check
      to avoid bat low spike detection when strong pullup withch draws short sudden power
    </comment>
    <define name="BAT_CHECKER_DELAY" value="80" /><!-- in seconds-->
    <define name="CATASTROPHIC_BATTERY_KILL_DELAY" value="80" /><!-- in seconds-->

    <module name="telemetry" type="transparent">
      <configure name="MODEM_PORT" value="UART2"/>
      <configure name="MODEM_BAUD" value="B115200"/>
    </module>

    <module name="imu" type="px4fmu_v2.4"/>

    <!--module name="stabilization" type="indi_simple"/-->
    <module name="stabilization" type="float_euler"/>

    <!--module name="guidance" type="indi">
      <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
    </module-->



    <!--module name="ahrs" type="int_cmpl_quat">
      <define name="AHRS_ICQ_IMU_ID" value="IMU_PX4_ID" />
      <define name="AHRS_ICQ_MAG_ID" value="MAG_HMC58XX_SENDER_ID" />
      <configure name="USE_MAGNETOMETER" value="TRUE"/>
      <define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
    </module-->

    <module name="ahrs" type="float_mlkf">
      <configure name="SECONDARY_AHRS" value="float_mlkf"/>
      <define name="AHRS_MLKF_IMU_ID" value="IMU_PX4_ID"/>
      <comment>
        use external mag
      </comment>
      <define name="AHRS_MLKF_MAG_ID" value="MAG_HMC58XX_SENDER_ID" />
      <configure name="USE_MAGNETOMETER" value="TRUE"/>
    </module>



    <module name="ahrs" type="vectornav">
      <configure name="VN_PORT" value="UART3"/>
      <configure name="VN_BAUD" value="B921600"/>
      <!--configure name="SECONDARY_AHRS" value="vectornav"/-->
    </module>

    <!--module name="ins" type="float_invariant">
      <define name="INS_FINV_IMU_ID" value="IMU_PX4_ID" />
      <define name="INS_FINV_MAG_ID" value="MAG_HMC58XX_SENDER_ID" />
      <define name="INS_FINV_GPS_ID" value="GPS_UBX_ID" />
      <define name="SEND_INVARIANT_FILTER"/>
    </module-->

    <!-- extended INS uses sonar for vertical estimates -->
    <module name="ins" type="extended"/>
    <!-- Horizontal Filter Float  (HFF) INS uses simple Kalman filters to horizontal estimate,
         No sonar/lidar updates though -->
    <!--module name="ins" type="hff"/-->

    <module name="current_sensor">
      <configure name="ADC_CURRENT_SENSOR" value="ADC_3" />
    </module>

    <module name="intermcu" type="uart">
      <configure name="INTERMCU_PORT" value="UART6" />
      <configure name="INTERMCU_BAUD" value="B1500000" /> <!-- This is only during first 10s start up, afterwards it is set to 230400-->
    </module>

    <module name="actuators" type="pwm">
      <define name="SERVO_HZ" value="200" />
    </module>

<!--
    <module name="tlsf"/>
    <module name="pprzlog"/>
    <module name="logger" type="sd_chibios"/>
    <module name="flight_recorder"/>
-->
  </firmware>


  <firmware name="rotorcraft">
    <target name="fbw" board="px4io_2.4" />
    <configure name="PERIODIC_FREQUENCY" value="200"/>
    <module name="motor_mixing" />
    <module name="radio_control" type="ppm">
      <define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="1" />
      <define name="RADIO_KILL_SWITCH" value="RADIO_KILL" />
    </module>
    <module name="actuators" type="pwm">
      <define name="SERVO_HZ" value="200" />
    </module>
    <define name="RC_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE" />
    <!-- Switch to Failsafe or to Autopilot on RC loss? -->
    <define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_AUTO" />
    <define name="AP_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE" />
    <!-- Switch to Failsafe or to Manual on AP loss? -->
    <define name="INTERMCU_LOST_CNT" value="100" />
    <module name="intermcu" type="uart">
      <configure name="INTERMCU_PORT" value="UART2" />
      <configure name="INTERMCU_BAUD" value="B1500000" />
    </module>
  </firmware>

  <modules main_freq="PERIODIC_FREQUENCY">
    <!-- allow flashing FBW target through AP -->
    <module name="px4_flash"/>

    <!-- CPU utilization monitor -->
    <module name="sys_mon"/>

    <!-- optical flow camera over i2c-->
    <module name="px4flow_i2c">
      <configure name="USE_PX4FLOW_AGL" value="0" description="don't send AGL measurements"/>
      <comment>compensate AGL for body rotation</comment>
      <configure name="PX4FLOW_COMPENSATE_ROTATION" value="1"/>
      <configure name="PX4FLOW_NOISE_STDDEV" value="10.0"/> <!-- standard deviation of the optical flow measurements-->
    </module>

    <!-- laser range finder for precise AGL measurement -->
    <module name="lidar_lite">
      <configure name="USE_LIDAR_LITE_AGL" value="1"/> <!-- send AGL measurements -->
      <configure name="LIDAR_LITE_COMPENSATE_ROTATION" value="1" /> <!-- compensate AGL for body rotation -->
    </module>

    <!-- laser range finder for precise AGL measurement -->
    <module name="lidar_sf11">
      <configure name="USE_LIDAR_SF11_AGL" value="0"/> <!-- dont send AGL measurements -->
      <configure name="LIDAR_SF11_COMPENSATE_ROTATION" value="1" /> <!-- compensate AGL for body rotation -->
    </module>

    <!-- external mag for better heading estimate -->
    <module name="mag" type="hmc58xx">
      <configure name="MAG_HMC58XX_I2C_DEV" value="i2c1"/>
      <define name="MODULE_HMC58XX_UPDATE_AHRS" value="TRUE"/>
      <define name="HMC58XX_CHAN_X" value="1"/>
      <define name="HMC58XX_CHAN_Y" value="0"/>
      <define name="HMC58XX_CHAN_Z" value="2"/>
      <define name="HMC58XX_CHAN_X_SIGN" value="-"/>
      <define name="HMC58XX_CHAN_Y_SIGN" value="+"/>
      <define name="HMC58XX_CHAN_Z_SIGN" value="+"/>
    </module>

    <module name="air_data"/>
  </modules>

  <section name="MISC">
    <define name="MilliAmpereOfAdc(adc)" value="((float)adc) * (3.3f / 4096.0f) * (90.0f / 5.0f)" />
    <!-- 100Amp = 2Volt -> 2482,42 tick/100Amp"(0.0402832*adc)" -->
  </section>

  <section name="IMU" prefix="IMU_">

    <!-- #3 calibraton -->
    <!--
    <define name="ACCEL_X_NEUTRAL" value="-2"/>
    <define name="ACCEL_Y_NEUTRAL" value="23"/>
    <define name="ACCEL_Z_NEUTRAL" value="21"/>
    <define name="ACCEL_X_SENS" value="7.68290727703" integer="16"/>
    <define name="ACCEL_Y_SENS" value="7.41725548536" integer="16"/>
    <define name="ACCEL_Z_SENS" value="7.25648506436" integer="16"/>
    -->

    <!--#1 calibration -->
    <define name="ACCEL_X_NEUTRAL" value="0"/>
    <define name="ACCEL_Y_NEUTRAL" value="12"/>
    <define name="ACCEL_Z_NEUTRAL" value="23"/>
    <define name="ACCEL_X_SENS" value="7.40346702354" integer="16"/>
    <define name="ACCEL_Y_SENS" value="7.31092326815" integer="16"/>
    <define name="ACCEL_Z_SENS" value="7.60884042632" integer="16"/>

    <!-- #3 internal mag -->
    <!--
    <define name="MAG_X_NEUTRAL" value="-831"/>
    <define name="MAG_Y_NEUTRAL" value="-2458"/>
    <define name="MAG_Z_NEUTRAL" value="1623"/>
    <define name="MAG_X_SENS" value="0.383688492241" integer="16"/>
    <define name="MAG_Y_SENS" value="0.363160500566" integer="16"/>
    <define name="MAG_Z_SENS" value="0.398814954916" integer="16"/>
    -->

    <!-- #3 external mag -->
    <!--
    <define name="MAG_X_NEUTRAL" value="100"/>
    <define name="MAG_Y_NEUTRAL" value="-11"/>
    <define name="MAG_Z_NEUTRAL" value="-84"/>
    <define name="MAG_X_SENS" value="3.95372806212" integer="16"/>
    <define name="MAG_Y_SENS" value="3.85554408345" integer="16"/>
    <define name="MAG_Z_SENS" value="4.43807458722" integer="16"/>
    -->

    <!-- #1 external mag -->
    <define name="MAG_X_NEUTRAL" value="90"/>
    <define name="MAG_Y_NEUTRAL" value="-12"/>
    <define name="MAG_Z_NEUTRAL" value="3"/>
    <define name="MAG_X_SENS" value="3.9581923374" integer="16"/>
    <define name="MAG_Y_SENS" value="4.06927263588" integer="16"/>
    <define name="MAG_Z_SENS" value="4.43592657252" integer="16"/>


    <!-- #1 internal mag -->
    <!--
    <define name="MAG_X_NEUTRAL" value="-770"/>
    <define name="MAG_Y_NEUTRAL" value="2338"/>
    <define name="MAG_Z_NEUTRAL" value="-4760"/>
    <define name="MAG_X_SENS" value="0.37262897642" integer="16"/>
    <define name="MAG_Y_SENS" value="0.367752873196" integer="16"/>
    <define name="MAG_Z_SENS" value="0.388678431908" 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>

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

  <rc_commands>
    <set command="THRUST" value="@THROTTLE" />
    <set command="ROLL" value="@ROLL" />
    <set command="PITCH" value="@PITCH" />
    <set command="YAW" value="@YAW" />
  </rc_commands>

  <servos driver="Default">
    <servo name="TOP_LEFT" no="2" min="1000" neutral="1100" max="2000" />
    <servo name="TOP_RIGHT" no="0" min="1000" neutral="1100" max="2000" />
    <servo name="BOTTOM_RIGHT" no="3" min="1000" neutral="1100" max="2000" />
    <servo name="BOTTOM_LEFT" no="1" min="1000" neutral="1100" max="2000" />
  </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="REVERSE" value="FALSE" />
    <define name="TYPE" value="QUAD_X" />
  </section>

  <command_laws>
    <call fun="motor_mixing_run(fbw_motors_on,FALSE,values)" />
    <set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]" />
    <set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]" />
    <set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]" />
    <set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]" />
  </command_laws>

  <!-- local magnetic field -->
  <!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
  <section name="AHRS" prefix="AHRS_">
    <!-- For vibrating airfames -->
    <define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
  </section>

  <section name="INS" prefix="INS_">
    <define name="SONAR_MAX_RANGE" value="5.0" />
    <define name="SONAR_UPDATE_ON_AGL" value="TRUE" />
    <!-- Portland 
    Normalize[{19443.3, 5354.0,48519.5}] = (0.37004, 0.101896, 0.923411)-->
    <define name="H_X" value="0.37004"/>
    <define name="H_Y" value="0.101896"/>
    <define name="H_Z" value="0.923411"/>
    <!-- trust more the baro over the gps alt -->
    <define name="INV_NXZ" value="0.3"/>
    <define name="INV_NH" value="2.0"/>
  </section>

  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
    <!-- setpoints -->
    <define name="SP_MAX_PHI"     value="35." unit="deg"/>
    <define name="SP_MAX_THETA"   value="35." unit="deg"/>
    <define name="SP_MAX_R"       value="40." unit="deg/s"/>
    <define name="DEADBAND_A"     value="5"/>
    <define name="DEADBAND_E"     value="5"/>
    <define name="DEADBAND_R"     value="10"/>

    <!-- inetgrator limits -->
    <define name="MAX_SUM_ERR"   value="50"/>

    <!-- 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.)"/>

    <!-- Big fat ARK Gains -->
    <define name="PHI_PGAIN"  value="2700"/>
    <define name="PHI_DGAIN"  value="700"/>
    <define name="PHI_IGAIN"  value="5.0"/>

    <define name="THETA_PGAIN"  value="2700"/>
    <define name="THETA_DGAIN"  value="700"/>
    <define name="THETA_IGAIN"  value="5.0"/>

    <define name="PSI_PGAIN" value="4000"/>
    <define name="PSI_DGAIN" value="3500"/>
    <define name="PSI_IGAIN" value="1.5"/>

    <!-- feedforward -->
    <define name="PHI_DDGAIN"   value="5"/>
    <define name="THETA_DDGAIN" value="5"/>
    <define name="PSI_DDGAIN"   value="10"/>
  </section>

  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
    <define name="HOVER_KP" value="70" />
    <define name="HOVER_KD" value="70" />
    <define name="HOVER_KI" value="40" />
    <define name="NOMINAL_HOVER_THROTTLE" value="0.55" />
    <define name="ADAPT_THROTTLE_ENABLED" value="FALSE" />
  </section>

  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
    <!-- Good weather -->
    <define name="MAX_BANK" value="20" unit="deg" />
    <define name="REF_MAX_SPEED" value="2" unit="m/s" />
    <!-- Bad weather -->
    <!-- define name="MAX_BANK" value="32" unit="deg"/ -->
    <define name="PGAIN" value="120" />
    <define name="DGAIN" value="100" />
    <define name="IGAIN" value="30" />
  </section>

  <section name="NAVIGATION" prefix="NAV_">
    <define name="CLIMB_VSPEED" value="4.5" />
    <define name="DESCEND_VSPEED" value="-1.0" />
  </section>

  <section name="SIMULATOR" prefix="NPS_">
    <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]" />
    <define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string" />
    <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string" />
  </section>

  <section name="AUTOPILOT">
    <define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT" />
    <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
    <define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
    <define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_Z_HOLD"/>
    <!--define name="MODE_AUTO2" value="AP_MODE_HOVER_DIRECT"/-->
    <!--define name="MODE_AUTO2" value="AP_MODE_HOVER_Z_HOLD"/-->
    <define name="NO_RC_THRUST_LIMIT" value="FALSE" />
  </section>

  <section name="BAT">
    <define name="MILLIAMP_AT_FULL_THROTTLE" value="8700" />
    <define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V" />
    <define name="CRITIC_BAT_LEVEL" value="10.8" unit="V" />
    <define name="LOW_BAT_LEVEL" value="11.1" unit="V" />
    <define name="MAX_BAT_LEVEL" value="12.4" unit="V" />
  </section>

</airframe>
