Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

plane: add tailsitter transision rates, give more info in transision complete messages

Browse Source
This commit is contained in:
Peter Hall 2021-03-25 20:32:31 +00:00 committed by Andrew Tridgell
parent d822499155
commit 33f9e3d4b6
4 changed files with 83 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/Attitude.cpp
View File

@ -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;
}

45
ArduPlane/quadplane.cpp
View File

@ -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;
}

10
ArduPlane/quadplane.h
View File

@ -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

42
ArduPlane/tailsitter.cpp
View File

@ -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
*/