diff --git a/Jenkinsfile b/Jenkinsfile index a46a7e6146b4..69e43d8ea5fe 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -240,7 +240,7 @@ pipeline { } } - stage('ROS vtol mission new 1') { + stage('ROS vtol standard mission new 1') { agent { docker { image 'px4io/px4-dev-ros:2018-03-30' @@ -271,6 +271,37 @@ pipeline { } } + stage('ROS vtol tailsitter mission') { + agent { + docker { + image 'px4io/px4-dev-ros:2018-03-30' + args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' + } + } + steps { + sh 'export' + sh 'make distclean; rm -rf .ros; rm -rf .gazebo' + sh 'git fetch --tags' + sh 'make posix_sitl_default' + sh 'make posix_sitl_default sitl_gazebo' + sh './test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=vtol_new_1 vehicle:=tailsitter' + sh './Tools/ecl_ekf/process_logdata_ekf.py `find . -name *.ulg -print -quit`' + } + post { + always { + sh './Tools/upload_log.py -q --description "${JOB_NAME}: ${STAGE_NAME}" --feedback "${JOB_NAME} ${CHANGE_TITLE} ${CHANGE_URL}" --source CI .ros/rootfs/fs/microsd/log/*/*.ulg' + archiveArtifacts '.ros/**/*.pdf' + archiveArtifacts '.ros/**/*.csv' + sh 'make distclean' + } + failure { + archiveArtifacts '.ros/**/*.ulg' + archiveArtifacts '.ros/**/rosunit-*.xml' + archiveArtifacts '.ros/**/rostest-*.log' + } + } + } + stage('ROS vtol mission new 2') { agent { docker { diff --git a/posix-configs/SITL/init/ekf2/tailsitter b/posix-configs/SITL/init/ekf2/tailsitter index 6c841e6fa49a..f8edcc9ca931 100644 --- a/posix-configs/SITL/init/ekf2/tailsitter +++ b/posix-configs/SITL/init/ekf2/tailsitter @@ -31,15 +31,18 @@ param set MPC_XY_VEL_D 0.005 param set MPC_XY_VEL_P 0.05 param set MPC_Z_VEL_I 0.15 param set MPC_Z_VEL_P 0.8 +param set MPC_LAND_SPEED 1.5 param set NAV_DLL_ACT 2 param set SDLOG_DIRS_MAX 7 param set SENS_BOARD_ROT 0 param set SENS_BOARD_X_OFF 0.000001 param set SENS_DPRES_OFF 0.001 -param set SYS_AUTOSTART 4010 +param set SYS_AUTOSTART 13003 param set SYS_MC_EST_GROUP 2 param set SYS_RESTART_TYPE 2 param set VT_TYPE 0 +param set VT_F_TRANS_DUR 1.5 +param set VT_F_TRANS_THR 0.7 replay tryapplyparams simulator start -s tone_alarm start diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index f765c424fb1c..73fee6f092dd 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -198,8 +198,6 @@ void Tailsitter::update_transition_state() _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f, _pitch_transition_start); - _v_att_sp->thrust = _mc_virtual_att_sp->thrust; - // disable mc yaw control once the plane has picked up speed if (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { _mc_yaw_weight = 0.0f; @@ -222,9 +220,6 @@ void Tailsitter::update_transition_state() time_since_trans_start / _params->back_trans_duration; _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, -2.0f, PITCH_TRANSITION_BACK + 0.2f); - - _v_att_sp->thrust = _mc_virtual_att_sp->thrust; - // keep yaw disabled _mc_yaw_weight = 0.0f; @@ -233,6 +228,13 @@ void Tailsitter::update_transition_state() } + if (_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust = _params->front_trans_throttle; + + } else { + _v_att_sp->thrust = _mc_virtual_att_sp->thrust; + } + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);