diff --git a/ROMFS/px4fmu_common/init.d-posix/10016_iris b/ROMFS/px4fmu_common/init.d-posix/10016_iris index c0a8911cab..12263a526a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/10016_iris +++ b/ROMFS/px4fmu_common/init.d-posix/10016_iris @@ -7,5 +7,7 @@ # @maintainer Julian Oes # -pwm_out_sim mode_pwm -mixer load /dev/pwm_output0 etc/mixers/quad_w.main.mix +sh /etc/init.d/rc.mc_defaults + +set MIXER quad_w + diff --git a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 index 3a09ad079a..eff9c0a653 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 @@ -5,6 +5,8 @@ # @type Hexarotor x # +sh /etc/init.d/rc.mc_defaults + if [ $AUTOCNF == yes ] then param set MAV_TYPE 13 @@ -23,10 +25,5 @@ then param set MAV_PROTO_VER 2 fi -mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 +set MIXER hexa_x -pwm_out_sim mode_pwm16 -mixer load /dev/pwm_output0 etc/mixers/hexa_x.main.mix -mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix -vmount start -camera_trigger start diff --git a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post new file mode 100644 index 0000000000..5c133dc83f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post @@ -0,0 +1,8 @@ + +mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix + +mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 + +mavlink stream -r 10 -s MOUNT_ORIENTATION -u 14556 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14557 +mavlink stream -r 10 -s MOUNT_ORIENTATION -u 14557 diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index e83da1da69..c51a9a8ba9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -14,7 +14,20 @@ then exit 0 fi -RUN_MINIMAL_SHELL=no +# initialize script variables +set AUX_MODE none +set IO_PRESENT no +set LOG_FILE bootlog.txt +set MAV_TYPE none +set MIXER none +set MIXER_AUX none +set OUTPUT_MODE sim +set PWM_OUT none +set SDCARD_MIXERS_PATH etc/mixers +set USE_IO no +set VEHICLE_TYPE none + +set RUN_MINIMAL_SHELL no # Use the variable set by sitl_run.sh to choose the model settings. if [ "$PX4_SIM_MODEL" == "shell" ]; then @@ -28,6 +41,10 @@ else exit -1 fi +# clear bootlog +[ -f $LOG_FILE ] && rm $LOG_FILE + + uorb start if [ -f eeprom/parameters ] then @@ -73,16 +90,19 @@ then param set CAL_GYRO0_XOFF 0.01 param set CAL_MAG0_ID 196616 param set CAL_MAG0_XOFF 0.01 + param set COM_DISARM_LAND 3 param set COM_OBL_ACT 2 param set COM_OBL_RC_ACT 0 param set COM_OF_LOSS_T 5 param set COM_RC_IN_MODE 1 + param set EKF2_AID_MASK 1 param set EKF2_ANGERR_INIT 0.01 param set EKF2_GBIAS_INIT 0.01 param set EKF2_HGT_MODE 0 param set EKF2_MAG_TYPE 1 + param set MC_PITCH_P 6 param set MC_PITCHRATE_P 0.2 param set MC_ROLL_P 6 @@ -96,6 +116,7 @@ then param set RTL_DESCEND_ALT 5.0 param set RTL_LAND_DELAY 5 param set RTL_RETURN_ALT 30.0 + param set SDLOG_DIRS_MAX 7 param set SENS_BOARD_ROT 0 param set SENS_BOARD_X_OFF 0.000001 @@ -114,7 +135,6 @@ fi sh "$autostart_file" - dataman start replay tryapplyparams simulator start -s @@ -125,27 +145,27 @@ barosim start gpssim start sensors start commander start -land_detector start multicopter navigator start -if param compare SYS_MC_EST_GROUP 0 -then - attitude_estimator_q start - position_estimator_inav start -elif param compare SYS_MC_EST_GROUP 1 + +if ! param compare MNT_MODE_IN -1 then - attitude_estimator_q start - local_position_estimator start -elif param compare SYS_MC_EST_GROUP 2 + vmount start +fi + +if param greater TRIG_MODE 0 then - ekf2 start -else - echo "No estimator chosen" - exit -1 + camera_trigger start + param set CAM_FBACK_MODE 1 + camera_feedback start fi -mc_pos_control start -mc_att_control start +# Configure vehicle type specific parameters. +# Note: rc.vehicle_setup is the entry point for rc.interface, +# rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps. +# +sh etc/init.d/rc.vehicle_setup + mavlink start -x -u 14556 -r 4000000 mavlink start -x -u 14557 -r 4000000 -m onboard -o 14540 @@ -160,10 +180,9 @@ mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -# Required for the typhoon -mavlink stream -r 10 -s MOUNT_ORIENTATION -u 14556 -mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14557 -mavlink stream -r 10 -s MOUNT_ORIENTATION -u 14557 +# execute autostart post script if any +[ -e "$autostart_file".post ] && sh "$autostart_file".post + logger start -e -t -b 1000 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 73975eb380..a3d2f528a9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -11,7 +11,6 @@ set MIXER_AUX_FILE none set OUTPUT_AUX_DEV /dev/pwm_output1 -set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers # # If mount (gimbal) control is enabled and output mode is AUX, set the aux @@ -25,7 +24,7 @@ then fi fi -if ver hwcmp AEROFC_V1 AV_X_V1 CRAZYFLIE MINDPX_V2 NXPHLITE_V3 PX4FMU_V4 OMNIBUS_F4SD +if ver hwcmp AEROFC_V1 AV_X_V1 CRAZYFLIE MINDPX_V2 NXPHLITE_V3 PX4FMU_V4 OMNIBUS_F4SD SITL then set MIXER_AUX none fi @@ -75,7 +74,7 @@ then fi fi - if [ $OUTPUT_MODE == hil ] + if [ $OUTPUT_MODE == hil -o $OUTPUT_MODE == sim ] then if ! pwm_out_sim start then @@ -313,4 +312,3 @@ fi unset MIXER_AUX_FILE unset OUTPUT_AUX_DEV -unset SDCARD_MIXERS_PATH diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f4d6902086..b86d1e1fb9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -79,6 +79,7 @@ set PWM_MAX p:PWM_MAX set PWM_MIN p:PWM_MIN set PWM_OUT none set PWM_RATE p:PWM_RATE +set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers set USE_IO no set VEHICLE_TYPE none @@ -560,6 +561,7 @@ unset PWM_RATE unset PWM_DISARMED unset PWM_MAX unset PWM_MIN +unset SDCARD_MIXERS_PATH unset USE_IO unset VEHICLE_TYPE