px4 rcS脚本分析

rcS是nuttx系统启动后,执行的脚本,主要实现对飞控外设以及核心算法模块的启动。

先贴一张图,这个图已经大致写出了rcS的流程。
这里写图片描述

给出代码分析。有些地方没有分析到,还望指正。

1.rcS文件

#!nsh
#
# PX4FMU startup script.
#
#  NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
#

#
# Start CDC/ACM serial driver
#
sercon                                          #启动USB串口驱动

#
# Default to auto-start mode.
#
set MODE autostart                              #设置参数 MODE TUNE_ERR LOG_FILE    

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

#
# Try to mount the microSD card.
#
# REBOOTWORK this needs to start after the flight control loop
if mount -t vfat /dev/mmcsd0 /fs/microsd       #挂载SD卡
then
    # Start playing the startup tune
    tone_alarm start                           #挂载成功则发出对应声音
else
    tone_alarm MBAGP                           #挂载不成功则发出对应声音,并格式化SD卡
    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

#
# Look for an init script on the microSD card.
# Disable autostart if the script found.
#
set FRC /fs/microsd/etc/rc.txt                  #判断rc.txt文件是否存在,存在则运行该文件,并设置MODE为custom。下面的就不执行了
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 start                                  #启动uorb

    #
    # Load parameters
    #
    set PARAM_FILE /fs/microsd/params
    if mtd start                                #启动mtd文件系统,并设置PARAM_FILE
    then
        set PARAM_FILE /fs/mtd_params
    fi

    param select $PARAM_FILE                    #加载PARAM_FILE文件里的内容。这些参数应该是通过qgc的parameters设置
    if param load                               #加载失败了则 reset参数
    then
    else
        if param reset
        then
        fi
    fi

    #
    # Start system state indicator
    #
    if rgbled start                            #启动led
    then
    else
        if blinkm start
        then
            blinkm systemstate
        fi
    fi

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

    #
    # Set AUTOCNF flag to use it in AUTOSTART scripts
    #
    if param compare SYS_AUTOCONFIG 1       #该参数默认为0.为1的话表示复位除RC以外的参数
    then
        # Wipe out params except RC*
        param reset_nostart RC*
        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 none
    set PWM_DISARMED none
    set PWM_MIN none
    set PWM_MAX none
    set PWM_AUX_OUT none
    set PWM_AUX_RATE none
    set PWM_ACHDIS none
    set PWM_AUX_DISARMED none
    set PWM_AUX_MIN none
    set PWM_AUX_MAX none
    set FAILSAFE_AUX none
    set MK_MODE none
    set FMU_MODE pwm
    set AUX_MODE pwm
    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 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 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
    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
    #
    if param compare SYS_AUTOSTART 0     #SYS_AUTOSTART不是0时,执行rc.autostart
    then
    else
        sh /etc/init.d/rc.autostart      #该文件根据SYS_AUTOSTART进行对应的机型脚本执行,比如vtol则执行sh /etc/init.d/ 13001_caipirinha_vtol,                                  #机型文件里设置一些复位时默认PID参数,以及设置MIXER,PWM_OUT,这两个参数后边会用到
    fi
    unset MODE

    #
    # Wipe incompatible settings for boards not having two outputs
    if ver hwcmp PX4FMU_V4
    then
        set MIXER_AUX none
    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    #判断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
        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
        #
        if [ -f /etc/extras/px4io-v2.bin ]       #px4io-v2.bin是否存在,存在则设置IO_FILE为该文件
        then
            set IO_FILE /etc/extras/px4io-v2.bin
        else
            set IO_FILE /etc/extras/px4io-v1.bin
        fi

        if px4io checkcrc ${IO_FILE}             #对px4io-v2.bin文件进行CRC校验
        then
            echo "[init] PX4IO CRC OK" >> $LOG_FILE      #校验成功

            set IO_PRESENT yes
        else                                        #校验失败
            tone_alarm MLL32CP8MB

            if px4io start                          #如果px4io已经启动
            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}    #更新IO文件
            then
                usleep 500000
                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
        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     设置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

        # Avoid using ttyS0 for MAVLink on FMUv1
        if ver hwcmp PX4FMU_V1
        then
            set FMU_MODE serial
        fi
    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
        if ver hwcmp PX4FMU_V1
        then
            set FMU_MODE serial
        fi
        unset HIL
    else
        unset HIL
        gps start                 #启动gps
    fi

    # waypoint storage
    # REBOOTWORK this needs to start in parallel
    if dataman start
    then
    fi

    #
    # Sensors System (start before Commander so Preflight checks are properly run)
    #
    sh /etc/init.d/rc.sensors    #运行rc.sensors脚本,启动除gps以外的传感器

    # Needs to be this early for in-air-restarts
    if [ $OUTPUT_MODE == hil ]
    then
        commander start -hil
    else
        commander start
    fi

    #
    # Start CPU load monitor
    #
    load_mon start               #启动CPU 监控

    #
    # Start primary output
    #
    set TTYS1_BUSY no            #设置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
    # clear pins 5 and 6
    if param compare SENS_EN_LL40LS 1
    then
        set FMU_MODE pwm4
        set AUX_MODE pwm4
    fi
    if param greater TRIG_MODE 0
    then
        set FMU_MODE pwm4
        set AUX_MODE pwm4
        camera_trigger start
    fi

    # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
    if [ $OUTPUT_MODE != none ]            #如果OUTPUT_MODE为空,则启动该有问题,不应该使能输出。上面已经将OUTPUT_MODE置为 io
    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 ]  #OUTPUT_MODE为 io,则进入该步骤
        then
            if px4io start                                       #启动px4io
            then
                sh /etc/init.d/rc.io                             #执行 rc.io脚本, px4io recovery:允许 px4io从半空启动。
                                                                 #set PX4IO_LIMIT 400:设置 pwm限速为400hz
            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
            then
            else
                echo "FMU start failed" >> $LOG_FILE
                tone_alarm $TUNE_ERR
            fi

            if ver hwcmp PX4FMU_V1
            then
                if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
                then
                    set TTYS1_BUSY yes
                fi
                if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
                then
                    set TTYS1_BUSY yes
                fi
            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
                then
                else
                    echo "FMU mode_$FMU_MODE start failed" >> $LOG_FILE
                    tone_alarm $TUNE_ERR
                fi

                if ver hwcmp PX4FMU_V1
                then
                    if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
                    then
                        set TTYS1_BUSY yes
                    fi
                    if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
                    then
                        set TTYS1_BUSY yes
                    fi
                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"
            # 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/ttyS4"
            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
    #
    if ver hwcmp PX4FMU_V1
    then
    else
        # 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
        fi
        if param compare SYS_COMPANION 57600
        then
            mavlink start -d $MAVLINK_COMPANION_DEVICE -b 57600 -m onboard -r 5000 -x
        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
        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 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 100000 -x
        fi
    fi

    unset MAVLINK_COMPANION_DEVICE

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

    #
    # Optional drivers
    #

    # Sensors on the PWM interface bank
    if param compare SENS_EN_LL40LS 1  #下面一部分目测为雷达,声呐等模块的启动,加入这些模块需要设置对应的param
    then
        if pwm_input start
        then
            if ll40ls start pwm
            then
            fi
        fi
    fi

    # lightware serial lidar sensor
    if param compare SENS_EN_SF0X 0
    then
    else
        sf0x start
    fi

    # lightware i2c lidar sensor
    if param compare SENS_EN_SF1XX 0
    then
    else
        sf1xx start
    fi

    # mb12xx sonar sensor
    if param compare SENS_EN_MB12XX 1
    then
        mb12xx start
    fi

    # teraranger one tof sensor
    if param compare SENS_EN_TRONE 1
    then
        trone start
    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 MINDPX_V2
    then
        px4flow start &
    fi

    # Start MAVLink
    mavlink start -r 800000 -d /dev/ttyACM0 -m config -x     #启动mavlink链接

    #
    # Logging
    #
    if ver hwcmp PX4FMU_V1
    then
        if sdlog2 start -r 30 -a -b 2 -t
        then
        fi
    else
        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 ""
            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 logger start -b 12 -t $LOGGER_ARGS   #启动log采集吧
            then
            fi
            unset LOGGER_ARGS
        fi
    fi

    #
    # Start up ARDrone Motor interface
    #
    if [ $OUTPUT_MODE == ardrone ]
    then
        ardrone_interface start -d /dev/ttyS1
    fi

    #
    # Fixed wing setup           #这部分直接查看vtol
    #
    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 == sk450_deadcat ]
            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 ]          #前面已经设置MIXER 为caipirinha_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         #为尾坐式无人机,则设置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     #启动 rc.interface。后边再看

        # Start standard vtol apps
        sh /etc/init.d/rc.vtol_apps     #启动 rc.vtol_apps,里面有姿态估计,位置估计等核心部分
    fi

    #
    # Rover setup
    #
    if [ $VEHICLE_TYPE == rover ]
    then
        # 10 is MAV_TYPE_GROUND_ROVER
        set MAV_TYPE 10

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

        # Start standard rover apps
        sh /etc/init.d/rc.axialracing_ax10_apps

        param set MAV_TYPE 10
    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
        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

        # Run no SD alarm
        if [ $LOG_FILE == /dev/null ]
        then
            # Play SOS
            tone_alarm error
        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

2.rc.interface文件

#!nsh
#
# Script to configure control interface
#

set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers

if [ $MIXER != none -a $MIXER != skip ]
then
    #
    # Load main mixer
    #

    if [ $MIXER_AUX == none -a $USE_IO == yes ]
    then
        set MIXER_AUX $MIXER.aux                     #设置MIXER_AUX  为 caipirinha_vtol.aux
    fi

    # Use the mixer file from the SD-card if it exists
    if [ -f $SDCARD_MIXERS_PATH/$MIXER.main.mix ]
    then
        set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.main.mix    #设置MIXER_FILE 为 caipirinha_vtol.main.mix
    # Try out the old convention, for backward compatibility
    else

        if [ -f $SDCARD_MIXERS_PATH/$MIXER.mix ]
        then
            set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.mix
        else
            set MIXER_FILE /etc/mixers/$MIXER.main.mix
        fi
    fi

    if [ $OUTPUT_MODE == mkblctrl ]
    then
        set OUTPUT_DEV /dev/mkblctrl0
    else
        set OUTPUT_DEV /dev/pwm_output0
    fi

    if [ $OUTPUT_MODE == uavcan_esc ]
    then
        set OUTPUT_DEV /dev/uavcan/esc
    fi

    if [ $OUTPUT_MODE == tap_esc ]
    then
        set OUTPUT_DEV /dev/tap_esc
    fi

    if mixer load $OUTPUT_DEV $MIXER_FILE   #加载 caipirinha_vtol.main.mix 文件
    then
        echo "INFO  [init] Mixer: $MIXER_FILE on $OUTPUT_DEV"
    else
        echo "ERROR [init] Error loading mixer: $MIXER_FILE"
        echo "ERROR:[init] Could not load mixer: $MIXER_FILE" >> $LOG_FILE
        tone_alarm $TUNE_ERR
    fi

    unset MIXER_FILE
else
    if [ $MIXER != skip ]
    then
        echo "ERROR [init] Mixer not defined"
        echo "ERROR [init] Mixer not defined" >> $LOG_FILE
        tone_alarm $TUNE_ERR
    fi
fi

if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
then
    if [ $PWM_OUT != none ]        # PWM_OUT 在  caipirinha_vtol 设置为1234
    then
        #
        # Set PWM output frequency
        #
        if [ $PWM_RATE != none ]   #下面的这几个参数没有在启动脚本里找到,但是在qgc的Parmameter里面找到了,主要是设置pwm的最大最小速率等。
        then
            pwm rate -c $PWM_OUT -r $PWM_RATE
        fi

        #
        # Set disarmed, min and max PWM values
        #
        if [ $PWM_DISARMED != none ]
        then
            pwm disarmed -c $PWM_OUT -p $PWM_DISARMED
        fi
        if [ $PWM_MIN != none ]
        then
            pwm min -c $PWM_OUT -p $PWM_MIN
        fi
        if [ $PWM_MAX != none ]
        then
            pwm max -c $PWM_OUT -p $PWM_MAX
        fi
    fi

    if [ $FAILSAFE != none ]
    then
        pwm failsafe -d $OUTPUT_DEV $FAILSAFE
    fi
fi

# This is a FMUv2+ thing
if ver hwcmp PX4FMU_V1
then
    set MIXER_AUX none
fi

#MindPX has not aux mixer
if ver hwcmp MINDPX_V2
then
        set MIXER_AUX none
fi

if ver hwcmp CRAZYFLIE
then
    set MIXER_AUX none
fi

if ver hwcmp AEROFC_V1
then
    set MIXER_AUX none
fi

if [ $MIXER_AUX != none -a $AUX_MODE != none ]
then
    #
    # Load aux mixer
    #

    set MIXER_AUX_FILE none
    set OUTPUT_AUX_DEV /dev/pwm_output1

    if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix ]
    then
        set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.aux.mix
    else

        if [ -f /etc/mixers/$MIXER_AUX.aux.mix ]
        then
            set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.aux.mix
        fi
    fi

    if [ $MIXER_AUX_FILE != none ]
    then
        if fmu mode_$AUX_MODE
        then
            # Append aux mixer to main device
            if [ $OUTPUT_MODE == hil ]
            then
                if mixer append $OUTPUT_DEV $MIXER_AUX_FILE
                then
                    echo "INFO  [init] Mixer: $MIXER_AUX_FILE appended to $OUTPUT_DEV"
                else
                    echo "ERROR [init] Error appending mixer: $MIXER_AUX_FILE"
                    echo "ERROR [init] Could not append mixer: $MIXER_AUX_FILE" >> $LOG_FILE
                fi
            fi
            if [ -e $OUTPUT_AUX_DEV -a $OUTPUT_MODE != hil ]
            then
                if mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE
                then
                    echo "INFO  [init] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV"
                else
                    echo "ERROR [init] Error loading mixer: $MIXER_AUX_FILE"
                    echo "ERROR [init] Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE
                fi
            else
                set PWM_AUX_OUT none
                set FAILSAFE_AUX none
            fi
        else
            echo "ERROR: Could not start: fmu mode_pwm" >> $LOG_FILE
            tone_alarm $TUNE_ERR
            set PWM_AUX_OUT none
            set FAILSAFE_AUX none
        fi

        # Set min / max for aux out and rates
        if [ $PWM_AUX_OUT != none ]
        then
            #
            # Set PWM_AUX output frequency
            #
            if [ $PWM_AUX_RATE != none ]
            then
                pwm rate -c $PWM_AUX_OUT -r $PWM_AUX_RATE -d $OUTPUT_AUX_DEV
            fi

            if [ $PWM_AUX_MIN != none ]
            then
                pwm min -c $PWM_AUX_OUT -p $PWM_AUX_MIN -d $OUTPUT_AUX_DEV
            fi
            if [ $PWM_AUX_MAX != none ]
            then
                pwm max -c $PWM_AUX_OUT -p $PWM_AUX_MAX -d $OUTPUT_AUX_DEV
            fi
        fi

        # Set disarmed values for aux out

        # Transitional support until all configs
        # are updated
        if [ $PWM_ACHDIS == none ]
        then
            set PWM_ACHDIS ${PWM_AUX_OUT}
        fi

        #
        # Set disarmed, min and max PWM_AUX values
        #
        if [ $PWM_AUX_DISARMED != none -a $PWM_ACHDIS != none ]
        then
            pwm disarmed -c $PWM_ACHDIS -p $PWM_AUX_DISARMED -d $OUTPUT_AUX_DEV
        fi

        if [ $FAILSAFE_AUX != none ]
        then
            pwm failsafe -d $OUTPUT_AUX_DEV $FAILSAFE
        fi

    fi
fi

unset PWM_OUT
unset PWM_RATE
unset PWM_ACHDIS
unset PWM_MIN
unset PWM_MAX
unset PWM_AUX_OUT
unset PWM_AUX_RATE
unset PWM_AUX_DISARMED
unset PWM_AUX_MIN
unset PWM_AUX_MAX
unset FAILSAFE_AUX
unset FAILSAFE
unset OUTPUT_DEV
unset OUTPUT_AUX_DEV
  • 2
    点赞
  • 8
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值