diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 15e5edb133..6dd61c8b01 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1497,7 +1497,6 @@ Mission::generate_waypoint_from_heading(struct position_setpoint_s *setpoint, fl 1000000.0f, &(setpoint->lat), &(setpoint->lon)); - setpoint->alt = _navigator->get_global_position()->alt; setpoint->type = position_setpoint_s::SETPOINT_TYPE_POSITION; setpoint->yaw = yaw; } diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index d987b96311..50995b08a3 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -86,11 +86,11 @@ Standard::parameters_update() /* duration of a forwards transition to fw mode */ param_get(_params_handles_standard.front_trans_dur, &v); - _params_standard.front_trans_dur = math::constrain(v, 0.0f, 5.0f); + _params_standard.front_trans_dur = math::constrain(v, 0.0f, 20.0f); /* duration of a back transition to mc mode */ param_get(_params_handles_standard.back_trans_dur, &v); - _params_standard.back_trans_dur = math::constrain(v, 0.0f, 5.0f); + _params_standard.back_trans_dur = math::constrain(v, 0.0f, 20.0f); /* target throttle value for pusher motor during the transition to fw mode */ param_get(_params_handles_standard.pusher_trans, &v); @@ -315,9 +315,10 @@ void Standard::update_transition_state() _v_att_sp->q_d_valid = true; // continually increase mc attitude control as we transition back to mc mode - if (_params_standard.back_trans_dur > 0.0f) { - float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * - 1000000.0f); + if (_params_standard.back_trans_dur > FLT_EPSILON) { + float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + ((_params_standard.back_trans_dur / 2) * 1000000.0f); + weight = math::constrain(weight, 0.0f, 1.0f); _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; @@ -451,15 +452,26 @@ void Standard::fill_actuator_outputs() // fixed wing controls _actuators_out_1->timestamp = _actuators_fw_in->timestamp; - //roll - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - //pitch - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); - // yaw - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = - _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); + + if (_vtol_schedule.flight_mode != MC_MODE) { + + //roll + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; + //pitch + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; + // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = + _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; + + } else { + + // zero outputs when inactive + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0.0f; + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _params->fw_pitch_trim; + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = 0.0f; + } // set the fixed wing throttle control if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) { diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index fb8dcd13b9..c9e6cffd49 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -199,12 +199,12 @@ PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0); * * @unit s * @min 0.00 - * @max 10.00 + * @max 20.00 * @increment 1 * @decimal 2 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f); +PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 5.0f); /** * Duration of a back transition @@ -213,12 +213,12 @@ PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f); * * @unit s * @min 0.00 - * @max 10.00 + * @max 20.00 * @increment 1 * @decimal 2 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 2.0f); +PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 4.0f); /** * Transition blending airspeed