/* run output for tailsitters */ void QuadPlane::tailsitter_output(void) { if (!tailsitter_active()) { return; } motors_output(); plane.pitchController.reset_I(); plane.rollController.reset_I(); if (tailsitter.input_mask_chan > 0 && tailsitter.input_mask > 0 && hal.rcin->read(tailsitter.input_mask_chan-1) > 1700) { // the user is learning to prop-hang if (tailsitter.input_mask & TAILSITTER_MASK_AILERON) { SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz()); } if (tailsitter.input_mask & TAILSITTER_MASK_ELEVATOR) { SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz()); } if (tailsitter.input_mask & TAILSITTER_MASK_THROTTLE) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.channel_throttle->get_control_in_zero_dz()); } if (tailsitter.input_mask & TAILSITTER_MASK_RUDDER) { SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.channel_rudder->get_control_in_zero_dz()); } } }
// handle different tailsitter input types void QuadPlane::tailsitter_check_input(void) { if (tailsitter_active() && tailsitter.input_type == TAILSITTER_INPUT_PLANE) { // the user has asked for body frame controls when tailsitter // is active. We switch around the control_in value for the // channels to do this, as that ensures the value is // consistent throughout the code int16_t roll_in = plane.channel_roll->get_control_in(); int16_t yaw_in = plane.channel_rudder->get_control_in(); plane.channel_roll->set_control_in(yaw_in); plane.channel_rudder->set_control_in(-roll_in); } }
/* run output for tailsitters */ void QuadPlane::tailsitter_output(void) { if (!is_tailsitter()) { return; } 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); float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); float tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain; float tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain; SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); } else { 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); pos_control->get_accel_z_pid().set_integrator(throttle*10); } return; } motors_output(false); plane.pitchController.reset_I(); plane.rollController.reset_I(); if (hal.util->get_soft_armed()) { // scale surfaces for throttle tailsitter_speed_scaling(); } if (tailsitter.vectored_hover_gain > 0) { // thrust vectoring VTOL modes float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator); /* apply extra elevator when at high pitch errors, using a power law. This allows the motors to point straight up for takeoff without integrator windup */ int32_t pitch_error_cd = (plane.nav_pitch_cd - ahrs_view->pitch_sensor) * 0.5; float extra_pitch = constrain_float(pitch_error_cd, -4500, 4500) / 4500.0; float extra_sign = extra_pitch > 0?1:-1; float extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * 4500; float tilt_left = extra_elevator + (elevator + aileron) * tailsitter.vectored_hover_gain; float tilt_right = extra_elevator + (elevator - aileron) * tailsitter.vectored_hover_gain; if (fabsf(tilt_left) >= 4500 || fabsf(tilt_right) >= 4500) { // prevent integrator windup motors->limit.roll_pitch = 1; motors->limit.yaw = 1; } SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right); } if (tailsitter.input_mask_chan > 0 && tailsitter.input_mask > 0 && RC_Channels::get_radio_in(tailsitter.input_mask_chan-1) > 1700) { // the user is learning to prop-hang if (tailsitter.input_mask & TAILSITTER_MASK_AILERON) { SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz()); } if (tailsitter.input_mask & TAILSITTER_MASK_ELEVATOR) { SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz()); } if (tailsitter.input_mask & TAILSITTER_MASK_THROTTLE) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.channel_throttle->get_control_in_zero_dz()); } if (tailsitter.input_mask & TAILSITTER_MASK_RUDDER) { SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.channel_rudder->get_control_in_zero_dz()); } } }