Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: added tilt vectoring in fixed wing modes 

this allows for vectoring for roll and pitch in fixed wing modes on
tilt-vectored quadplanes
This commit is contained in:
Andrew Tridgell 2021-01-03 16:52:23 +11:00
parent a74d087fd8
commit a0fcef6ceb
3 changed files with 79 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduPlane/quadplane.cpp
View File

@ -509,6 +509,22 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
// @User: Standard
AP_GROUPINFO("TAILSIT_DSKLD", 21, QuadPlane, tailsitter.disk_loading, 0),
// @Param: TILT_FIX_ANGLE
// @DisplayName: Fixed wing tiltrotor angle
// @Description: This is the angle the motors tilt down when at maximum output for forward flight. Set this to a non-zero value to enable vectoring for roll/pitch in forward flight on tilt-vectored aircraft
// @Units: deg
// @Range: 0 30
// @User: Standard
AP_GROUPINFO("TILT_FIX_ANGLE", 22, QuadPlane, tilt.fixed_angle, 0),
// @Param: TILT_FIX_GAIN
// @DisplayName: Fixed wing tiltrotor gain
// @Description: This is the gain for use of tilting motors in fixed wing flight for tilt vectored quadplanes
// @Range: 0 1
// @User: Standard
AP_GROUPINFO("TILT_FIX_GAIN", 23, QuadPlane, tilt.fixed_gain, 0),
AP_GROUPEND
};

5
ArduPlane/quadplane.h
View File

@ -485,6 +485,8 @@ private:
AP_Int8 max_angle_deg;
AP_Int8 tilt_type;
AP_Float tilt_yaw_angle;
AP_Float fixed_angle;
AP_Float fixed_gain;
float current_tilt;
float current_throttle;
bool motors_active:1;
@ -554,8 +556,9 @@ private:
void tiltrotor_update(void);
void tiltrotor_continuous_update(void);
void tiltrotor_binary_update(void);
void tiltrotor_vectored_yaw(void);
void tiltrotor_vectoring(void);
void tiltrotor_bicopter(void);
float tilt_throttle_scaling(void);
void tilt_compensate_angle(float *thrust, uint8_t num_motors, float non_tilted_mul, float tilted_mul);
void tilt_compensate(float *thrust, uint8_t num_motors);
bool is_motor_tilting(uint8_t motor) const {

78
ArduPlane/tiltrotor.cpp
View File

@ -209,7 +209,7 @@ void QuadPlane::tiltrotor_update(void)
}
if (tilt.tilt_type == TILT_TYPE_VECTORED_YAW) {
tiltrotor_vectored_yaw();
tiltrotor_vectoring();
}
}
@ -319,17 +319,33 @@ bool QuadPlane::tiltrotor_fully_fwd(void)
}
/*
control vectored yaw with tilt multicopters
return scaling factor for tilt rotors by throttle
we want to scale back tilt angle for roll/pitch by throttle in
forward flight
*/
void QuadPlane::tiltrotor_vectored_yaw(void)
float QuadPlane::tilt_throttle_scaling(void)
{
const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01;
// scale relative to a fixed 0.5 mid throttle so that changes in TRIM_THROTTLE in missions don't change
// the scaling of tilt
const float mid_throttle = 0.5;
return mid_throttle / constrain_float(throttle, 0.1, 1.0);
}
/*
control vectoring for tilt multicopters
*/
void QuadPlane::tiltrotor_vectoring(void)
{
// total angle the tilt can go through
float total_angle = 90 + tilt.tilt_yaw_angle;
const float total_angle = 90 + tilt.tilt_yaw_angle + tilt.fixed_angle;
// output value (0 to 1) to get motors pointed straight up
float zero_out = tilt.tilt_yaw_angle / total_angle;
const float zero_out = tilt.tilt_yaw_angle / total_angle;
const float fixed_tilt_limit = tilt.fixed_angle / total_angle;
const float level_out = 1.0 - fixed_tilt_limit;
// calculate the basic tilt amount from current_tilt
float base_output = zero_out + (tilt.current_tilt * (1 - zero_out));
float base_output = zero_out + (tilt.current_tilt * (level_out - zero_out));
// for testing when disarmed, apply vectored yaw in proportion to rudder stick
// Wait TILT_DELAY_MS after disarming to allow props to spin down first.
@ -338,15 +354,32 @@ void QuadPlane::tiltrotor_vectored_yaw(void)
if (!hal.util->get_soft_armed() && (plane.quadplane.options & OPTION_DISARMED_TILT)) {
// this test is subject to wrapping at ~49 days, but the consequences are insignificant
if ((now - hal.util->get_last_armed_change()) > TILT_DELAY_MS) {
float yaw_out = plane.channel_rudder->get_control_in();
yaw_out /= plane.channel_rudder->get_range();
float yaw_range = zero_out;
if (in_vtol_mode()) {
float yaw_out = plane.channel_rudder->get_control_in();
yaw_out /= plane.channel_rudder->get_range();
float yaw_range = zero_out;
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * constrain_float(base_output + yaw_out * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * constrain_float(base_output - yaw_out * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft, 1000 * constrain_float(base_output + yaw_out * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight, 1000 * constrain_float(base_output - yaw_out * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * constrain_float(base_output + yaw_out * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * constrain_float(base_output - yaw_out * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft, 1000 * constrain_float(base_output + yaw_out * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight, 1000 * constrain_float(base_output - yaw_out * yaw_range,0,1));
} else {
// fixed wing tilt
const float gain = tilt.fixed_gain * fixed_tilt_limit;
// base the tilt on elevon mixing, which means it
// takes account of the MIXING_GAIN. The rear tilt is
// based on elevator
const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) / 4500.0;
const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) / 4500.0;
const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0;
// front tilt is effective canards, so need to swap and use negative. Rear motors are treated live elevons.
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,1000 * constrain_float(base_output - right,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight,1000 * constrain_float(base_output - left,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft,1000 * constrain_float(base_output + left,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight,1000 * constrain_float(base_output + right,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output + mid,0,1));
}
}
return;
}
@ -354,11 +387,18 @@ void QuadPlane::tiltrotor_vectored_yaw(void)
float tilt_threshold = (tilt.max_angle_deg/90.0f);
bool no_yaw = (tilt.current_tilt > tilt_threshold);
if (no_yaw) {
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * base_output);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * base_output);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * base_output);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft, 1000 * base_output);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight, 1000 * base_output);
// fixed wing tilt. We need to apply inverse scaling with throttle, and remove the surface speed scaling as
// we don't want tilt impacted by airspeed
const float scaler = plane.control_mode == &plane.mode_manual?1:(tilt_throttle_scaling() / plane.get_speed_scaler());
const float gain = tilt.fixed_gain * fixed_tilt_limit * scaler;
const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) / 4500.0;
const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) / 4500.0;
const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0;
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,1000 * constrain_float(base_output - right,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight,1000 * constrain_float(base_output - left,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft,1000 * constrain_float(base_output + left,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight,1000 * constrain_float(base_output + right,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output + mid,0,1));
} else {
const float yaw_out = motors->get_yaw();
const float roll_out = motors->get_roll();