#!nsh # # PX4FMU startup script. # # NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. # # UART mapping on FMUv1/2/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. # mount -t procfs /proc # # Start CDC/ACM serial driver # sercon # # Default to auto-start mode. # set MODE autostart set TUNE_ERR ML<> $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 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 # 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 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 # 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 # # 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 # 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 ] 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 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 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 fi fi unset MAVLINK_COMPANION_DEVICE # # Starting stuff according to UAVCAN_ENABLE value # if param greater UAVCAN_ENABLE 0 then if uavcan start then uavcan start fw else tone_alarm $TUNE_ERR fi fi # # Optional drivers # # Sensors on the PWM interface bank if param compare SENS_EN_LL40LS 1 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 # # 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 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 # 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 # # 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