<!-- this is a quadrocopter frame equiped with Lisa/S 1.0 and generic china pwm motor controllers -->

<!--
     The configuration of the quadrotor looking from the top is the following:

           Front
             ^
             |

          Motor5
            CW
             |
             |
           ,___,
    Motor4 |   |  Motor1
     CCW===|   |===CCW
           |___|
             |
             |
            CW
          Motor0
-->

<!--
     Applicable configuration:
     airframe="airframes/examples/quadrotor_lisa_s.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"
-->

<airframe name="quadrotor_lisa_s">

  <servos driver="Pwm">
    <servo name="LEFT" no="4" min="1000" neutral="1100" max="1900"/>
    <servo name="FRONT" no="5" min="1000" neutral="1100" max="1900"/>
    <servo name="RIGHT" no="1" min="1000" neutral="1100" max="1900"/>
    <servo name="BACK" no="0" min="1000" neutral="1100" max="1900"/>
  </servos>

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

  <command_laws>
    <call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
    <set servo="FRONT" value="motor_mixing.commands[0]"/>
    <set servo="BACK" value="motor_mixing.commands[1]"/>
    <set servo="LEFT" value="motor_mixing.commands[2]"/>
    <set servo="RIGHT" value="motor_mixing.commands[3]"/>
  </command_laws>

  <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="4"/>
    <define name="SCALE" value="256"/>
    <define name="ROLL_COEF" value="{     0,     0,  256, -256 }"/>
    <define name="PITCH_COEF" value="{  256,  -256,    0,    0 }"/>
    <define name="YAW_COEF" value="{   -256,  -256,  256,  256 }"/>
    <define name="THRUST_COEF" value="{ 256,   256,  256,  256 }"/>
  </section>

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

    <define name="ACCEL_X_NEUTRAL" value="0"/>
    <define name="ACCEL_Y_NEUTRAL" value="0"/>
    <define name="ACCEL_Z_NEUTRAL" value="0"/>

    <!-- MAGNETO CALIBRATION DELFT -->
    <define name="MAG_X_NEUTRAL" value="286"/>
    <define name="MAG_Y_NEUTRAL" value="-72"/>
    <define name="MAG_Z_NEUTRAL" value="97"/>
    <define name="MAG_X_SENS" value="3.94431833863" integer="16"/>
    <define name="MAG_Y_SENS" value="4.14629702271" integer="16"/>
    <define name="MAG_Z_SENS" value="4.54518768636" integer="16"/>

    <!-- MAGNETO CURRENT CALIBRATION -->
    <define name="MAG_X_CURRENT_COEF" value="0.0103422023767"/>
    <define name="MAG_Y_CURRENT_COEF" value="0.0084568317783"/>
    <define name="MAG_Z_CURRENT_COEF" value="-0.01935617335"/>
  </section>

 <section name="AUTOPILOT">
   <define name="MODE_STARTUP" value="AP_MODE_NAV"/>
   <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_NAV"/>
 </section>

 <section name="BAT">
   <define name="MILLIAMP_AT_FULL_THROTTLE" value="14000"/>
   <define name="CATASTROPHIC_BAT_LEVEL" value="4.0" unit="V"/>
   <define name="CRITIC_BAT_LEVEL" value="4.4" unit="V"/>
    <define name="LOW_BAT_LEVEL" value="4.5" unit="V"/>
    <define name="MAX_BAT_LEVEL" value="4.8" unit="V"/>
 </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="90." unit="deg/s"/>
    <define name="DEADBAND_R" value="250"/>
    <define name="DEADBAND_A" value="250"/>

    <!-- reference -->
    <define name="REF_OMEGA_P" value="750" 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(8000.)"/>

    <define name="REF_OMEGA_Q" value="750" 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(8000.)"/>

    <define name="REF_OMEGA_R" value="250" 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(1000.)"/>

    <!-- feedback -->
    <define name="PHI_PGAIN" value="399"/>
    <define name="PHI_DGAIN" value="183"/>
    <define name="PHI_IGAIN" value="474"/>

    <define name="THETA_PGAIN" value="399"/>
    <define name="THETA_DGAIN" value="162"/>
    <define name="THETA_IGAIN" value="474"/>

    <define name="PSI_PGAIN" value="500"/>
    <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="INS" prefix="INS_">
  </section>

 <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
   <define name="HOVER_KP" value="276"/>
   <define name="HOVER_KD" value="455"/>
   <define name="HOVER_KI" value="100"/>
   <define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value="0.9"/>
  </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"/>
  </section>

 <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
   <define name="PGAIN" value="39"/>
   <define name="DGAIN" value="50"/>
   <define name="IGAIN" value="19"/>
 </section>

 <section name="SIMULATOR" prefix="NPS_">
   <define name="ACTUATOR_NAMES" value="front_motor, back_motor, left_motor, right_motor" type="string[]"/>
   <define name="JSBSIM_MODEL" value="simple_quad" type="string"/>
   <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
 </section>

 <modules main_freq="512">
   <module name="gps" type="ubx_ucenter"/>
   <module name="send_imu_mag_current"/>
 </modules>

  <firmware name="rotorcraft">
    <target name="ap" board="lisa_s_1.0">
      <module name="radio_control" type="superbitrf_rc">
	<!-- To store the binding parameters for the superbit radio in your
             airframe file uncomment the following three lines and set the
             correct values based on the output of the superbitrf telemetry
             messages. -->
        <!--define name="RADIO_TRANSMITTER_ID" value="1335259868"/--> <!-- Esden (1BitSquared) Dx6i: TX 1 -->
        <!--define name="RADIO_TRANSMITTER_CHAN" value="6"/-->
        <!--define name="RADIO_TRANSMITTER_PROTOCOL" value="0x01"/-->
        <define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
      </module>

      <configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
    </target>

    <target name="nps" board="pc">
      <module name="fdm" type="jsbsim"/>
      <module name="radio_control" type="ppm"/>
    </target>

    <module name="motor_mixing"/>
    <module name="actuators" type="pwm">
      <define name="SERVO_HZ" value="400"/>
      <define name="USE_SERVOS_1AND2"/>
    </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="ahrs" type="int_cmpl_quat"/>
    <module name="ins"/>
  </firmware>
</airframe>
