From c787f4c56fb4f65ebfe5bbde5b9f1623431fe2bc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 11 Feb 2017 19:12:56 +1100 Subject: [PATCH] Plane: initial support for tailsitter transitions --- ArduPlane/ArduPlane.cpp | 9 ++++++-- ArduPlane/Attitude.cpp | 5 +++++ ArduPlane/GCS_Mavlink.cpp | 16 ++++++++++--- ArduPlane/quadplane.cpp | 47 +++++++++++++++++++++++++++++++++++---- ArduPlane/quadplane.h | 14 +++++++++++- ArduPlane/tailsitter.cpp | 11 ++++++++- ArduPlane/takeoff.cpp | 2 +- 7 files changed, 92 insertions(+), 12 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 033cc7f229..df07bc27e8 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -175,8 +175,13 @@ void Plane::ahrs_update() } // calculate a scaled roll limit based on current pitch - roll_limit_cd = aparm.roll_limit_cd * cosf(ahrs.pitch); - pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll)); + roll_limit_cd = aparm.roll_limit_cd; + pitch_limit_min_cd = aparm.pitch_limit_min_cd; + + if (!quadplane.tailsitter_active()) { + roll_limit_cd *= ahrs.cos_pitch(); + pitch_limit_min_cd *= fabsf(ahrs.cos_roll()); + } // updated the summed gyro used for ground steering and // auto-takeoff. Dot product of DCM.c with gyro vector gives earth diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index f66a8942ce..53b3bf2771 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -671,6 +671,11 @@ void Plane::update_load_factor(void) // no roll limits when inverted return; } + if (quadplane.tailsitter_active()) { + // no limits while hovering + return; + } + float max_load_factor = smoothed_airspeed / aparm.airspeed_min; if (max_load_factor <= 1) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index b40544c2e5..65707ce959 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -99,13 +99,23 @@ void Plane::send_heartbeat(mavlink_channel_t chan) void Plane::send_attitude(mavlink_channel_t chan) { + float r = ahrs.roll; + float p = ahrs.pitch - radians(g.pitch_trim_cd*0.01f); + float y = ahrs.yaw; + + if (quadplane.tailsitter_active()) { + r = quadplane.ahrs_view->roll; + p = quadplane.ahrs_view->pitch; + y = quadplane.ahrs_view->yaw; + } + const Vector3f &omega = ahrs.get_gyro(); mavlink_msg_attitude_send( chan, millis(), - ahrs.roll, - ahrs.pitch - radians(g.pitch_trim_cd*0.01f), - ahrs.yaw, + r, + p, + y, omega.x, omega.y, omega.z); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c83400d9a3..cc31bf5ee9 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -333,6 +333,12 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE // @Values: 0:Continuous,1:Binary AP_GROUPINFO("TILT_TYPE", 47, QuadPlane, tilt.tilt_type, TILT_TYPE_CONTINUOUS), + + // @Param: Q_TAILSIT_ANGLE + // @DisplayName: Tailsitter transition angle + // @Description: This is the angle at which tailsitter aircraft will change from VTOL control to fixed wing control. + // @Range: 5 80 + AP_GROUPINFO("TAILSIT_ANGLE", 48, QuadPlane, tailsitter.transition_angle, 30), AP_GROUPEND }; @@ -377,7 +383,8 @@ bool QuadPlane::setup(void) float loop_delta_t = 1.0 / plane.scheduler.get_loop_rate_hz(); enum AP_Motors::motor_frame_class motor_class; - + enum Rotation rotation = ROTATION_NONE; + /* cope with upgrade from old AP_Motors values for frame_class */ @@ -455,6 +462,7 @@ bool QuadPlane::setup(void) case AP_Motors::MOTOR_FRAME_TAILSITTER: motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz()); var_info = AP_MotorsTailsitter::var_info; + rotation = ROTATION_PITCH_90; break; default: motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz()); @@ -466,15 +474,22 @@ bool QuadPlane::setup(void) hal.console->printf("%s motors\n", strUnableToAllocate); goto failed; } - + AP_Param::load_object_from_eeprom(motors, var_info); - attitude_control = new AC_AttitudeControl_Multi(ahrs, aparm, *motors, loop_delta_t); + + // create the attitude view used by the VTOL code + ahrs_view = ahrs.create_view(rotation); + if (ahrs_view == nullptr) { + goto failed; + } + + attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, loop_delta_t); if (!attitude_control) { hal.console->printf("%s attitude_control\n", strUnableToAllocate); goto failed; } AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info); - pos_control = new AC_PosControl(ahrs, inertial_nav, *motors, *attitude_control, + pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control, p_alt_hold, p_vel_z, pid_accel_z, p_pos_xy, pi_vel_xy); if (!pos_control) { @@ -1011,6 +1026,7 @@ void QuadPlane::update_transition(void) */ if (have_airspeed && assistance_needed(aspeed) && + !is_tailsitter() && (plane.auto_throttle_mode || plane.channel_throttle->get_control_in()>0 || plane.is_flying())) { @@ -1025,6 +1041,14 @@ void QuadPlane::update_transition(void) assisted_flight = false; } + if (is_tailsitter()) { + if (transition_state == TRANSITION_ANGLE_WAIT && + (ahrs_view->pitch_sensor < -tailsitter.transition_angle*100 || plane.fly_inverted())) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition done"); + transition_state = TRANSITION_DONE; + } + } + // if rotors are fully forward then we are not transitioning if (tiltrotor_fully_fwd()) { transition_state = TRANSITION_DONE; @@ -1092,6 +1116,19 @@ void QuadPlane::update_transition(void) break; } + case TRANSITION_ANGLE_WAIT: { + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + assisted_flight = true; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, + -(tailsitter.transition_angle+15)*100, + 0, + smoothing_gain); + attitude_control->set_throttle_out(1.0f, true, 0); + run_rate_controller(); + motors_output(); + break; + } + case TRANSITION_DONE: if (!tilt.motors_active && !is_tailsitter()) { motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); @@ -1143,6 +1180,8 @@ void QuadPlane::update(void) transition_start_ms = 0; if (throttle_wait && !plane.is_flying()) { transition_state = TRANSITION_DONE; + } else if (is_tailsitter()) { + transition_state = TRANSITION_ANGLE_WAIT; } else { transition_state = TRANSITION_AIRSPEED_WAIT; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 9dde914f15..bc86160746 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -75,8 +75,11 @@ public: // see if we are flying from vtol point of view bool is_flying_vtol(void); - // return true when flying a tailsitter + // return true when tailsitter frame configured bool is_tailsitter(void); + + // return true when flying a tailsitter in VTOL + bool tailsitter_active(void); // create outputs for tailsitters void tailsitter_output(void); @@ -255,6 +258,7 @@ private: enum { TRANSITION_AIRSPEED_WAIT, TRANSITION_TIMER, + TRANSITION_ANGLE_WAIT, TRANSITION_DONE } transition_state; @@ -320,6 +324,14 @@ private: bool motors_active:1; } tilt; + // tailsitter control variables + struct { + AP_Int8 transition_angle; + } tailsitter; + + // the attitude view of the VTOL attitude controller + AP_AHRS_View *ahrs_view; + // time when motors were last active uint32_t last_motors_active_ms; diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 13835f2cd0..0dc030930b 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -26,12 +26,21 @@ bool QuadPlane::is_tailsitter(void) return available() && frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER; } +/* + check if we are flying as a tailsitter + */ +bool QuadPlane::tailsitter_active(void) +{ + return is_tailsitter() && in_vtol_mode(); +} + /* run output for tailsitters */ void QuadPlane::tailsitter_output(void) { - if (is_tailsitter() && in_vtol_mode()) { + if (tailsitter_active()) { motors_output(); } } + diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 053e6270de..8624c33d78 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -58,7 +58,7 @@ bool Plane::auto_takeoff_check(void) goto no_launch; } - if (!is_tailsitter()) { + if (!quadplane.is_tailsitter()) { // Check aircraft attitude for bad launch if (ahrs.pitch_sensor <= -3000 || ahrs.pitch_sensor >= 4500 ||