#!/bin/sh
#
# @name Advanced Technology Labs (ATL) Mantis EDU
#
# @type Quadrotor x
# @class Copter
#
# @maintainer
# @board px4_fmu-v2 exclude
#

. ${R}etc/init.d/rc.mc_defaults


# Battery settings
param set-default BAT_CRIT_THR 0.20
param set-default BAT_LOW_THR 0.25

param set-default BAT1_CAPACITY 2800.0
param set-default BAT1_N_CELLS 3
param set-default BAT1_R_INTERNAL 0.02
param set-default BAT1_V_CHARGED 4.26
param set-default BAT1_V_EMPTY 3.45

param set-default CBRK_SUPPLY_CHK 894281

param set-default COM_DISARM_LAND 0.1
param set-default COM_DISARM_PRFLT 3
param set-default COM_DL_LOSS_T 10
param set-default COM_FLTMODE1  2
param set-default COM_FLTMODE2 -1
param set-default COM_FLTMODE3 -1
param set-default COM_FLTMODE4 -1
param set-default COM_FLTMODE5 -1
param set-default COM_FLTMODE6  1
param set-default COM_RC_LOSS_T 3


# ekf2
param set-default EKF2_AID_MASK 33
param set-default EKF2_TERR_MASK 1
param set-default EKF2_BARO_DELAY 0
param set-default EKF2_BARO_NOISE 2.0

param set-default EKF2_BCOEF_X 31.5
param set-default EKF2_BCOEF_Y 25.5

param set-default EKF2_GPS_DELAY 100
param set-default EKF2_GPS_POS_X 0.06
param set-default EKF2_GPS_POS_Y 0.0
param set-default EKF2_GPS_POS_Z 0.0
param set-default EKF2_GPS_V_NOISE 0.5

param set-default EKF2_IMU_POS_X 0.06

param set-default EKF2_MAG_DELAY 0
param set-default EKF2_MAG_NOISE 0.1

param set-default EKF2_MIN_RNG 0.15

param set-default EKF2_OF_DELAY 38
param set-default EKF2_OF_GATE 2.0
param set-default EKF2_OF_POS_X -0.035
param set-default EKF2_OF_POS_Y 0.0
param set-default EKF2_OF_POS_Z 0.033
param set-default EKF2_OF_MIN_RNG 0.01
param set-default EKF2_OF_A_HMAX 7.0
param set-default EKF2_OF_QMIN 30

param set-default EKF2_PCOEF_XN -0.3
param set-default EKF2_PCOEF_XP -0.4
param set-default EKF2_PCOEF_YN -0.4
param set-default EKF2_PCOEF_YP -0.4

param set-default EKF2_RNG_A_VMAX 1.0
param set-default EKF2_RNG_CTRL 0
param set-default EKF2_RNG_DELAY 55
param set-default EKF2_RNG_POS_X -0.035
param set-default EKF2_RNG_POS_Y 0.0
param set-default EKF2_RNG_POS_Z 0.033

param set-default EKF2_TERR_NOISE 1.0


# Maximum allowed angle velocity in the landed state
param set-default LNDMC_ROT_MAX 40.0

# Maximum vertical velocity allowed in the landed state
param set-default LNDMC_Z_VEL_MAX 0.7


# filtering
param set-default IMU_DGYRO_CUTOFF 50
param set-default IMU_GYRO_CUTOFF 65


# Pitch angle & rate setting
param set-default MC_PITCHRATE_P 0.075
param set-default MC_PITCHRATE_I 0.1
param set-default MC_PITCHRATE_D 0.0005
param set-default MC_PITCHRATE_MAX 360.0
param set-default MC_PITCH_P 8.0

# Roll angle & rate setting
param set-default MC_ROLLRATE_P 0.055
param set-default MC_ROLLRATE_I 0.1
param set-default MC_ROLLRATE_D 0.0005
param set-default MC_ROLLRATE_MAX 360.0
param set-default MC_ROLL_P 8.0

# Yaw angle & rate setting
param set-default MC_YAWRATE_P 0.1
param set-default MC_YAWRATE_MAX 120.0
param set-default MC_YAW_P 2.5

param set-default MPC_ACC_DOWN_MAX 2.0
param set-default MPC_ACC_HOR 3.0
param set-default MPC_ACC_HOR_MAX 10.0
param set-default MPC_ACC_UP_MAX 3.0
param set-default MPC_ALT_MODE 0
param set-default MPC_LAND_SPEED 0.5
param set-default MPC_LAND_VEL_XY 10
param set-default MPC_MAN_TILT_MAX 20
param set-default MPC_YAWRAUTO_MAX 80.0
param set-default MPC_POS_MODE 4
param set-default MPC_THR_HOVER 0.54
param set-default MPC_THR_MAX 0.9
param set-default MPC_THR_MIN 0.06
param set-default MPC_TILTMAX_AIR 30
param set-default MPC_XY_P 1.0
param set-default MPC_XY_VEL_D 0.005
param set-default MPC_XY_VEL_I 0.02
param set-default MPC_XY_VEL_P 0.15
param set-default MPC_Z_P 2.0
param set-default MPC_Z_VEL_D 0.0
param set-default MPC_Z_VEL_I 0.02
param set-default MPC_Z_VEL_MAX_DN 2.0
param set-default MPC_Z_VEL_P 0.27


# gimbal configuration
param set-default MNT_MODE_IN 0
param set-default MNT_MODE_OUT 1
param set-default MNT_MAN_PITCH 2
param set-default MNT_RC_IN_MODE 1
param set-default MNT_RATE_PITCH 30

# RC
param set-default RC_CHAN_CNT       12

param set-default RC_MAP_THROTTLE   1
param set-default RC_MAP_YAW        2
param set-default RC_MAP_PITCH      3
param set-default RC_MAP_ROLL       4
param set-default RC_MAP_AUX2       5
param set-default RC_MAP_AUX3       10
param set-default RC_MAP_AUX4       8
param set-default RC_MAP_FLTMODE    6
param set-default RC_MAP_RETURN_SW  7

param set-default RC1_TRIM          1000


# optical flow
param set-default SENS_FLOW_MAXR   7.4
param set-default SENS_FLOW_MINHGT 0.15
param set-default SENS_FLOW_MAXHGT 5.0
param set-default SENS_FLOW_ROT 0

# ignore the SD card errors and use normal startup sound
set STARTUP_TUNE "1"

param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR0_PX 0.15
param set-default CA_ROTOR0_PY 0.15
param set-default CA_ROTOR1_PX -0.15
param set-default CA_ROTOR1_PY -0.15
param set-default CA_ROTOR2_PX 0.15
param set-default CA_ROTOR2_PY -0.15
param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.15
param set-default CA_ROTOR3_PY 0.15
param set-default CA_ROTOR3_KM -0.05

param set-default TAP_ESC_FUNC1 101
param set-default TAP_ESC_FUNC2 102
param set-default TAP_ESC_FUNC3 103
param set-default TAP_ESC_FUNC4 104
