#!nsh
#
# @name Crazyflie 2.0
#
# @board px4fmu-v2 exclude
# @board px4fmu-v3 exclude
# @board px4fmu-v4 exclude
# @board px4fmu-v4pro exclude
# @board px4fmu-v5 exclude
# @board aerofc-v1 exclude
#
# @type Quadrotor x
# @class Copter
#
# @maintainer Dennis Shtatov <densht@gmail.com>
#
sh /etc/init.d/4001_quad_x
if [ $AUTOCNF == yes ]
then
    param set COM_RC_IN_MODE 1
    param set BAT_N_CELLS 1
    param set BAT_CAPACITY 240
    param set BAT_SOURCE 1
    param set PWM_DISARMED 0
    param set PWM_MIN 0
    param set PWM_MAX 255
    param set SYS_COMPANION 20
    param set MC_PITCHRATE_D 0.002
    param set MC_PITCHRATE_I 0.2
    param set MC_PITCHRATE_P 0.07
    param set MC_PITCH_P 6.5
    param set MC_ROLLRATE_D 0.002
    param set MC_ROLLRATE_I 0.2
    param set MC_ROLLRATE_P 0.07
    param set MC_ROLL_P 6.5
    param set MC_YAW_P 3.0
    param set EKF2_HGT_MODE 2
    param set EKF2_AID_MASK 3
    param set EKF2_OF_DELAY 10
    param set MPC_THR_HOVER 0.7
    param set MPC_MANTHR_MAX 1.0
    param set MPC_THR_MAX 1.0
    param set MPC_Z_P 1.5
    param set MPC_Z_VEL_I 0.3
    param set MPC_Z_VEL_P 0.4
    param set MPC_HOLD_MAX_XY 0.1
    param set MPC_MAX_FLOW_HGT 3
    param set IMU_GYRO_CUTOFF 100
    param set IMU_ACCEL_CUTOFF 30
    param set MC_DTERM_CUTOFF 70
    param set SYS_FMU_TASK 1
    param set CBRK_SUPPLY_CHK 894281
    param set CBRK_USB_CHK 197848
    param set SDLOG_PROFILE 1
    param set EKF2_MAG_TYPE 1
    param set EKF2_ABL_LIM 2.0
    param set MC_AIRMODE 1
    param set NAV_RCL_ACT 3
    param set SENS_FLOW_MINRNG 0.05
fi
set PWM_MIN none
set PWM_MAX none
set PWM_DISARMED none
# Will run the motors at 328.125 kHz (recommended)
set PWM_RATE 3921