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

# Figure out what platform we are running on.
PLATFORM=`/usr/bin/voxl-platform 2> /dev/null`
RETURNCODE=$?
if [ $RETURNCODE -ne 0 ]; then
    # If we couldn't get the platform from the voxl-platform utility then check
    # /etc/version to see if there is an M0052 substring in the version string. If so,
    # then we assume that we are on M0052.
    VERSIONSTRING=$(</etc/version)
    M0052SUBSTRING="M0052"
    if [[ "$VERSIONSTRING" == *"$M0052SUBSTRING"* ]]; then
        PLATFORM="M0052"
    fi
fi

# We can only run on M0052, M0054, or M0104 so exit with error if that is not the case
if [ $PLATFORM = "M0052" ]; then
    /bin/echo "Running on M0052"
elif [ $PLATFORM = "M0054" ]; then
    /bin/echo "Running on M0054"
elif [ $PLATFORM = "M0104" ]; then
    /bin/echo "Running on M0104"
else
    /bin/echo "Error, cannot determine platform!"
    exit 0
fi

# Sleep a little here. A lot happens when the uorb and muorb start
# and we need to make sure that it all completes successfully to avoid
# any possible race conditions.
/bin/sleep 1

if [ ! -f /data/px4/param/hitl_parameters ]; then
    echo "[INFO] Setting default parameters for PX4 on voxl"
    . /etc/modalai/voxl-px4-hitl-set-default-parameters.config
    /bin/sync
else
    param select /data/px4/param/hitl_parameters
    param load
fi

/bin/sleep 1

# Start all of the processing modules on DSP
qshell sensors start -h
qshell ekf2 start
qshell mc_pos_control start
qshell mc_att_control start
qshell mc_rate_control start
qshell mc_hover_thrust_estimator start
qshell mc_autotune_attitude_control start
qshell land_detector start multicopter
qshell manual_control start
qshell control_allocator start
qshell rc_update start
qshell commander start -h
qshell commander mode posctl
qshell load_mon start

# This is needed for altitude and position hold modes
qshell flight_mode_manager start

/bin/sleep 1

# Start all of the processing modules on the applications processor
dataman start
navigator start

# Start microdds_client for ros2 offboard messages from agent over localhost
microdds_client start -t udp -h 127.0.0.1 -p 8888

qshell pwm_out_sim start -m hil
# g = gps, m = mag, o = odometry (vio), h = distance sensor, f = optic flow
# qshell dsp_hitl start -g -m -o -h -f
qshell dsp_hitl start -g -m

# start the onboard fast link to connect to voxl-mavlink-server
mavlink start -x -u 14556 -o 14557 -r 100000 -n lo -m onboard

# slow down some of the fastest streams
mavlink stream -u 14556 -s HIGHRES_IMU -r 10
mavlink stream -u 14556 -s ATTITUDE -r 10
mavlink stream -u 14556 -s ATTITUDE_QUATERNION -r 10
mavlink stream -u 14556 -s GLOBAL_POSITION_INT -r 30

# start the slow normal mode for voxl-mavlink-server to forward to GCS
mavlink start -x -u 14558 -o 14559 -r 100000 -n lo

# Start logging and use timestamps for log files when possible.
logger start -t -b 256

/bin/sleep 1

mavlink boot_complete