diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0a68a37a9e..1b458983ad 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -364,15 +364,25 @@ void Plane::stabilize() } float speed_scaler = get_speed_scaler(); + if (quadplane.in_tailsitter_vtol_transition()) { + /* + during transition to vtol in a tailsitter try to raise the + nose rapidly while keeping the wings level + */ + nav_pitch_cd = constrain_float((quadplane.tailsitter.transition_angle+5)*100, 5500, 8500), + nav_roll_cd = 0; + } + if (control_mode == TRAINING) { stabilize_training(speed_scaler); } else if (control_mode == ACRO) { stabilize_acro(speed_scaler); - } else if (control_mode == QSTABILIZE || - control_mode == QHOVER || - control_mode == QLOITER || - control_mode == QLAND || - control_mode == QRTL) { + } else if ((control_mode == QSTABILIZE || + control_mode == QHOVER || + control_mode == QLOITER || + control_mode == QLAND || + control_mode == QRTL) && + !quadplane.in_tailsitter_vtol_transition()) { quadplane.control_run(); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1d5057db38..914f0717d3 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1240,9 +1240,9 @@ void QuadPlane::update_transition(void) } if (is_tailsitter()) { - if (transition_state == TRANSITION_ANGLE_WAIT && - tailsitter_transition_complete()) { - gcs().send_text(MAV_SEVERITY_INFO, "Transition done"); + if (transition_state == TRANSITION_ANGLE_WAIT_FW && + tailsitter_transition_fw_complete()) { + gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done"); transition_state = TRANSITION_DONE; } } @@ -1330,19 +1330,25 @@ void QuadPlane::update_transition(void) break; } - case TRANSITION_ANGLE_WAIT: { + case TRANSITION_ANGLE_WAIT_FW: { 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, + plane.nav_pitch_cd = constrain_float(-(tailsitter.transition_angle+5)*100, -8500, -2500); + plane.nav_roll_cd = 0; + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, + plane.nav_pitch_cd, 0, smoothing_gain); - attitude_control->set_throttle_out(1.0f, true, 0); + attitude_control->set_throttle_out(motors->get_throttle_hover(), true, 0); run_rate_controller(); motors_output(); break; } - + + case TRANSITION_ANGLE_WAIT_VTOL: + // nothing to do, this is handled in the fw attitude controller + break; + case TRANSITION_DONE: if (!tilt.motors_active && !is_tailsitter()) { motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); @@ -1394,6 +1400,8 @@ void QuadPlane::update(void) if (!in_vtol_mode()) { update_transition(); } else { + uint32_t now = AP_HAL::millis(); + assisted_flight = false; // give full authority to attitude control @@ -1404,16 +1412,50 @@ void QuadPlane::update(void) // output to motors motors_output(); - transition_start_ms = 0; - if (throttle_wait && !plane.is_flying()) { - transition_state = TRANSITION_DONE; - } else if (is_tailsitter()) { - transition_state = TRANSITION_ANGLE_WAIT; - transition_start_ms = AP_HAL::millis(); + + if (now - last_vtol_mode_ms > 1000 && is_tailsitter()) { + /* + we are just entering a VTOL mode as a tailsitter, set + our transition state so the fixed wing controller brings + the nose up before we start trying to fly as a + multicopter + */ + transition_state = TRANSITION_ANGLE_WAIT_VTOL; + transition_start_ms = now; + } else if (is_tailsitter() && + transition_state == TRANSITION_ANGLE_WAIT_VTOL) { + if (tailsitter_transition_vtol_complete()) { + /* + 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; + } } else { - transition_state = TRANSITION_AIRSPEED_WAIT; + /* + setup the transition state appropriately for next time we go into a non-VTOL mode + */ + transition_start_ms = 0; + if (throttle_wait && !plane.is_flying()) { + transition_state = TRANSITION_DONE; + } else if (is_tailsitter()) { + /* + setup for the transition back to fixed wing for later + */ + transition_state = TRANSITION_ANGLE_WAIT_FW; + transition_start_ms = now; + } else { + /* + setup for airspeed wait for later + */ + transition_state = TRANSITION_AIRSPEED_WAIT; + } + last_throttle = motors->get_throttle(); } - last_throttle = motors->get_throttle(); + + last_vtol_mode_ms = now; } // disable throttle_wait when throttle rises above 10% @@ -1494,6 +1536,15 @@ void QuadPlane::motors_output(void) return; } + if (in_tailsitter_vtol_transition()) { + /* + don't run the motor outputs while in tailsitter->vtol + transition. That is taken care of by the fixed wing + stabilisation code + */ + return; + } + // see if motors should be shut down check_throttle_suppression(); @@ -1626,7 +1677,7 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) /* are we in a VTOL auto state? */ -bool QuadPlane::in_vtol_auto(void) +bool QuadPlane::in_vtol_auto(void) const { if (!enable) { return false; @@ -1659,7 +1710,7 @@ bool QuadPlane::in_vtol_auto(void) /* are we in a VTOL mode? */ -bool QuadPlane::in_vtol_mode(void) +bool QuadPlane::in_vtol_mode(void) const { if (!enable) { return false; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index a66457ec4d..b7033857dd 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -54,6 +54,11 @@ public: return true if we are in a transition to fwd flight from hover */ bool in_transition(void) const; + + /* + return true if we are a tailsitter transitioning to VTOL flight + */ + bool in_tailsitter_vtol_transition(void) const; bool handle_do_vtol_transition(enum MAV_VTOL_STATE state); @@ -61,8 +66,8 @@ public: bool do_vtol_land(const AP_Mission::Mission_Command& cmd); bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd); bool verify_vtol_land(void); - bool in_vtol_auto(void); - bool in_vtol_mode(void); + bool in_vtol_auto(void) const; + bool in_vtol_mode(void) const; // vtol help for is_flying() bool is_flying(void); @@ -80,7 +85,7 @@ public: bool is_flying_vtol(void); // return true when tailsitter frame configured - bool is_tailsitter(void); + bool is_tailsitter(void) const; // return true when flying a tailsitter in VTOL bool tailsitter_active(void); @@ -91,9 +96,12 @@ public: // handle different tailsitter input types void tailsitter_check_input(void); - // check if we have completed transition - bool tailsitter_transition_complete(void); + // check if we have completed transition to fixed wing + bool tailsitter_transition_fw_complete(void); + // check if we have completed transition to vtol + bool tailsitter_transition_vtol_complete(void) const; + // user initiated takeoff for guided mode bool do_user_takeoff(float takeoff_altitude); @@ -294,7 +302,8 @@ private: enum { TRANSITION_AIRSPEED_WAIT, TRANSITION_TIMER, - TRANSITION_ANGLE_WAIT, + TRANSITION_ANGLE_WAIT_FW, + TRANSITION_ANGLE_WAIT_VTOL, TRANSITION_DONE } transition_state; @@ -399,6 +408,9 @@ private: // time when we last ran the vertical accel controller uint32_t last_pidz_active_ms; uint32_t last_pidz_init_ms; + + // time when we were last in a vtol control mode + uint32_t last_vtol_mode_ms; void tiltrotor_slew(float tilt); void tiltrotor_binary_slew(bool forward); diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 2275774e68..77ca4f5d76 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -21,7 +21,7 @@ /* return true when flying a tailsitter */ -bool QuadPlane::is_tailsitter(void) +bool QuadPlane::is_tailsitter(void) const { return available() && frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER; } @@ -37,8 +37,8 @@ bool QuadPlane::tailsitter_active(void) if (in_vtol_mode()) { return true; } - // check if we are in ANGLE_WAIT transition - if (transition_state == TRANSITION_ANGLE_WAIT) { + // check if we are in ANGLE_WAIT fixed wing transition + if (transition_state == TRANSITION_ANGLE_WAIT_FW) { return true; } return false; @@ -52,7 +52,7 @@ void QuadPlane::tailsitter_output(void) if (!is_tailsitter()) { return; } - if (!tailsitter_active()) { + if (!tailsitter_active() || in_tailsitter_vtol_transition()) { if (tailsitter.vectored_forward_gain > 0) { // thrust vectoring in fixed wing flight float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); @@ -65,6 +65,19 @@ void QuadPlane::tailsitter_output(void) SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 0); } + if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying() && hal.util->get_soft_armed()) { + /* + during transitions to vtol mode set the throttle to the + hover throttle, and set the altitude controller + integrator to the same throttle level + */ + uint8_t throttle = motors->get_throttle_hover() * 100; + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); + pid_accel_z.set_integrator(throttle*10); + } return; } @@ -120,14 +133,37 @@ void QuadPlane::tailsitter_output(void) /* return true when we have completed enough of a transition to switch to fixed wing control */ -bool QuadPlane::tailsitter_transition_complete(void) +bool QuadPlane::tailsitter_transition_fw_complete(void) { if (plane.fly_inverted()) { // transition immediately 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 || - labs(ahrs_view->roll_sensor) > tailsitter.transition_angle*100 || + roll_cd > tailsitter.transition_angle*100 || + AP_HAL::millis() - transition_start_ms > 2000) { + return true; + } + // still waiting + return false; +} + + +/* + return true when we have completed enough of a transition to switch to VTOL control + */ +bool QuadPlane::tailsitter_transition_vtol_complete(void) const +{ + if (plane.fly_inverted()) { + // transition immediately + 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) { return true; } @@ -150,3 +186,11 @@ void QuadPlane::tailsitter_check_input(void) plane.channel_rudder->set_control_in(-roll_in); } } + +/* + return true if we are a tailsitter transitioning to VTOL flight + */ +bool QuadPlane::in_tailsitter_vtol_transition(void) const +{ + return is_tailsitter() && in_vtol_mode() && transition_state == TRANSITION_ANGLE_WAIT_VTOL; +}