#!/bin/sh
#
# @name Crazyflie 2
#
# @type Quadrotor x
# @class Copter
#
# @maintainer Dennis Shtatov <densht@gmail.com>
#
# @board px4_fmu-v2 exclude
# @board px4_fmu-v3 exclude
# @board px4_fmu-v4 exclude
# @board px4_fmu-v4pro exclude
# @board px4_fmu-v5 exclude
# @board px4_fmu-v5x exclude
#
. ${R}etc/init.d/rc.mc_defaults

param set-default BAT1_N_CELLS 1
param set-default BAT1_CAPACITY 240
param set-default BAT1_SOURCE 1

param set-default CBRK_SUPPLY_CHK 894281
param set-default CBRK_USB_CHK 197848
param set-default COM_RC_IN_MODE 1

param set-default EKF2_ABL_LIM 2
param set-default EKF2_AID_MASK 3
param set-default EKF2_HGT_REF 2
param set-default EKF2_RNG_CTRL 2
param set-default EKF2_MAG_TYPE 1
param set-default EKF2_OF_DELAY 10

param set-default IMU_GYRO_CUTOFF 100
param set-default IMU_ACCEL_CUTOFF 30

param set-default MC_AIRMODE 1
param set-default IMU_DGYRO_CUTOFF 70
param set-default MC_PITCHRATE_D 0.002
param set-default MC_PITCHRATE_P 0.07
param set-default MC_ROLLRATE_D 0.002
param set-default MC_ROLLRATE_P 0.07
param set-default MC_YAW_P 3

param set-default MPC_THR_HOVER 0.7
param set-default MPC_THR_MAX 1
param set-default MPC_Z_P 1.5
param set-default MPC_Z_VEL_P_ACC 8
param set-default MPC_Z_VEL_I_ACC 6
param set-default MPC_HOLD_MAX_XY 0.1
param set-default MPC_MAX_FLOW_HGT 3

param set-default NAV_RCL_ACT 3

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

# Run the motors at 328.125 kHz (recommended)
param set-default PWM_MAIN_TIM0 3921
param set-default PWM_MAIN_TIM1 3921

param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
param set-default PWM_MAIN_DIS0 0
param set-default PWM_MAIN_DIS1 0
param set-default PWM_MAIN_DIS2 0
param set-default PWM_MAIN_DIS3 0
param set-default PWM_MAIN_MIN0 0
param set-default PWM_MAIN_MIN1 0
param set-default PWM_MAIN_MIN2 0
param set-default PWM_MAIN_MIN3 0
param set-default PWM_MAIN_MAX0 255
param set-default PWM_MAIN_MAX1 255
param set-default PWM_MAIN_MAX2 255
param set-default PWM_MAIN_MAX3 255


param set-default SENS_FLOW_MINRNG 0.05

syslink start
mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000
