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

echo -e "\n*************************"
echo "AIRFRAME: $AIRFRAME"
echo "GPS: $GPS"
echo "RC: $RC"
echo "ESC: $ESC"
echo "POWER MANAGER: $POWER_MANAGER"
echo "AIRSPEED SENSOR: $AIRSPEED_SENSOR"
echo "DISTANCE SENSOR: $DISTANCE_SENSOR"
echo "OSD: $OSD"
echo "ARTIFACT_MODE: $ARTIFACT_MODE"
echo "EXTRA STEPS:"
for i in "${EXTRA_STEPS[@]}"
do
	echo -e "\t$i"
done
echo -e "*************************\n"

# In order to just exit after starting the uorb / muorb modules define
# the environment variable MINIMAL_PX4. (e.g. export MINIMAL_PX4=1)
# This is useful for testing / debug where you may want to start drivers
# and modules manually from the px4 command shell
if [ ! -z $MINIMAL_PX4 ]; then
    /bin/echo "Running minimal script"
    param select /data/px4/param/parameters
    param load
    exit 0
fi

# 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=""
    if [ -f /etc/version ]; then
        VERSIONSTRING=$(</etc/version)
    fi
    M0052SUBSTRING="M0052"
    if [[ "$VERSIONSTRING" == *"$M0052SUBSTRING"* ]]; then
        PLATFORM="M0052"
    fi
fi

# We can only run on M0054 or M0104 so exit with error if that is not the case
if [ $PLATFORM = "M0052" ]; then
    /bin/echo "Running on M0052"
    /bin/echo "Error M0052 is no longer supported!!!"
    exit 1
elif [ $PLATFORM = "M0054" ]; then
    /bin/echo "Running on M0054"
elif [ $PLATFORM = "M0104" ]; then
    /bin/echo "Running on M0104"
elif [ $PLATFORM = "M0197" ]; then
    /bin/echo "Running on M0197"
else
    /bin/echo "Error, cannot determine platform!"
    exit 1
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

param select /data/px4/param/parameters

# Load in all of the parameters that have been saved in the file
param load

# IMU (accelerometer / gyroscope)
if [ "$PLATFORM" == "M0104" ]; then
    /bin/echo "Starting IMU driver with rotation 12"
    qshell icm42688p start -s -R 12
elif [ "$PLATFORM" == "M0197" ]; then
    /bin/echo "Starting bmi270 IMU driver with no rotation"
    qshell voxl_bmi270 start -s -R 26
else
    /bin/echo "Starting IMU driver with no rotation"
    qshell icm42688p start -s
fi

# First look for any external barometers connected to the apps proc
EXTERNAL_BAROMETER=0

/bin/echo "Looking for external DPS368 barometer"
if dps310 start -X -b /dev/i2c-0; then
	/bin/echo "Detected external DPS368 barometer"
	EXTERNAL_BAROMETER=1
fi

if (( EXTERNAL_BAROMETER == 0 )); then
	/bin/echo "Looking for external ICP10100 barometer"
	if icp101xx start -X -b /dev/i2c-0; then
		/bin/echo "Detected external ICP10100 barometer"
		EXTERNAL_BAROMETER=1
	fi
fi

# If no external barometers found, use the builtin barometer
if (( EXTERNAL_BAROMETER == 0 )); then
    /bin/echo "Did not detect an external barometer, starting onboard barometer"
    if [ "$PLATFORM" == "M0197" ]; then
       /bin/echo "Starting dps368 barometer on M0197"
       qshell dps310 start -I -b 5
    else
        # Start Invensense ICP 101xx barometer built on to VOXL 2
        qshell icp101xx start -I -b 5
    fi
    qshell temperature_compensation start
fi

# Auto detect the magnetometer. If one or both of these devices
# are not connected it will fail but not cause any harm.
# Rotation 10 = ROLL_180_YAW_90
/bin/echo "Looking for qmc5883l magnetometer"
qshell qmc5883l start -R 10 -X -b 1
/bin/echo "Looking for ist8310 magnetometer"
qshell ist8310 start -R 10 -X -b 1
/bin/echo "Looking for ist8308 magnetometer"
# Rotation 12 = PITCH_180
qshell ist8308 start -R 12 -X -b 1
/bin/echo "Looking for iis2mdc magnetometer"
# Note: Rotation may not be correct
qshell iis2mdc start -R 10 -X -b 1

# GPS and magnetometer
if [ "$GPS" != "NONE" ]; then
    if [ "$PLATFORM" == "M0197" ]; then
	gps start -d /dev/ttyHS7
    else
	qshell gps start
    fi
fi

# Auto detect an ncp5623c i2c RGB LED. If one isn't connected this will
# fail but not cause any harm.
/bin/echo "Looking for ncp5623c RGB LED"
qshell rgbled_ncp5623c start -X -b 1 -f 400 -a 56

# We do not change the following parameters but QGC will complain if they aren't
# being reported as "used" by px4. "Touching" them accomplishes that.
param touch SYS_AUTOCONFIG
param touch SDLOG_MODE

# ESC driver
if [ "$ESC" == "VOXL_ESC" ]; then
    /bin/echo "Starting VOXL ESC driver"
    qshell voxl_esc start
elif [ "$ESC" == "VOXL2_IO_PWM_ESC" ]; then
	if [ "$RC" == "M0065_SBUS" ]; then
		/bin/echo "Starting VOXL IO for PWM ESC with SBUS RC"
		qshell voxl2_io start
	else
		/bin/echo "Starting VOXL IO for PWM ESC without SBUS RC"
		qshell voxl2_io start -e
	fi
else
	/bin/echo "No ESC type specified, not starting an ESC driver"
fi


# RC driver
if [ "$RC" == "FAKE_RC_INPUT" ]; then
    /bin/echo "Starting fake RC driver"
    qshell rc_controller start
elif [ "$RC" == "CRSF_RAW" ]; then
    /bin/echo "Starting CRSF RC driver"
    qshell crsf_rc start -d 7
elif [ "$RC" == "CRSF_MAV" ]; then
    /bin/echo "Starting TBS crossfire RC - MAV Mode"
    qshell mavlink_rc_in start -m -p 7 -b 115200
elif [ "$RC" == "SPEKTRUM" ]; then
    /bin/echo "Starting Spektrum RC"
    qshell spektrum_rc start
elif [ "$RC" == "GHST" ]; then
    /bin/echo "Starting GHST RC driver"
    qshell ghst_rc start -d 7
elif [ "$RC" == "M0065_SBUS" ]; then
	if [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then
    	/bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW"
    	qshell dsp_sbus start
		retVal=$?
		if [ $retVal -ne 0 ]; then
	    	/bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed"
	    	/bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW"
			qshell voxl2_io start -d -p 7
		fi
	else
    	/bin/echo "M0065 SBUS RC driver already started with PWM ESC start"
	fi
fi

if [ "$DISTANCE_SENSOR" == "LIGHTWARE_SF000" ]; then
	# Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor
	qshell lightware_laser_serial start -d 7
fi

if [ "$POWER_MANAGER" == "VOXLPM" ]; then
	# APM power monitor
	qshell voxlpm start -X -b 2
elif [ "$POWER_MANAGER" == "INA226" ]; then
	/bin/echo "Starting INA226 power monitor"
	qshell ina226 start -X -b 2
elif [ "$POWER_MANAGER" == "INA228" ]; then
	/bin/echo "Starting INA228 power monitor"
	qshell ina228 start -X -b 2
fi

if [ "$AIRSPEED_SENSOR" == "MS4525DO" ]; then
	qshell ms4525do start -X -b 4
fi

# Optional distance sensor on spare i2c
# qshell vl53l0x start -X -b 4
# qshell vl53l1x start -X -b 4

# Start all of the processing modules on DSP
qshell sensors start
qshell ekf2 start

if [ "$AIRFRAME" == "FIXED_WING" ]; then
	qshell fw_pos_control start
	qshell fw_att_control start
	qshell fw_rate_control start
	qshell airspeed_selector start
	qshell fw_autotune_attitude_control start
	qshell land_detector start fixedwing
elif [ $AIRFRAME = "MULTICOPTER" ]; then
	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
fi

qshell manual_control start
qshell control_allocator start
qshell load_mon start
qshell rc_update start
qshell commander start

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

# Start all of the processing modules on the applications processor
dataman start
navigator start
load_mon start
vehicle_air_data_bridge start
sensor_baro_bridge start
crsf_bridge start
vehicle_local_position_bridge start

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

# 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
mavlink stream -u 14556 -s SCALED_PRESSURE -r 10

# Increase heartbeat rate so VFC can get faster mode updates
mavlink stream -u 14556 -s HEARTBEAT -r 10

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

mavlink boot_complete

# Optional MSP OSD driver for DJI or HDZero goggles
# This is only supported on M0054 (with M0125 accessory board)
if [ "$OSD" == "ENABLE" ] || [ "$OSD" == "DJI" ]; then
    /bin/echo "Starting DJI OSD driver"
    msp_osd start -d /dev/ttyHS1
elif [ "$PLATFORM" == "M0054" ] && [ "$OSD" == "HDZERO" ] || [ "$OSD" == "DJI_MSP_DP" ]; then 
    /bin/echo "Starting $OSD OSD driver for $PLATFORM"
    msp_dp_osd start -d /dev/ttyHS1
elif [ "$PLATFORM" == "M0104" ] && [ "$OSD" == "HDZERO" ] || [ "$OSD" == "DJI_MSP_DP" ] ; then
	/bin/echo "Starting $OSD OSD driver for $PLATFORM"
	msp_dp_osd start -d /dev/ttyHS0
fi

# Start optional EXTRA_STEPS
for i in "${EXTRA_STEPS[@]}"
do
	$i
done

if [ "$ARTIFACT_MODE" == "ENABLE" ]; then
	# Do not start the logger module if we are in artifact mode. Logging
	# produces a log artifact. If there are currently any saved logs make
	# sure to delete them now.
	# Also remove the data manager file that may have saved GPS waypoint information
	# in it.
	# Use shred to remove all files so that they cannot be recovered.
	# shred should be effective on the /data partition since it has this configuration:
	# /dev/sda8 /data ext4 rw,nosuid,nodev,noatime,discard,noauto_da_alloc,data=ordered 0 0
	/usr/bin/find /data/px4/log -name "*.ulg" -exec /usr/bin/shred -u -n 7 {} \;
	/bin/rm -fR /data/px4/log
	/usr/bin/shred -u -n 7 /data/px4/dataman
    /bin/echo "Artifact mode has been enabled"
else
	# Start logging module
	# Add the "-t" option to use timestamps for log files when possible
	# Add the "-e" option to start logging immediately. Default is
	# to log only when armed. Caution must be used with the "-e" option
	# because if power is removed without stopping the logger gracefully then
	# the log file may be corrupted.
	# This is done as the last step because any topics marked as optional will only
	# be logged if they have been advertised when this is started. By starting it last it
	# makes sure to see those advertisements as the other modules are starting before it.
	#
	# Set logger mode based on SDLOG_MODE parameter:
	#   0: log when armed until disarm (default)
	#   1: log from boot until disarm
	#   2: log from boot until shutdown
	#   3: log based on AUX1 RC channel
	#   4: log from first armed until shutdown
	LOGGER_ARGS=""
	if param compare SDLOG_MODE 1
	then
		LOGGER_ARGS="-e"
	fi
	if param compare SDLOG_MODE 2
	then
		LOGGER_ARGS="-f"
	fi
	if param compare SDLOG_MODE 3
	then
		LOGGER_ARGS="-x"
	fi
	if param compare SDLOG_MODE 4
	then
		LOGGER_ARGS="-a"
	fi
	logger start $LOGGER_ARGS
fi
