Commit a8d362de authored by Anton Babushkin's avatar Anton Babushkin
Browse files

Autostart: use MIXER instead of FRAME_GEOMETRY

parent d1b21868
......@@ -25,6 +25,7 @@ then
fi
set VEHICLE_TYPE mc
set FRAME_GEOMETRY quad_w
set MIXER FMU_quad_w
set PWM_OUTPUTS 1234
set PWM_RATE 400
......@@ -44,10 +44,10 @@ then
fi
set VEHICLE_TYPE mc
set FRAME_GEOMETRY quad_w
set MIXER FMU_quad_w
set PWM_OUTPUTS 1234
set PWM_RATE 400
set PWM_DISARMED 900
set PWM_MIN 1000
set PWM_MAX 2000
......@@ -43,4 +43,4 @@ fi
set HIL yes
set VEHICLE_TYPE mc
set FRAME_GEOMETRY quad_x
set MIXER FMU_quad_x
......@@ -43,4 +43,4 @@ fi
set HIL yes
set VEHICLE_TYPE mc
set FRAME_GEOMETRY quad_+
set MIXER FMU_quad_+
......@@ -36,4 +36,4 @@ then
fi
set VEHICLE_TYPE fw
set FRAME_GEOMETRY RET
set MIXER FMU_RET
......@@ -41,4 +41,4 @@ then
fi
set VEHICLE_TYPE fw
set FRAME_GEOMETRY Q
set MIXER FMU_Q
......@@ -11,4 +11,4 @@ then
fi
set VEHICLE_TYPE fw
set FRAME_GEOMETRY Q
set MIXER FMU_Q
......@@ -40,4 +40,4 @@ then
fi
set VEHICLE_TYPE fw
set FRAME_GEOMETRY Q
set MIXER FMU_Q
......@@ -40,4 +40,4 @@ then
fi
set VEHICLE_TYPE fw
set FRAME_GEOMETRY Q
set MIXER FMU_FX79
......@@ -41,10 +41,10 @@ then
fi
set VEHICLE_TYPE mc
set FRAME_GEOMETRY quad_x
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
......
......@@ -27,10 +27,10 @@ then
fi
set VEHICLE_TYPE mc
set FRAME_GEOMETRY quad_x
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
# DJI ESC range
set PWM_DISARMED 900
set PWM_MIN 1200
......
......@@ -27,6 +27,7 @@ then
fi
set VEHICLE_TYPE mc
set FRAME_GEOMETRY quad_x
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
#!nsh
#
# Script to configure fixedwing control interface
#
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Load mixer
#
echo "[init] Frame geometry: $FRAME_GEOMETRY"
set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix
#Use the mixer file from the sd-card if it exists
if [ -f $MIXERSD ]
then
set MIXER $MIXERSD
else
set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix
fi
if mixer load /dev/pwm_output $MIXER
then
echo "[init] Mixer loaded: $MIXER"
else
echo "[init] Error loading mixer: $MIXER"
tone_alarm $TUNE_OUT_ERROR
fi
#!nsh
#
# Script to configure control interface
#
if [ $MIXER != none ]
then
#
# Load mixer
#
set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix
#Use the mixer file from the SD-card if it exists
if [ -f $MIXERSD ]
then
set MIXER_FILE $MIXERSD
else
set MIXER_FILE /etc/mixers/$MIXER.mix
fi
if [ $OUTPUT_MODE == mkblctrl ]
then
set OUTPUT_DEV /dev/mkblctrl
else
set OUTPUT_DEV /dev/pwm_output
fi
if mixer load $OUTPUT_DEV $MIXER_FILE
then
echo "[init] Mixer loaded: $MIXER_FILE"
else
echo "[init] Error loading mixer: $MIXER_FILE"
tone_alarm $TUNE_OUT_ERROR
fi
else
echo "[init] Mixer not defined
tone_alarm $TUNE_OUT_ERROR
fi
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
then
if [ $PWM_OUTPUTS != none ]
then
#
# Set PWM output frequency
#
if [ $PWM_RATE != none ]
then
echo "[init] Set PWM rate: $PWM_RATE"
pwm rate -c $PWM_OUTPUTS -r $PWM_RATE
fi
#
# Set disarmed, min and max PWM values
#
if [ $PWM_DISARMED != none ]
then
echo "[init] Set PWM disarmed: $PWM_DISARMED"
pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED
fi
if [ $PWM_MIN != none ]
then
echo "[init] Set PWM min: $PWM_MIN"
pwm min -c $PWM_OUTPUTS -p $PWM_MIN
fi
if [ $PWM_MAX != none ]
then
echo "[init] Set PWM max: $PWM_MAX"
pwm max -c $PWM_OUTPUTS -p $PWM_MAX
fi
fi
fi
......@@ -9,11 +9,13 @@
px4io recovery
#
# Adjust px4io topic limiting
# Adjust PX4IO update rate limit
#
set PX4IO_LIMIT 400
if hw_ver compare PX4FMU_V1
then
px4io limit 200
else
px4io limit 400
set PX4IO_LIMIT 200
fi
echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
px4io limit $PX4IO_LIMIT
#!nsh
#
# Script to configure multicopter control interface
#
if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ]
then
set OUTPUTS 1234
param set MAV_TYPE 2
fi
if [ $FRAME_GEOMETRY == quad_w -o $FRAME_GEOMETRY == quad_v ]
then
set OUTPUTS 1234
param set MAV_TYPE 2
fi
if [ $FRAME_GEOMETRY == hex_x -o $FRAME_GEOMETRY == hex_+ ]
then
set OUTPUTS 123456
param set MAV_TYPE 13
fi
if [ $FRAME_GEOMETRY == octo_x -o $FRAME_GEOMETRY == octo_+ ]
then
set OUTPUTS 12345678
param set MAV_TYPE 14
fi
#
# Load mixer
#
echo "[init] Frame geometry: $FRAME_GEOMETRY"
set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix
if [ $OUTPUT_MODE == mkblctrl ]
then
set OUTPUT_DEV /dev/mkblctrl
else
set OUTPUT_DEV /dev/pwm_output
fi
if mixer load $OUTPUT_DEV $MIXER
then
echo "[init] Mixer loaded: $MIXER"
else
echo "[init] Error loading mixer: $MIXER"
tone_alarm $TUNE_OUT_ERROR
fi
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
then
#
# Set PWM output frequency
#
if [ $PWM_RATE != none ]
then
echo "[init] Set PWM rate: $PWM_RATE"
pwm rate -c $OUTPUTS -r $PWM_RATE
fi
#
# Set disarmed, min and max PWM values
#
if [ $PWM_DISARMED != none ]
then
echo "[init] Set PWM disarmed: $PWM_DISARMED"
pwm disarmed -c $OUTPUTS -p $PWM_DISARMED
fi
if [ $PWM_MIN != none ]
then
echo "[init] Set PWM min: $PWM_MIN"
pwm min -c $OUTPUTS -p $PWM_MIN
fi
if [ $PWM_MAX != none ]
then
echo "[init] Set PWM max: $PWM_MAX"
pwm max -c $OUTPUTS -p $PWM_MAX
fi
fi
......@@ -10,39 +10,42 @@
ms5611 start
adc start
# mag might be external
# Mag might be external
if hmc5883 start
then
echo "using HMC5883"
echo "[init] Using HMC5883"
fi
if mpu6000 start
then
echo "using MPU6000"
echo "[init] Using MPU6000"
fi
if l3gd20 start
then
echo "using L3GD20(H)"
echo "[init] Using L3GD20(H)"
fi
if lsm303d start
if hw_ver compare PX4FMU_V2
then
echo "using LSM303D"
if lsm303d start
then
echo "[init] Using LSM303D"
fi
fi
# Start airspeed sensors
if meas_airspeed start
then
echo "using MEAS airspeed sensor"
echo "[init] Using MEAS airspeed sensor"
else
if ets_airspeed start
then
echo "using ETS airspeed sensor (bus 3)"
echo "[init] Using ETS airspeed sensor (bus 3)"
else
if ets_airspeed start -b 1
then
echo "Using ETS airspeed sensor (bus 1)"
echo "[init] Using ETS airspeed sensor (bus 1)"
fi
fi
fi
......
......@@ -14,6 +14,7 @@ set CONFIG_FILE /fs/microsd/etc/config.txt
set EXTRAS_FILE /fs/microsd/etc/extras.txt
set TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4
#
# Try to mount the microSD card.
#
......@@ -73,26 +74,22 @@ then
#
# Load parameters
#
if mtd start
set PARAM_FILE /fs/microsd/params
if hw_ver compare PX4FMU_V2
then
param select /fs/mtd_params
if param load /fs/mtd_params
if mtd start
then
else
echo "FAILED LOADING PARAMS"
set PARAM_FILE /fs/mtd_params
fi
fi
param select $PARAM_FILE
if param load
then
echo "[init] Parameters loaded: $PARAM_FILE"
else
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
if param load /fs/microsd/params
then
echo "Parameters loaded"
else
echo "Parameter file corrupt - ignoring"
fi
fi
fi
echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
fi
#
# Start system state indicator
......@@ -103,6 +100,7 @@ then
else
if blinkm start
then
echo "[init] Using blinkm"
blinkm systemstate
fi
fi
......@@ -112,15 +110,17 @@ then
#
set HIL no
set VEHICLE_TYPE none
set FRAME_GEOMETRY none
set MIXER none
set USE_IO yes
set OUTPUT_MODE none
set PWM_OUTPUTS none
set PWM_RATE none
set PWM_DISARMED none
set PWM_MIN none
set PWM_MAX none
set MKBLCTRL_MODE none
set FMU_MODE pwm
set MAV_TYPE none
#
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
......@@ -183,7 +183,7 @@ then
set IO_PRESENT yes
else
echo "[init] PX4IO CRC failure"
echo "[init] PX4IO CRC failure, trying to update"
echo "PX4IO CRC failure" >> $LOG_FILE
tone_alarm MLL32CP8MB
......@@ -193,17 +193,17 @@ then
usleep 500000
if px4io checkcrc $IO_FILE
then
echo "[init] PX4IO CRC OK after updating"
echo "[init] PX4IO CRC OK, update successful"
echo "PX4IO CRC OK after updating" >> $LOG_FILE
tone_alarm MLL8CDE
set IO_PRESENT yes
else
echo "[init] PX4IO update failed"
echo "[init] ERROR: PX4IO update failed"
echo "PX4IO update failed" >> $LOG_FILE
fi
else
echo "[init] PX4IO update failed"
echo "[init] ERROR: PX4IO update failed"
echo "PX4IO update failed" >> $LOG_FILE
fi
fi
......@@ -220,16 +220,27 @@ then
#
if [ $OUTPUT_MODE == none ]
then
if [ $IO_PRESENT == yes ]
if [ $USE_IO == yes ]
then
# If PX4IO present, use it as primary PWM output by default
set OUTPUT_MODE io
else
# Else use PX4FMU PWM output
set OUTPUT_MODE fmu
fi
fi
if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ]
then
# Need IO for output but it not present, disable output
set OUTPUT_MODE none
echo "[init] ERROR: PX4IO not found, disabling output"
# Avoid using ttyS0 for MAVLink on FMUv1
if hw_ver compare PX4FMU_V1
then
set FMU_MODE serial
fi
fi
if [ $HIL == yes ]
then
set OUTPUT_MODE hil
......@@ -256,7 +267,7 @@ then
echo "[init] PX4IO started"
sh /etc/init.d/rc.io
else
echo "[init] PX4IO start error"
echo "[init] ERROR: PX4IO start failed"
tone_alarm $TUNE_OUT_ERROR
fi
fi
......@@ -267,7 +278,7 @@ then
then
echo "[init] FMU mode_$FMU_MODE started"
else
echo "[init] FMU mode_$FMU_MODE start error"
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_OUT_ERROR
fi
......@@ -300,7 +311,7 @@ then
then
echo "[init] MKBLCTRL started"
else
echo "[init] MKBLCTRL start error"
echo "[init] ERROR: MKBLCTRL start failed"
tone_alarm $TUNE_OUT_ERROR
fi
......@@ -312,7 +323,7 @@ then
then
echo "[init] HIL output started"
else
echo "[init] HIL output error"
echo "[init] ERROR: HIL output start failed"
tone_alarm $TUNE_OUT_ERROR
fi
fi
......@@ -329,7 +340,7 @@ then
echo "[init] PX4IO started"
sh /etc/init.d/rc.io
else
echo "[init] PX4IO start error"
echo "[init] ERROR: PX4IO start failed"
tone_alarm $TUNE_OUT_ERROR
fi
fi
......@@ -340,13 +351,17 @@ then
then
echo "[init] FMU mode_$FMU_MODE started"
else
echo "[init] FMU mode_$FMU_MODE start error"
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_OUT_ERROR
fi
if hw_ver compare PX4FMU_V1
then
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio -o $FMU_MODE == pwm_gpio ]
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
then
set TTYS1_BUSY yes
fi
if [ $FMU_MODE == pwm_gpio ]
then
set TTYS1_BUSY yes
fi
......@@ -401,14 +416,22 @@ then
then
echo "[init] Vehicle type: FIXED WING"
if [ $FRAME_GEOMETRY == none ]
if [ $MIXER == none ]
then
# Set default frame geometry for fixed wing
set FRAME_GEOMETRY AERT
# Set default mixer for fixed wing if not defined
set MIXER FMU_AERT
fi
if [ $MAV_TYPE == none ]
then
# Use MAV_TYPE = 1 (fixed wing) if not defined
set MAV_TYPE 1
fi
param set MAV_TYPE $MAV_TYPE
# Load mixer and configure outputs
sh /etc/init.d/rc.fw_interface
sh /etc/init.d/rc.interface
# Start standard fixedwing apps
sh /etc/init.d/rc.fw_apps
......@@ -420,15 +443,37 @@ then
if [ $VEHICLE_TYPE == mc ]
then
echo "[init] Vehicle type: MULTICOPTER"
if [ $FRAME_GEOMETRY == none ]
if [ $MIXER == none ]
then
# Set default frame geometry for multicopter
set FRAME_GEOMETRY quad_x
# Set default mixer for multicopter if not defined
set MIXER quad_x
fi
if [ $MAV_TYPE == none ]
then
# Use MAV_TYPE = 2 (quadcopter) if not defined
set MAV_TYPE 2
# Use mixer to detect vehicle type
if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ]
then
param set MAV_TYPE 13
fi
if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ]
then
param set MAV_TYPE 14
fi
if [ $MIXER == FMU_octo_cox ]
then
param set MAV_TYPE 14
fi
fi
param set MAV_TYPE $MAV_TYPE
# Load mixer and configure outputs
sh /etc/init.d/rc.mc_interface
sh /etc/init.d/rc.interface
# Start standard multicopter apps
sh /etc/init.d/rc.mc_apps
......@@ -440,9 +485,9 @@ then
if [ $VEHICLE_TYPE == none ]
then
echo "[init] Vehicle type: GENERIC"
attitude_estimator_ekf start
position_estimator_inav start
# Load mixer and configure outputs
sh /etc/init.d/rc.interface
fi
# Start any custom addons
......