From 0eab3faf32a7ec73ce0f0d3771b0e371c9538c4b Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 14 Jul 2021 21:15:49 +0100 Subject: [PATCH] Plane: use new tailsiter class --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/Attitude.cpp | 6 +-- ArduPlane/Plane.h | 1 + ArduPlane/mode.cpp | 2 +- ArduPlane/mode_qstabilize.cpp | 2 +- ArduPlane/quadplane.cpp | 74 +++++++++++++++++------------------ ArduPlane/radio.cpp | 2 +- ArduPlane/servos.cpp | 4 +- ArduPlane/system.cpp | 2 +- ArduPlane/takeoff.cpp | 2 +- 10 files changed, 49 insertions(+), 48 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 1802dbcdc6..28283669e9 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -143,7 +143,7 @@ void Plane::ahrs_update() roll_limit_cd = aparm.roll_limit_cd; pitch_limit_min_cd = aparm.pitch_limit_min_cd; - if (!quadplane.tailsitter_active()) { + if (!quadplane.tailsitter.active()) { roll_limit_cd *= ahrs.cos_pitch(); pitch_limit_min_cd *= fabsf(ahrs.cos_roll()); } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 06a1acc020..1b655e7b5d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -432,7 +432,7 @@ void Plane::stabilize() float speed_scaler = get_speed_scaler(); uint32_t now = AP_HAL::millis(); - if (quadplane.in_tailsitter_vtol_transition(now)) { + if (quadplane.tailsitter.in_vtol_transition(now)) { /* during transition to vtol in a tailsitter try to raise the nose while keeping the wings level @@ -467,7 +467,7 @@ void Plane::stabilize() control_mode == &mode_qrtl || control_mode == &mode_qacro || control_mode == &mode_qautotune) && - !quadplane.in_tailsitter_vtol_transition(now)) { + !quadplane.tailsitter.in_vtol_transition(now)) { quadplane.control_run(); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != &mode_stabilize) { @@ -732,7 +732,7 @@ void Plane::update_load_factor(void) // no roll limits when inverted return; } - if (quadplane.tailsitter_active()) { + if (quadplane.tailsitter.active()) { // no limits while hovering return; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 18d79f7b4f..e3083d66f6 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -138,6 +138,7 @@ public: friend class GCS_Plane; friend class RC_Channel_Plane; friend class RC_Channels_Plane; + friend class Tailsitter; friend class Mode; friend class ModeCircle; diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index a8be2360d8..50964259d4 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -91,7 +91,7 @@ bool Mode::enter() bool Mode::is_vtol_man_throttle() const { - if (plane.quadplane.is_tailsitter_in_fw_flight() && + if (plane.quadplane.tailsitter.is_in_fw_flight() && plane.quadplane.assisted_flight) { // We are a tailsitter that has fully transitioned to Q-assisted forward flight. // In this case the forward throttle directly drives the vertical throttle so diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index fcaaa35093..b16fd8b993 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -24,7 +24,7 @@ void ModeQStabilize::update() const float pitch_input = (float)plane.channel_pitch->get_control_in() / plane.channel_pitch->get_range(); // then scale to target angles in centidegrees - if (plane.quadplane.tailsitter_active()) { + if (plane.quadplane.tailsitter.active()) { // tailsitters are different set_tailsitter_roll_pitch(roll_input, pitch_input); return; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 56d33b817f..240ddc505c 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -740,7 +740,7 @@ bool QuadPlane::setup(void) // TODO: update this if servo function assignments change // used by relax_attitude_control() to control special behavior for vectored tailsitters - _is_vectored = (frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) && + tailsitter._is_vectored = (frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) && (!is_zero(tailsitter.vectored_hover_gain) && (SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorLeft) || SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorRight))); @@ -882,7 +882,7 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) { check_attitude_relax(); - bool use_multicopter_control = in_vtol_mode() && !in_tailsitter_vtol_transition(); + bool use_multicopter_control = in_vtol_mode() && !tailsitter.in_vtol_transition(); bool use_multicopter_eulers = false; if (!use_multicopter_control && @@ -899,10 +899,10 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) // tailsitter-only body-frame roll control options // Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw. - if (is_tailsitter() && - (tailsitter.input_type & TAILSITTER_INPUT_BF_ROLL)) { + if (tailsitter.enabled() && + (tailsitter.input_type & Tailsitter::input::TAILSITTER_INPUT_BF_ROLL)) { - if (!(tailsitter.input_type & TAILSITTER_INPUT_PLANE)) { + if (!(tailsitter.input_type & Tailsitter::input::TAILSITTER_INPUT_PLANE)) { // In multicopter input mode, the roll and yaw stick axes are independent of pitch attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(false, plane.nav_roll_cd, @@ -956,7 +956,7 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) // use the fixed wing desired rates float roll_rate = plane.rollController.get_pid_info().target; float pitch_rate = plane.pitchController.get_pid_info().target; - if (is_tailsitter()) { + if (tailsitter.enabled()) { // tailsitter roll and yaw swapped due to change in reference frame attitude_control->input_rate_bf_roll_pitch_yaw_2(yaw_rate_cds,pitch_rate*100.0f, -roll_rate*100.0f); } else { @@ -978,7 +978,7 @@ void QuadPlane::hold_stabilize(float throttle_in) } else { set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); bool should_boost = true; - if (is_tailsitter() && assisted_flight) { + if (tailsitter.enabled() && assisted_flight) { // tailsitters in forward flight should not use angle boost should_boost = false; } @@ -1009,7 +1009,7 @@ void QuadPlane::run_z_controller(void) pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); // initialise the vertical position controller - if (!is_tailsitter()) { + if (!tailsitter.enabled()) { pos_control->init_z_controller(); } else { // initialise the vertical position controller with no descent @@ -1024,7 +1024,7 @@ void QuadPlane::relax_attitude_control() { // disable roll and yaw control for vectored tailsitters // if not a vectored tailsitter completely disable attitude control - attitude_control->relax_attitude_controllers(_is_vectored); + attitude_control->relax_attitude_controllers(tailsitter._is_vectored); } /* @@ -1198,7 +1198,7 @@ void QuadPlane::control_qacro(void) float target_roll = 0; float target_pitch = plane.channel_pitch->norm_input() * acro_pitch_rate * 100.0f; float target_yaw = 0; - if (is_tailsitter()) { + if (tailsitter.enabled()) { // Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw target_roll = plane.channel_rudder->norm_input() * acro_yaw_rate * 100.0f; target_yaw = -plane.channel_roll->norm_input() * acro_roll_rate * 100.0f; @@ -1286,7 +1286,7 @@ bool QuadPlane::is_flying(void) if (motors->get_throttle() > 0.01f && !motors->limit.throttle_lower) { return true; } - if (in_tailsitter_vtol_transition()) { + if (tailsitter.in_vtol_transition()) { return true; } return false; @@ -1437,7 +1437,7 @@ void QuadPlane::control_loiter() plane.nav_roll_cd = loiter_nav->get_roll(); plane.nav_pitch_cd = loiter_nav->get_pitch(); - if (now - last_pidz_init_ms < (uint32_t)transition_time_ms*2 && !is_tailsitter()) { + if (now - last_pidz_init_ms < (uint32_t)transition_time_ms*2 && !tailsitter.enabled()) { // we limit pitch during initial transition float pitch_limit_cd = linear_interpolate(loiter_initial_pitch_cd, aparm.angle_max, now, @@ -1503,8 +1503,8 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const // add in rudder input float max_rate = yaw_rate_max; - if (is_tailsitter() && - tailsitter.input_type & TAILSITTER_INPUT_BF_ROLL) { + if (tailsitter.enabled() && + tailsitter.input_type & Tailsitter::input::TAILSITTER_INPUT_BF_ROLL) { // must have a non-zero max yaw rate for scaling to work max_rate = (yaw_rate_max < 1.0f) ? 1 : yaw_rate_max; } @@ -1618,7 +1618,7 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const */ bool QuadPlane::assistance_needed(float aspeed, bool have_airspeed) { - if (assist_speed <= 0 || is_control_surface_tailsitter()) { + if (assist_speed <= 0 || tailsitter.is_control_surface_tailsitter()) { // assistance disabled in_angle_assist = false; angle_error_start_ms = 0; @@ -1718,7 +1718,7 @@ void QuadPlane::update_transition(void) plane.control_mode == &plane.mode_acro || plane.control_mode == &plane.mode_training) { // in manual modes quad motors are always off - if (!tilt.motors_active && !is_tailsitter()) { + if (!tilt.motors_active && !tailsitter.enabled()) { set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); motors->output(); } @@ -1746,7 +1746,7 @@ void QuadPlane::update_transition(void) bool have_airspeed = ahrs.airspeed_estimate(aspeed); // tailsitters use angle wait, not airspeed wait - if (is_tailsitter() && transition_state == TRANSITION_AIRSPEED_WAIT) { + if (tailsitter.enabled() && transition_state == TRANSITION_AIRSPEED_WAIT) { transition_state = TRANSITION_ANGLE_WAIT_FW; } @@ -1757,7 +1757,7 @@ void QuadPlane::update_transition(void) (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed)))) { // the quad should provide some assistance to the plane assisted_flight = true; - if (!is_tailsitter()) { + if (!tailsitter.enabled()) { // update transition state for vehicles using airspeed wait if (transition_state != TRANSITION_AIRSPEED_WAIT) { gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed); @@ -1771,9 +1771,9 @@ void QuadPlane::update_transition(void) assisted_flight = false; } - if (is_tailsitter()) { + if (tailsitter.enabled()) { if (transition_state == TRANSITION_ANGLE_WAIT_FW && - tailsitter_transition_fw_complete()) { + tailsitter.transition_fw_complete()) { transition_state = TRANSITION_DONE; transition_start_ms = 0; transition_low_airspeed_ms = 0; @@ -1923,7 +1923,7 @@ void QuadPlane::update_transition(void) return; case TRANSITION_DONE: - if (!tilt.motors_active && !is_tailsitter()) { + if (!tilt.motors_active && !tailsitter.enabled()) { set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); motors->output(); } @@ -1968,7 +1968,7 @@ void QuadPlane::update(void) /* make sure we don't have any residual control from previous flight stages */ - if (is_tailsitter()) { + if (tailsitter.enabled()) { // tailsitters only relax I terms, to make ground testing easier attitude_control->reset_rate_controller_I_terms(); } else { @@ -1991,7 +1991,7 @@ void QuadPlane::update(void) // output to motors motors_output(); - if (now - last_vtol_mode_ms > 1000 && is_tailsitter()) { + if (now - last_vtol_mode_ms > 1000 && tailsitter.enabled()) { /* we are just entering a VTOL mode as a tailsitter, set our transition state so the fixed wing controller brings @@ -2001,7 +2001,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() && + } else if (tailsitter.enabled() && transition_state == TRANSITION_ANGLE_WAIT_VTOL) { float aspeed; bool have_airspeed = ahrs.airspeed_estimate(aspeed); @@ -2010,7 +2010,7 @@ void QuadPlane::update(void) (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed)))) { assisted_flight = true; } - if (tailsitter_transition_vtol_complete()) { + if (tailsitter.transition_vtol_complete()) { /* we have completed transition to VTOL as a tailsitter, setup for the back transition when needed @@ -2026,7 +2026,7 @@ void QuadPlane::update(void) transition_low_airspeed_ms = 0; if (throttle_wait && !plane.is_flying()) { transition_state = TRANSITION_DONE; - } else if (is_tailsitter()) { + } else if (tailsitter.enabled()) { /* setup for the transition back to fixed wing for later */ @@ -2154,7 +2154,7 @@ void QuadPlane::update_throttle_hover() // do not update if quadplane forward motor is running (wing may be generating lift) // we use the THR_MIN value to account for petrol motors idling at THR_MIN - if (!is_tailsitter() && (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > MAX(0,plane.aparm.throttle_min+10))) { + if (!tailsitter.enabled() && (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > MAX(0,plane.aparm.throttle_min+10))) { return; } @@ -2214,7 +2214,7 @@ void QuadPlane::motors_output(bool run_rate_controller) return; } - if (in_tailsitter_vtol_transition() && !assisted_flight) { + if (tailsitter.in_vtol_transition() && !assisted_flight) { /* don't run the motor outputs while in tailsitter->vtol transition. That is taken care of by the fixed wing @@ -2621,7 +2621,7 @@ void QuadPlane::vtol_position_controller(void) gives us a nice lot of deceleration */ if (poscontrol.get_state() == QPOS_APPROACH && distance < stop_distance) { - if (is_tailsitter() || motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { + if (tailsitter.enabled() || motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { // tailsitters don't use airbrake stage for landing gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 v=%.1f d=%.0f sd=%.0f h=%.1f", groundspeed, @@ -2668,7 +2668,7 @@ void QuadPlane::vtol_position_controller(void) vel_forward.last_ms = now_ms; } - if (tilt.tilt_mask == 0 && !is_tailsitter()) { + if (tilt.tilt_mask == 0 && !tailsitter.enabled()) { /* cope with fwd motor thrust loss during approach. We detect this by looking for the fwd throttle saturating. This only @@ -2709,8 +2709,8 @@ void QuadPlane::vtol_position_controller(void) case QPOS_POSITION1: { setup_target_position(); - if (is_tailsitter()) { - if (in_tailsitter_vtol_transition()) { + if (tailsitter.enabled()) { + if (tailsitter.in_vtol_transition()) { break; } poscontrol.set_state(QPOS_POSITION2); @@ -2895,7 +2895,7 @@ void QuadPlane::vtol_position_controller(void) } break; case QPOS_POSITION1: - if (in_tailsitter_vtol_transition()) { + if (tailsitter.in_vtol_transition()) { pos_control->relax_z_controller(0); break; } @@ -3307,7 +3307,7 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) if (plane.current_loc.alt < plane.next_WP_loc.alt) { return false; } - transition_state = is_tailsitter() ? TRANSITION_ANGLE_WAIT_FW : TRANSITION_AIRSPEED_WAIT; + transition_state = tailsitter.enabled() ? TRANSITION_ANGLE_WAIT_FW : TRANSITION_AIRSPEED_WAIT; plane.TECS_controller.set_pitch_max_limit(transition_pitch_max); // todo: why are you doing this, I want to delete it. @@ -3498,7 +3498,7 @@ void QuadPlane::Log_Write_QControl_Tuning() target_climb_rate : target_climb_rate_cms, climb_rate : int16_t(inertial_nav.get_velocity_z()), throttle_mix : attitude_control->get_throttle_mix(), - speed_scaler : log_spd_scaler, + speed_scaler : tailsitter.log_spd_scaler, transition_state : static_cast(transition_state), assist : assisted_flight, }; @@ -3976,7 +3976,7 @@ bool QuadPlane::show_vtol_view() const { bool show_vtol = in_vtol_mode(); - if (is_tailsitter()) { + if (tailsitter.enabled()) { if (show_vtol && (transition_state == TRANSITION_ANGLE_WAIT_VTOL)) { // in a vtol mode but still transitioning from forward flight return false; @@ -4014,7 +4014,7 @@ bool QuadPlane::use_fw_attitude_controllers(void) const motors->armed() && motors->get_desired_spool_state() >= AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED && in_vtol_mode() && - !is_tailsitter() && + !tailsitter.enabled() && poscontrol.get_state() != QPOS_AIRBRAKE) { // we want the desired rates for fixed wing slaved to the // multicopter rates diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index cbdbf05876..ce81b72a80 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -207,7 +207,7 @@ void Plane::read_radio() rudder_arm_disarm_check(); // potentially swap inputs for tailsitters - quadplane.tailsitter_check_input(); + quadplane.tailsitter.check_input(); // check for transmitter tuning changes tuning.check_input(control_mode->mode_number()); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 4d197eff6c..018c7e7ff8 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -741,7 +741,7 @@ void Plane::force_flare(void) if (quadplane.tilt.tilt_type == QuadPlane::TILT_TYPE_BICOPTER) { tilt = 0; // this is tilts up for a Bicopter } - if (quadplane.is_tailsitter()) { + if (quadplane.tailsitter.enabled()) { tilt = SERVO_MAX; //this is tilts up for a tailsitter } SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, tilt); @@ -910,7 +910,7 @@ void Plane::servos_output(void) servos_twin_engine_mix(); // cope with tailsitters and bicopters - quadplane.tailsitter_output(); + quadplane.tailsitter.output(); quadplane.tiltrotor_bicopter(); // support forced flare option diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index ddef79724b..753845f4f5 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -408,7 +408,7 @@ bool Plane::should_log(uint32_t mask) */ int8_t Plane::throttle_percentage(void) { - if (quadplane.in_vtol_mode() && !quadplane.in_tailsitter_vtol_transition()) { + if (quadplane.in_vtol_mode() && !quadplane.tailsitter.in_vtol_transition()) { return quadplane.throttle_percentage(); } float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index ea48fc1895..a3f66441c8 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -80,7 +80,7 @@ bool Plane::auto_takeoff_check(void) goto no_launch; } - if (!quadplane.is_tailsitter() && + if (!quadplane.tailsitter.enabled() && !(g2.flight_options & FlightOptions::DISABLE_TOFF_ATTITUDE_CHK)) { // Check aircraft attitude for bad launch if (ahrs.pitch_sensor <= -3000 || ahrs.pitch_sensor >= 4500 ||