diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 4f843e9aa1..b797ceebc8 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -52,8 +52,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -61,46 +59,27 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - # # Load mixer and start controllers (depends on px4io) # if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] then - echo "Using FMU_RET mixer from sd card" + echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix else - echo "Using standard FMU_RET mixer" + echo "Using /etc/mixers/FMU_RET.mix" mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix fi -fw_att_control start -fw_pos_control_l1 start + +# +Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index cef86c34d0..920a24e2f6 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -52,8 +52,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -61,39 +59,27 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors # -# Start logging (depends on sensors) +# Load mixer and start controllers (depends on px4io) # -sh /etc/init.d/rc.logging +if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +fi # -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) +Start common fixedwing apps # -mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fw_att_control start -fw_pos_control_l1 start +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 81ea292aae..467b56bbf9 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -59,10 +59,7 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -83,7 +80,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix pwm rate -c 1234 -r 400 # -# Set disarmed, min and max PWM signals +# Set disarmed, min and max PWM signals (for DJI ESCs) # pwm disarmed -c 1234 -p 900 pwm min -c 1234 -p 1200 diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 4dbf76cee7..818f9375e4 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -73,7 +73,7 @@ pwm min -c 1234 -p 1200 pwm max -c 1234 -p 1800 # -# Start common for all multirotors apps +# Start common multirotor apps # sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex index 0f0bb05ced..f83f6cfd05 100644 --- a/ROMFS/px4fmu_common/init.d/12-13_hex +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -61,10 +61,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 900 900 - px4io min 1200 1200 1200 1200 1200 1200 - px4io max 1900 1900 1900 1900 1900 1900 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -77,12 +73,19 @@ fi # # Load mixer # -mixer load /dev/pwm_output $MIXER +mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix # # Set PWM output frequency to 400 Hz # -pwm -u 400 -m 0xff +pwm rate -c 123456 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 123456 -p 900 +pwm min -c 123456 -p 1100 +pwm max -c 123456 -p 1900 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index bd6189a6d3..c79e9d2830 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -31,7 +31,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - + set EXIT_ON_END no # @@ -43,8 +43,6 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) @@ -66,11 +64,11 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix pwm rate -c 1234 -r 400 # -# Set disarmed, min and max PWM signals (for DJI ESCs) +# Set disarmed, min and max PWM signals # pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 +pwm min -c 1234 -p 1100 +pwm max -c 1234 -p 1900 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index d8cc0e9132..6be9178786 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -31,7 +31,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - + set EXIT_ON_END no # @@ -43,8 +43,6 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 191d8cd95f..ffab26c384 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -30,8 +30,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -39,41 +37,29 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors # -# Start logging (depends on sensors) +# Load mixer and start controllers (depends on px4io) # -sh /etc/init.d/rc.logging +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi # -# Start GPS interface (depends on orb) +Start common fixedwing apps # -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fw_att_control start -fw_pos_control_l1 start +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then exit -fi +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 6528337454..14607e2dfc 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -52,8 +52,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -61,41 +59,29 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors # -# Start logging (depends on sensors) +# Load mixer and start controllers (depends on px4io) # -sh /etc/init.d/rc.logging +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi # -# Start GPS interface (depends on orb) +Start common fixedwing apps # -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fw_att_control start -fw_pos_control_l1 start +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then exit -fi +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 index cd7677112d..11071613c4 100644 --- a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 @@ -30,8 +30,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -39,32 +37,10 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) @@ -77,8 +53,11 @@ else echo "Using /etc/mixers/FMU_Q.mix" mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fi -fw_att_control start -fw_pos_control_l1 start + +# +Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index eae37098bd..1ee84b9b01 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -1,6 +1,6 @@ #!nsh -echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs" +echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" # # Load default params for this platform @@ -33,17 +33,27 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 + +set EXIT_ON_END no # -# Start PWM output +# Start and configure PX4IO or FMU interface # -fmu mode_pwm +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi # # Load mixer @@ -55,10 +65,19 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # pwm rate -c 1234 -r 400 +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1100 +pwm max -c 1234 -p 1900 + # # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index a63d0e5f1d..4686382ca6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -84,10 +84,10 @@ then sh /etc/init.d/rc.io else - fmu mode_pwm # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 usleep 5000 + fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc index 0c0cfa53d3..045e41e529 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -60,9 +60,7 @@ then # Start MAVLink (depends on orb) mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - - commander start - + sh /etc/init.d/rc.io else fmu mode_pwm diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing new file mode 100644 index 0000000000..f028510064 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fixedwing @@ -0,0 +1,34 @@ +#!nsh +# +# Standard everything needed for fixedwing except mixer, actuator output and mavlink +# + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index dfff07198a..bc550ac5a1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -1,6 +1,6 @@ #!nsh # -# Standard everything needed for multirotors except mixer, output and mavlink +# Standard everything needed for multirotors except mixer, actuator output and mavlink # # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cff8446a6b..d8b5cb6087 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -105,6 +105,11 @@ then blinkm systemstate fi fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start if param compare SYS_AUTOSTART 1000 then diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ed090271ca..ace13bb784 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -116,6 +116,8 @@ extern struct system_load_s system_load; #define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define MAVLINK_OPEN_INTERVAL 50000 + #define STICK_ON_OFF_LIMIT 0.75f #define STICK_THRUST_RANGE 1.0f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 @@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - if (mavlink_fd < 0) { - /* try again later */ - usleep(20000); - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first."); - } - } - /* Main state machine */ /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); @@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { + if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { + /* try to open the mavlink log device every once in a while */ + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + /* update parameters */ orb_check(param_changed_sub, &updated);