PX4飞控学习(三)

启动脚本

#!nsh
# Un comment and use set +e to ignore and set -e to enable 'exit on error control'
set +e
# Un comment the line below to help debug scripts by printing a trace of the script commands
#set -x
# PX4FMU startup script.
#
# NOTE: environment variable references:
#    If the dollar sign ('$') is followed by a left bracket ('{') then the
#    variable name is terminated with the right bracket character ('}').
#    Otherwise, the variable name goes to the end of the argument.
#
#
# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
#
# 串口映射 FMUv2/3/4:
#
# UART1         /dev/ttyS0      IO debug
# USART2        /dev/ttyS1      TELEM1 (flow control)
# USART3        /dev/ttyS2      TELEM2 (flow control)
# UART4
# UART7                         CONSOLE
# UART8                         SERIAL4
#
#
# UART mapping on FMUv5:
#
# UART1         /dev/ttyS0      GPS
# USART2        /dev/ttyS1      TELEM1 (flow control)
# USART3        /dev/ttyS2      TELEM2 (flow control)
# UART4         /dev/ttyS3      ?
# USART6        /dev/ttyS4      TELEM3 (flow control)
# UART7         /dev/ttyS5      ?
# UART8         /dev/ttyS6      CONSOLE

#
# Mount the procfs.
# 挂载procfs
mount -t procfs /proc

#
# Start CDC/ACM serial driver
# 启动usb/串口驱动 (sercon)
if sercon
then
fi

#
# 默认自动启动模式 Default to auto-start mode.
#
set MODE autostart

set TUNE_ERR ML<set LOG_FILE /fs/microsd/bootlog.txt

#
# Try to mount the microSD card.
# 尝试挂载microSD卡
# REBOOTWORK this needs to start after the flight control loop
# tone_alarm 警报声音应用

if mount -t vfat /dev/mmcsd0 /fs/microsd
then
    echo "[i] microSD mounted: /fs/microsd"
    if hardfault_log check
    then
        tone_alarm error
        if hardfault_log commit
        then
            hardfault_log reset
            tone_alarm stop
        fi
  else
    # 启动报警器
    tone_alarm start
 fi
else
    tone_alarm MBAGP
    if mkfatfs /dev/mmcsd0
    then
        if mount -t vfat /dev/mmcsd0 /fs/microsd
        then
            echo "INFO  [init] MicroSD card formatted"
        else
            echo "ERROR [init] Format failed"
            tone_alarm MNBG
            set LOG_FILE /dev/null
        fi
    else
        set LOG_FILE /dev/null
    fi
fi

#
# 查找 microSD 卡上的启动脚本.
# 如果启动脚本存在 停止自动启动.
#
set FRC /fs/microsd/etc/rc.txt
if [ -f $FRC ]
then
    echo "INFO  [init] Executing script: ${FRC}"
    sh $FRC
    set MODE custom
fi
unset FRC

if [ $MODE == autostart ]
then

    #
    # Start the ORB (first app to start)
    # uorb消息总线启动(第一个需要启动的应用)
    uorb start

    #
    # Load parameters
    # 加载参数
    set PARAM_FILE /fs/microsd/params

    # mtd 连接板载e2rom的应用
    if mtd start
    then
        set PARAM_FILE /fs/mtd_params
    fi

#   读取参数
    param select $PARAM_FILE
    if param load
    then
    else
        if param reset
        then
        fi
    fi

    #
    # Start system state indicator
    # 启动系统状态指示器
    # rgbled i2c总线上的红绿蓝灯指示 不存在则blinkm
    if rgbled start
    then
    else
        if blinkm start
        then
            blinkm systemstate
        fi
    fi

    # FMUv5 同时支持 PWM、I2C,RGB LED

    rgbled_pwm start

    # Currently unused, but might be useful down the road
    #if pca8574 start
    #then
    #fi

    #
    # Set AUTOCNF flag to use it in AUTOSTART scripts
    # 设置 AUTOCNF 参数用于 AUTOSTART 脚本

    if param compare SYS_AUTOCONFIG 1
    then
        # Wipe out params except RC* and total flight time
        param reset_nostart RC* LND_FLIGHT_T_*
        set AUTOCNF yes
    else
        set AUTOCNF no

        #
        # Release 1.4.0 transitional support:
        # set to old default if unconfigured.
        # this preserves the previous behaviour
        #
        if param compare BAT_N_CELLS 0
        then
            param set BAT_N_CELLS 3
        fi
    fi

    #
    # Set default values
    # 设置默认参数

    # 模拟模式 否
    set HIL no
    # 飞行器类型 无
    set VEHICLE_TYPE none

    set MIXER none
    set MIXER_AUX none
    set OUTPUT_MODE none
    set PWM_OUT none
    set PWM_RATE p:PWM_RATE
    set PWM_DISARMED p:PWM_DISARMED
    set PWM_MIN p:PWM_MIN
    set PWM_MAX p:PWM_MAX
    set PWM_AUX_OUT none
    set PWM_AUX_RATE none
    set PWM_ACHDIS none
    set PWM_AUX_DISARMED p:PWM_AUX_DISARMED
    set PWM_AUX_MIN p:PWM_AUX_MIN
    set PWM_AUX_MAX p:PWM_AUX_MAX
    set FAILSAFE_AUX none
    set MK_MODE none
    set FMU_MODE pwm
    set AUX_MODE pwm
    set FMU_ARGS ""
    set MAVLINK_F default
    set MAVLINK_COMPANION_DEVICE /dev/ttyS2
    set EXIT_ON_END no
    set MAV_TYPE none
    set FAILSAFE none
    set USE_IO yes
    set LOGGER_BUF 16

    if param compare SYS_FMU_TASK 1
    then
        set FMU_ARGS "-t"
    fi

    if param compare SYS_HITL 1
    then
        # Enable HITL mode
        # 启动模拟模式
        set HIL yes
    fi

    #
    # Set USE_IO flag
    # 
    if param compare SYS_USE_IO 1
    then
        if ver hwcmp PX4FMU_V4
        then
            set USE_IO no
        fi

        if ver hwcmp PX4FMU_V5
        then
            set USE_IO no
            set MAVLINK_COMPANION_DEVICE /dev/ttyS3
        fi

        if ver hwcmp MINDPX_V2
        then
            set USE_IO no
        fi

        if ver hwcmp CRAZYFLIE
        then
            set USE_IO no

            if param compare SYS_AUTOSTART 0
            then
                param set SYS_AUTOSTART 4900
                set AUTOCNF yes
            fi
        fi

        if ver hwcmp AEROFC_V1
        then
            set USE_IO no
        fi

        if ver hwcmp AEROCORE2
        then
            set USE_IO no
        fi
    else
        set USE_IO no
    fi

    if ver hwcmp AEROFC_V1
    then
        if param compare SYS_AUTOSTART 0
        then
            set AUTOCNF yes
        fi

        # We don't allow changing AUTOSTART as it doesn't work in
        # other configurations
        param set SYS_AUTOSTART 4070
    fi

    #
    # Set parameters and env variables for selected AUTOSTART
    # 如果SYS_AUTOSTART参数为0 则ekf2 否则 rc.autostart
    if param compare SYS_AUTOSTART 0
    then
        ekf2 start
    else
        sh /etc/init.d/rc.autostart
    fi
    unset MODE

    #
    # If mount (gimbal) control is enabled and output mode is AUX, set the aux
    # mixer to mount (override the airframe-specific MIXER_AUX setting)
    #
    if param compare MNT_MODE_IN -1
    then
    else
        if param compare MNT_MODE_OUT 0
        then
            set MIXER_AUX mount
        fi
    fi

    #
    # Wipe incompatible settings for boards not having two outputs
    if ver hwcmp PX4FMU_V4
    then
        set MIXER_AUX none
        param set SYS_FMU_TASK 1
    fi

    if ver hwcmp PX4FMU_V5
    then
        set MIXER_AUX none
        param set SYS_FMU_TASK 1
    fi

    if ver hwcmp AEROFC_V1
    then
        set MIXER_AUX none
    fi

    #
    # Override parameters from user configuration file
    #
    set FCONFIG /fs/microsd/etc/config.txt
    if [ -f $FCONFIG ]
    then
        echo "Custom: ${FCONFIG}"
        sh $FCONFIG
    fi
    unset FCONFIG

    #
    # If autoconfig parameter was set, reset it and save parameters
    #
    if [ $AUTOCNF == yes ]
    then
        # Disable safety switch by default on Pixracer
        if ver hwcmp PX4FMU_V4
        then
            param set CBRK_IO_SAFETY 22027
        fi
        param set SYS_AUTOCONFIG 0
    fi
    unset AUTOCNF

    set IO_PRESENT no

    if [ $USE_IO == yes ]
    then
        #
        # Check if PX4IO present and update firmware if needed
        # 检查PX4IO固件是否需要更新
        if [ -f /etc/extras/px4io-v2.bin ]
        then
            set IO_FILE /etc/extras/px4io-v2.bin

            if px4io checkcrc ${IO_FILE}
            then
                echo "[init] PX4IO CRC OK" >> $LOG_FILE

                set IO_PRESENT yes
            else
                tone_alarm MLL32CP8MB

                if px4io start
                then
                    # try to safe px4 io so motor outputs dont go crazy
                    if px4io safety_on
                    then
                        # success! no-op
                    else
                        # px4io did not respond to the safety command
                        px4io stop
                    fi
                fi

                if px4io forceupdate 14662 ${IO_FILE}
                then
                    usleep 10000
                    if px4io checkcrc ${IO_FILE}
                    then
                        echo "PX4IO CRC OK after updating" >> $LOG_FILE
                        tone_alarm MLL8CDE

                        set IO_PRESENT yes
                    else
                        echo "PX4IO update failed" >> $LOG_FILE
                        tone_alarm ${TUNE_ERR}
                    fi
                else
                    echo "PX4IO update failed" >> $LOG_FILE
                    tone_alarm ${TUNE_ERR}
                fi
            fi
        fi
        unset IO_FILE

        if [ $IO_PRESENT == no ]
        then
            echo "PX4IO not found" >> $LOG_FILE
            tone_alarm ${TUNE_ERR}
        fi
    fi

    #
    # Set default output if not set
    #
    if [ $OUTPUT_MODE == none ]
    then
        if [ $USE_IO == yes ]
        then
            set OUTPUT_MODE io
        else
            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

    fi

    if [ $OUTPUT_MODE == ardrone ]
    then
        set FMU_MODE gpio_serial
    fi

    if [ $OUTPUT_MODE == tap_esc ]
    then
        set FMU_MODE rcin
    fi

    if [ $HIL == yes ]
    then
        set OUTPUT_MODE hil
    else
    #启动gps
        gps start
    fi

    #dataman地图航迹点管理

    set DATAMAN_OPT ""
    if ver hwcmp AEROFC_V1
    then
        set DATAMAN_OPT -i
    fi
    if ver hwcmp AEROCORE2
    then
        set DATAMAN_OPT "-f /fs/mtd_dataman"
    fi
    # waypoint storage 航迹点存储
    # REBOOTWORK this needs to start in parallel
    if dataman start $DATAMAN_OPT
    then
    fi
    unset DATAMAN_OPT

    #
    # Sensors System (start before Commander so Preflight checks are properly run)
    # 传感器系统(在commander启动前启动以确保检测是否正常运行)
    if [ $HIL == yes ]
    then
        sensors start -h
    else
        sh /etc/init.d/rc.sensors
    fi
    unset HIL

    # Needs to be this early for in-air-restarts
    # commander 中心控制程序
    if [ $OUTPUT_MODE == hil ]
    then
        commander start --hil
    else
        commander start
    fi

    if send_event start
    then
    fi

    #
    # Start CPU load monitor
    #
    load_mon start

    #
    # Start primary output
    #
    set TTYS1_BUSY no

    #
    # Check if UAVCAN is enabled, default to it for ESCs
    #
    if param greater UAVCAN_ENABLE 2
    then
        set OUTPUT_MODE uavcan_esc
    fi

    # Sensors on the PWM interface bank
    if param compare SENS_EN_LL40LS 1
    then
        # clear pins 5 and 6
        set FMU_MODE pwm4
        set AUX_MODE pwm4
    fi
    if param greater TRIG_MODE 0
    then
        # We ONLY support trigger on pins 5 and 6 when simultanously using AUX for actuator output
        if param compare TRIG_PINS 56
        then
            # clear pins 5 and 6
            set FMU_MODE pwm4
            set AUX_MODE pwm4
        else
            set FMU_MODE none
            set AUX_MODE none
        fi
        camera_trigger start

        param set CAM_FBACK_MODE 1
        camera_feedback start
    fi

    # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
    if [ $OUTPUT_MODE != none ]
    then
        if [ $OUTPUT_MODE == uavcan_esc ]
        then
            if param compare UAVCAN_ENABLE 0
            then
                echo "OVERRIDING UAVCAN_ENABLE = 3" >> $LOG_FILE
                param set UAVCAN_ENABLE 3
            fi
        fi

        if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
        then
            if px4io start
            then
                sh /etc/init.d/rc.io
            else
                echo "PX4IO start failed" >> $LOG_FILE
                tone_alarm $TUNE_ERR
            fi
        fi

        if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
        then
            if fmu mode_$FMU_MODE $FMU_ARGS
            then
            else
                echo "FMU start failed" >> $LOG_FILE
                tone_alarm $TUNE_ERR
            fi
        fi

        if [ $OUTPUT_MODE == mkblctrl ]
        then
            set MKBLCTRL_ARG ""
            if [ $MKBLCTRL_MODE == x ]
            then
                set MKBLCTRL_ARG "-mkmode x"
            fi
            if [ $MKBLCTRL_MODE == + ]
            then
                set MKBLCTRL_ARG "-mkmode +"
            fi

            if mkblctrl $MKBLCTRL_ARG
            then
            else
                echo "MK start failed" >> $LOG_FILE
                tone_alarm $TUNE_ERR
            fi
            unset MKBLCTRL_ARG
        fi
        unset MK_MODE

        if [ $OUTPUT_MODE == hil ]
        then
            if pwm_out_sim mode_pwm16
            then
            else
                tone_alarm $TUNE_ERR
            fi
        fi

        #
        # Start IO or FMU for RC PPM input if needed
        #
        if [ $IO_PRESENT == yes ]
        then
            if [ $OUTPUT_MODE != io ]
            then
                if px4io start
                then
                    sh /etc/init.d/rc.io
                else
                    echo "PX4IO start failed" >> $LOG_FILE
                    tone_alarm $TUNE_ERR
                fi
            fi
        else
            if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
            then
                if fmu mode_${FMU_MODE} $FMU_ARGS
                then
                else
                    echo "FMU mode_${FMU_MODE} start failed" >> $LOG_FILE
                    tone_alarm $TUNE_ERR
                fi
            fi
        fi
    fi

    if [ $MAVLINK_F == default ]
    then
        # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
        if [ $TTYS1_BUSY == yes ]
        then
            # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
            set MAVLINK_F "-r 1200 -d /dev/ttyS0"

            # Exit from nsh to free port for mavlink
            set EXIT_ON_END yes
        else
            set MAVLINK_F "-r 1200 -f"
            # Avoid using ttyS1 for MAVLink on FMUv4
            if ver hwcmp PX4FMU_V4
            then
                set MAVLINK_F "-r 1200 -d /dev/ttyS1"
                # Start MAVLink on Wifi (ESP8266 port)
                mavlink start -r 20000 -m config -b 921600 -d /dev/ttyS0
            fi

            if ver hwcmp AEROFC_V1
            then
                set MAVLINK_F "-r 1200 -d /dev/ttyS3"
            fi
        fi

        if ver hwcmp CRAZYFLIE
        then
            # Avoid using either of the two available serials
            set MAVLINK_F none
        fi
    fi

    if [ "x$MAVLINK_F" == xnone ]
    then
    else
        mavlink start ${MAVLINK_F}
    fi
    unset MAVLINK_F

    #
    # MAVLink onboard / TELEM2
    #
    # XXX We need a better way for runtime eval of shell variables,
    # but this works for now
    if param compare SYS_COMPANION 10
    then
        frsky_telemetry start -d ${MAVLINK_COMPANION_DEVICE}
    fi
    if param compare SYS_COMPANION 20
    then
        syslink start
        mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000
    fi
    if param compare SYS_COMPANION 921600
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 921600 -m onboard -r 80000 -x -f
    fi
    if param compare SYS_COMPANION 57600
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m onboard -r 5000 -x -f
    fi
    if param compare SYS_COMPANION 460800
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 460800 -m onboard -r 5000 -x -f
    fi
    if param compare SYS_COMPANION 157600
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m osd -r 1000
    fi
    if param compare SYS_COMPANION 257600
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m magic -r 5000 -x -f
    fi
    if param compare SYS_COMPANION 319200
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 19200 -r 1000
    fi
    if param compare SYS_COMPANION 338400
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 38400 -r 1000
    fi
    if param compare SYS_COMPANION 357600
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -r 1000
    fi
    if param compare SYS_COMPANION 3115200
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 115200 -r 1000
    fi
    if param compare SYS_COMPANION 419200
    then
        iridiumsbd start -d /dev/ttyS2
        mavlink start -d /dev/iridium -b 19200 -m iridium -r 10
    fi
    if param compare SYS_COMPANION 1921600
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 921600 -r 20000
    fi
    if param compare SYS_COMPANION 1500000
    then
        mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 1500000 -m onboard -r 10000 -x -f
    fi

    unset MAVLINK_COMPANION_DEVICE

    #
    # Starting stuff according to UAVCAN_ENABLE value
    #
    if param greater UAVCAN_ENABLE 0
    then
        if uavcan start
        then
            set LOGGER_BUF 6
            uavcan start fw
        else
            tone_alarm ${TUNE_ERR}
        fi
    fi

    if ver hwcmp PX4FMU_V4
    then
        frsky_telemetry start -d /dev/ttyS6
    fi

    if ver hwcmp MINDPX_V2
    then
        frsky_telemetry start -d /dev/ttyS6
    fi

    if ver hwcmp PX4FMU_V2
    then
        # Check for flow sensor - as it is a background task, launch it last
        px4flow start &
    fi

    if ver hwcmp PX4FMU_V4
    then
        # Check for flow sensor - as it is a background task, launch it last
        px4flow start &
    fi

    if ver hwcmp PX4FMU_V4PRO
    then
        # Check for flow sensor - as it is a background task, launch it last
        px4flow start &
    fi

    if ver hwcmp MINDPX_V2
    then
        px4flow start &
    fi

    if ver hwcmp AEROFC_V1
    then
    else
        # Start MAVLink
        mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
    fi

    #
    # Logging
    #
    if param compare SYS_LOGGER 0
    then
        # check if we should increase logging rate for ekf2 replay message logging
        if param greater EKF2_REC_RPL 0
        then
            if sdlog2 start -r 500 -e -b 18 -t
            then
            fi
        else
            if sdlog2 start -r 100 -a -b 9 -t
            then
            fi
        fi
    else
        set LOGGER_ARGS ""
        #
        # Adjust FMUv5 logging settings
        #
        if ver hwcmp PX4FMU_V5
        then
            set LOGGER_BUF 64
            param set SDLOG_MODE 2
        fi
        if param compare SDLOG_MODE 1
        then
            set LOGGER_ARGS "-e"
        fi
        if param compare SDLOG_MODE 2
        then
            set LOGGER_ARGS "-f"
        fi
        if ver hwcmp AEROFC_V1
        then
            set LOGGER_ARGS "-m mavlink"
        fi
        if logger start -b ${LOGGER_BUF} -t ${LOGGER_ARGS}
        then
        fi
        unset LOGGER_BUF
        unset LOGGER_ARGS
    fi

    #
    # Fixed wing setup
    #
    if [ $VEHICLE_TYPE == fw ]
    then
        if [ $MIXER == none ]
        then
            # Set default mixer for fixed wing if not defined
            set MIXER 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.interface

        # Start standard fixedwing apps
        sh /etc/init.d/rc.fw_apps
    fi

    #
    # Multicopters setup
    #
    if [ $VEHICLE_TYPE == mc ]
    then
        if [ $MIXER == none ]
        then
            echo "Mixer undefined"
        fi

        if [ $MAV_TYPE == none ]
        then
            # Use mixer to detect vehicle type
            if [ $MIXER == quad_x -o $MIXER == quad_+ ]
            then
                set MAV_TYPE 2
            fi
            if [ $MIXER == quad_w -o $MIXER == quad_dc ]
            then
                set MAV_TYPE 2
            fi
            if [ $MIXER == quad_h ]
            then
                set MAV_TYPE 2
            fi
            if [ $MIXER == tri_y_yaw- -o $MIXER == tri_y_yaw+ ]
            then
                set MAV_TYPE 15
            fi
            if [ $MIXER == hexa_x -o $MIXER == hexa_+ ]
            then
                set MAV_TYPE 13
            fi
            if [ $MIXER == hexa_cox ]
            then
                set MAV_TYPE 13
            fi
            if [ $MIXER == octo_x -o $MIXER == octo_+ ]
            then
                set MAV_TYPE 14
            fi
            if [ $MIXER == octo_cox -o $MIXER == octo_cox_w ]
            then
                set MAV_TYPE 14
            fi
            if [ $MIXER == coax ]
            then
                set MAV_TYPE 3
            fi
        fi

        # Still no MAV_TYPE found
        if [ $MAV_TYPE == none ]
        then
            echo "Unknown MAV_TYPE"
            param set MAV_TYPE 2
        else
            param set MAV_TYPE ${MAV_TYPE}
        fi

        # Load mixer and configure outputs
        sh /etc/init.d/rc.interface

        # Start standard multicopter apps
        sh /etc/init.d/rc.mc_apps
    fi

    #
    # VTOL setup
    #
    if [ $VEHICLE_TYPE == vtol ]
    then
        if [ $MIXER == none ]
        then
            echo "VTOL mixer undefined"
        fi

        if [ $MAV_TYPE == none ]
        then
            # Use mixer to detect vehicle type
            if [ $MIXER == caipirinha_vtol ]
            then
                set MAV_TYPE 19
            fi
            if [ $MIXER == firefly6 ]
            then
                set MAV_TYPE 21
            fi
            if [ $MIXER == quad_x_pusher_vtol ]
            then
                set MAV_TYPE 22
            fi
        fi

        # Still no MAV_TYPE found
        if [ $MAV_TYPE == none ]
        then
            echo "Unknown MAV_TYPE"
            param set MAV_TYPE 19
        else
            param set MAV_TYPE ${MAV_TYPE}
        fi

        # Load mixer and configure outputs
        sh /etc/init.d/rc.interface

        # Start standard vtol apps
        sh /etc/init.d/rc.vtol_apps
    fi

    #
    # UGV setup
    #
    if [ $VEHICLE_TYPE == ugv ]
    then
        if [ $MIXER == none ]
        then
            # Set default mixer for UGV if not defined
            set MIXER stampede
        fi

        if [ $MAV_TYPE == none ]
        then
            # Use MAV_TYPE = 10 (UGV) if not defined
            set MAV_TYPE 10
        fi

        param set MAV_TYPE ${MAV_TYPE}

        # Load mixer and configure outputs
        sh /etc/init.d/rc.interface

        # Start standard UGV apps
        sh /etc/init.d/rc.gnd_apps
    fi

    #
    # For snapdragon, we need a passthrough mode
    # Do not run any mavlink instances since we need the serial port for
    # communication with Snapdragon.
    #
    if [ $VEHICLE_TYPE == passthrough ]
    then
        mavlink stop-all
        commander stop

        # Stop multicopter attitude controller if it is running, the controls come
        # from Snapdragon.
        if mc_att_control stop
        then
        fi

        # Start snapdragon interface on serial port.
        if ver hwcmp PX4FMU_V2
        then
            # On Pixfalcon use the standard telemetry port (Telem 1).
            snapdragon_rc_pwm start -d /dev/ttyS1
            px4io start
        fi

        if ver hwcmp PX4FMU_V4
        then
            # On Pixracer use Telem 2 port (TL2).
            snapdragon_rc_pwm start -d /dev/ttyS2
            fmu mode_pwm4 $FMU_ARGS
        fi

        pwm failsafe -c 1234 -p 900
        pwm disarmed -c 1234 -p 900

        # Arm straightaway.
        pwm arm
        # Use 400 Hz PWM on all channels.
        pwm rate -a -r 400
    fi

    unset MIXER
    unset MAV_TYPE
    unset OUTPUT_MODE

    #
    # Start the navigator
    #
    navigator start

    #
    # Generic setup (autostart ID not found)
    #
    if [ $VEHICLE_TYPE == none ]
    then
        echo "No autostart ID found"
    fi

    # Start any custom addons
    set FEXTRAS /fs/microsd/etc/extras.txt
    if [ -f $FEXTRAS ]
    then
        echo "Addons script: ${FEXTRAS}"
        sh $FEXTRAS
    fi
    unset FEXTRAS

    if ver hwcmp CRAZYFLIE
    then
        # CF2 shouldn't have an sd card
    else

        if ver hwcmp AEROCORE2
        then
            # AEROCORE2 shouldn't have an sd card
        else

            # Run no SD alarm
            if [ $LOG_FILE == /dev/null ]
            then
                # Play SOS
                tone_alarm error
            fi

        fi

    fi

    #
    # Check if we should start a thermal calibration
    # TODO move further up and don't start unnecessary services if we are calibrating
    #
    set TEMP_CALIB_ARGS ""
    if param compare SYS_CAL_GYRO 1
    then
        set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -g"
        param set SYS_CAL_GYRO 0
    fi
    if param compare SYS_CAL_ACCEL 1
    then
        set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -a"
        param set SYS_CAL_ACCEL 0
    fi
    if param compare SYS_CAL_BARO 1
    then
        set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -b"
        param set SYS_CAL_BARO 0
    fi
    if [ "x$TEMP_CALIB_ARGS" != "x" ]
    then
        send_event temperature_calibration ${TEMP_CALIB_ARGS}
    fi
    unset TEMP_CALIB_ARGS

    # vmount to control mounts such as gimbals, disabled by default.
    if param compare MNT_MODE_IN -1
    then
    else
        if vmount start
        then
        fi
    fi

# End of autostart
fi

# There is no further script processing, so we can free some RAM
# XXX potentially unset all script variables.
unset TUNE_ERR

# Boot is complete, inform MAVLink app(s) that the system is now fully up and running
mavlink boot_complete

if [ $EXIT_ON_END == yes ]
then
    echo "NSH exit"
    exit
fi
unset EXIT_ON_END

PX4飞控学习(三)_第1张图片

你可能感兴趣的:(px4飞控)