#!/bin/sh
# PX4 commands need the 'px4-' prefix in bash.
# (px4-alias.sh is expected to be in the PATH)
. px4-alias.sh

uorb start
param select /home/root/parameters
if [ -f /home/root/parameters ]
then
	param load
fi
param set SYS_AUTOSTART 4013
param set MAV_BROADCAST 1
param set MAV_TYPE 2

param set MPC_XY_VEL_P 0.12
param set MPC_XY_P 1.3
param set MPC_XY_VEL_D 0.006
param set MPC_XY_VEL_I 0.05
param set MPC_XY_VEL_MAX 12

param set MPC_Z_VEL_P 0.3
param set MPC_Z_VEL_I 0.1

param set MC_YAW_P 3.0
param set MC_YAWRATE_P 0.05
param set MC_YAWRATE_I 3.0

param set MC_ROLLRATE_P 0.05
param set MC_ROLLRATE_I 0.5
param set MC_ROLLRATE_D 0.0
param set MC_RR_INT_LIM 0.3

param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.8
param set MC_PITCHRATE_D 0.0
param set MC_PR_INT_LIM 0.3

df_ms5607_wrapper start
df_mpu6050_wrapper start -R 8
df_ak8963_wrapper start -R 32
# Rangefinder disabled for now. It was found to cause delays of more than 10ms
#df_bebop_rangefinder_wrapper start
gps start -d /dev/ttyPA1
bebop_flow start
sensors start
commander start
ekf2 start
dataman start
navigator start
land_detector start multicopter
mc_pos_control start
mc_att_control start
sleep 1
mavlink start -x -u 14556 -r 20000
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
df_bebop_bus_wrapper start
mavlink boot_complete
logger start -b 200 -e -t