diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index f39f309fe3..e3aec4c00f 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -386,9 +386,11 @@ void Plane::stabilize() if (quadplane.in_tailsitter_vtol_transition(now)) { /* during transition to vtol in a tailsitter try to raise the - nose rapidly while keeping the wings level + nose while keeping the wings level */ - nav_pitch_cd = constrain_float((quadplane.tailsitter.transition_angle+5)*100, 5500, 8500), + uint32_t dt = now - quadplane.transition_start_ms; + // multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees + nav_pitch_cd = constrain_float(quadplane.transition_initial_pitch + (quadplane.tailsitter.transition_rate_vtol * dt) * 0.1f, -8500, 8500); nav_roll_cd = 0; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 82449145b8..a1e418abf8 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -271,10 +271,18 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { AP_GROUPINFO("TILT_TYPE", 47, QuadPlane, tilt.tilt_type, TILT_TYPE_CONTINUOUS), // @Param: TAILSIT_ANGLE - // @DisplayName: Tailsitter transition angle - // @Description: This is the angle at which tailsitter aircraft will change from VTOL control to fixed wing control. + // @DisplayName: Tailsitter fixed wing transition angle + // @Description: This is the pitch angle at which tailsitter aircraft will change from VTOL control to fixed wing control. + // @Units: deg // @Range: 5 80 - AP_GROUPINFO("TAILSIT_ANGLE", 48, QuadPlane, tailsitter.transition_angle, 45), + AP_GROUPINFO("TAILSIT_ANGLE", 48, QuadPlane, tailsitter.transition_angle_fw, 45), + + // @Param: TAILSIT_ANG_VT + // @DisplayName: Tailsitter VTOL transition angle + // @Description: This is the pitch angle at which tailsitter aircraft will change from fixed wing control to VTOL control, if zero Q_TAILSIT_ANGLE will be used + // @Units: deg + // @Range: 5 80 + AP_GROUPINFO("TAILSIT_ANG_VT", 61, QuadPlane, tailsitter.transition_angle_vtol, 0), // @Param: TILT_RATE_DN // @DisplayName: Tiltrotor downwards tilt rate @@ -343,8 +351,9 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), - + // 60 is used above for VELZ_MAX_DN + // 61 is used above for TS_ANGLE_VTOL AP_GROUPEND }; @@ -535,6 +544,19 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Standard AP_GROUPINFO("TILT_FIX_GAIN", 23, QuadPlane, tilt.fixed_gain, 0), + // @Param: TAILSIT_RAT_FW + // @DisplayName: Tailsitter VTOL to forward flight transition rate + // @Description: The pitch rate at which tailsitter aircraft will pitch down in the transition from VTOL to forward flight + // @Units: deg/s + // @Range: 10 500 + AP_GROUPINFO("TAILSIT_RAT_FW", 24, QuadPlane, tailsitter.transition_rate_fw, 50), + + // @Param: TAILSIT_RAT_VT + // @DisplayName: Tailsitter forward flight to VTOL transition rate + // @Description: The pitch rate at which tailsitter aircraft will pitch up in the transition from forward flight to VTOL + // @Units: deg/s + // @Range: 10 500 + AP_GROUPINFO("TAILSIT_RAT_VT", 25, QuadPlane, tailsitter.transition_rate_vtol, 50), AP_GROUPEND }; @@ -825,6 +847,11 @@ bool QuadPlane::setup(void) AP_Param::convert_old_parameters(&q_conversion_table[0], ARRAY_SIZE(q_conversion_table)); + // Set tailsitter transition rate to match old caculation + if (!tailsitter.transition_rate_fw.configured()) { + tailsitter.transition_rate_fw.set_and_save(tailsitter.transition_angle_fw / (transition_time_ms/2000.0f)); + } + // param count will have changed AP_Param::invalidate_count(); @@ -1772,7 +1799,6 @@ void QuadPlane::update_transition(void) if (is_tailsitter()) { if (transition_state == TRANSITION_ANGLE_WAIT_FW && tailsitter_transition_fw_complete()) { - gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done"); transition_state = TRANSITION_DONE; transition_start_ms = 0; transition_low_airspeed_ms = 0; @@ -1904,12 +1930,9 @@ void QuadPlane::update_transition(void) case TRANSITION_ANGLE_WAIT_FW: { motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); assisted_flight = true; - // calculate transition rate in degrees per - // millisecond. Assume we want to get to the transition angle - // in half the transition time - float transition_rate = tailsitter.transition_angle / float(transition_time_ms/2); uint32_t dt = now - transition_start_ms; - plane.nav_pitch_cd = constrain_float(transition_initial_pitch - (transition_rate * dt)*100, -8500, 8500); + // multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees + plane.nav_pitch_cd = constrain_float(transition_initial_pitch - (tailsitter.transition_rate_fw * dt) * 0.1f, -8500, 8500); plane.nav_roll_cd = 0; check_attitude_relax(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, @@ -1999,6 +2022,7 @@ void QuadPlane::update(void) */ transition_state = TRANSITION_ANGLE_WAIT_VTOL; transition_start_ms = now; + transition_initial_pitch = constrain_float(ahrs.pitch_sensor,-8500,8500); } else if (is_tailsitter() && transition_state == TRANSITION_ANGLE_WAIT_VTOL) { float aspeed; @@ -2013,7 +2037,6 @@ void QuadPlane::update(void) we have completed transition to VTOL as a tailsitter, setup for the back transition when needed */ - gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done"); transition_state = TRANSITION_ANGLE_WAIT_FW; transition_start_ms = now; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 665fc6f005..83ad208186 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -529,7 +529,12 @@ private: // tailsitter control variables struct { - AP_Int8 transition_angle; + // transition from VTOL to forward + AP_Int8 transition_angle_fw; + AP_Float transition_rate_fw; + // transition from forward to VTOL + AP_Int8 transition_angle_vtol; + AP_Float transition_rate_vtol; AP_Int8 input_type; AP_Int8 input_mask; AP_Int8 input_mask_chan; @@ -546,6 +551,9 @@ private: AP_Float disk_loading; } tailsitter; + // return the transition_angle_vtol value + int8_t get_tailsitter_transition_angle_vtol(void) const; + // tailsitter speed scaler float last_spd_scaler = 1.0f; // used to slew rate limiting with TAILSITTER_GSCL_ATT_THR option float log_spd_scaler; // for QTUN log diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index c5c11e2552..376a920fe3 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -244,15 +244,23 @@ bool QuadPlane::tailsitter_transition_fw_complete(void) { if (plane.fly_inverted()) { // transition immediately + gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done, inverted flight"); return true; } int32_t roll_cd = labs(ahrs_view->roll_sensor); if (roll_cd > 9000) { roll_cd = 18000 - roll_cd; } - if (labs(ahrs_view->pitch_sensor) > tailsitter.transition_angle*100 || - roll_cd > tailsitter.transition_angle*100 || - AP_HAL::millis() - transition_start_ms > uint32_t(transition_time_ms)) { + if (labs(ahrs_view->pitch_sensor) > tailsitter.transition_angle_fw*100) { + gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done"); + return true; + } + if (roll_cd > MAX(4500, plane.roll_limit_cd + 500)) { + gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done, roll error"); + return true; + } + if (AP_HAL::millis() - transition_start_ms > ((tailsitter.transition_angle_fw+(transition_initial_pitch*0.01f))/tailsitter.transition_rate_fw)*1500) { + gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done, timeout"); return true; } // still waiting @@ -267,6 +275,7 @@ bool QuadPlane::tailsitter_transition_vtol_complete(void) const { if (plane.fly_inverted()) { // transition immediately + gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done, inverted flight"); return true; } // for vectored tailsitters at zero pilot throttle @@ -274,12 +283,21 @@ bool QuadPlane::tailsitter_transition_vtol_complete(void) const // if we are not moving (hence on the ground?) or don't know // transition immediately to tilt motors up and prevent prop strikes if (ahrs.groundspeed() < 1.0f) { + gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done, zero throttle"); return true; } } - if (labs(plane.ahrs.pitch_sensor) > tailsitter.transition_angle*100 || - labs(plane.ahrs.roll_sensor) > tailsitter.transition_angle*100 || - AP_HAL::millis() - transition_start_ms > 2000) { + const float trans_angle = get_tailsitter_transition_angle_vtol(); + if (labs(plane.ahrs.pitch_sensor) > trans_angle*100) { + gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done"); + return true; + } + if (labs(plane.ahrs.roll_sensor) > MAX(4500, plane.roll_limit_cd + 500)) { + gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done, roll error"); + return true; + } + if (AP_HAL::millis() - transition_start_ms > ((trans_angle-(transition_initial_pitch*0.01f))/tailsitter.transition_rate_vtol)*1500) { + gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done, timeout"); return true; } // still waiting @@ -329,6 +347,18 @@ bool QuadPlane::is_tailsitter_in_fw_flight(void) const return is_tailsitter() && !in_vtol_mode() && transition_state == TRANSITION_DONE; } +/* + return the tailsitter.transition_angle_vtol value if non zero, otherwise returns the tailsitter.transition_angle_fw value. + */ +int8_t QuadPlane::get_tailsitter_transition_angle_vtol() const +{ + if (tailsitter.transition_angle_vtol == 0) { + return tailsitter.transition_angle_fw; + } + return tailsitter.transition_angle_vtol; +} + + /* account for speed scaling of control surfaces in VTOL modes */