Skip to content

Commit

Permalink
Plane: TKOFF_THR_MIN is applied to SLT transitions
Browse files Browse the repository at this point in the history
  • Loading branch information
Georacer committed Aug 1, 2024
1 parent 0d30dd1 commit cd0403b
Show file tree
Hide file tree
Showing 5 changed files with 27 additions and 0 deletions.
4 changes: 4 additions & 0 deletions ArduPlane/quadplane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -838,6 +838,10 @@ bool QuadPlane::setup(void)
pilot_speed_z_max_dn.convert_centi_parameter(AP_PARAM_INT16);
pilot_accel_z.convert_centi_parameter(AP_PARAM_INT16);

// Provisionally assign the SLT thrust type.
// It will be overwritten by tailsitter or tiltorotor setups.
thrust_type = ThrustType::THRUST_TYPE_SLT;

tailsitter.setup();

tiltrotor.setup();
Expand Down
11 changes: 11 additions & 0 deletions ArduPlane/quadplane.h
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,14 @@ class QuadPlane
void control_auto(void);
bool setup(void);

enum class ThrustType : uint8_t {
THRUST_TYPE_UNDEFINED=0, // Undefined. Means that no backend has configured it yet.
THRUST_TYPE_SLT, // Traditional quadplane, with a pusher motor and independent multicopter lift.
THRUST_TYPE_TAILSITTER,
THRUST_TYPE_TILTROTOR,
};
ThrustType get_thrust_type(void) {return thrust_type;}

void vtol_position_controller(void);
void setup_target_position(void);
void takeoff_controller(void);
Expand Down Expand Up @@ -198,6 +206,9 @@ class QuadPlane
AP_Enum<AP_Motors::motor_frame_class> frame_class;
AP_Enum<AP_Motors::motor_frame_type> frame_type;

// Types of different "quadplane" configurations.
ThrustType thrust_type;

// Initialise motors to allow passing it to tailsitter in its constructor
AP_MotorsMulticopter *motors = nullptr;
const struct AP_Param::GroupInfo *motors_var_info;
Expand Down
8 changes: 8 additions & 0 deletions ArduPlane/servos.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -559,6 +559,14 @@ float Plane::apply_throttle_limits(float throttle_in)
if (aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max.get();
}

// Apply minimum throttle limits only for SLT thrust types.
// The other types don't support it well.
if (quadplane.thrust_type == QuadPlane::ThrustType::THRUST_TYPE_SLT) {
if (aparm.takeoff_throttle_min.get() != 0) {
min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get());
}
}
}
#endif

Expand Down
2 changes: 2 additions & 0 deletions ArduPlane/tailsitter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -208,6 +208,8 @@ void Tailsitter::setup()
return;
}

quadplane.thrust_type = QuadPlane::ThrustType::THRUST_TYPE_TAILSITTER;

// Set tailsitter transition rate to match old calculation
if (!transition_rate_fw.configured()) {
transition_rate_fw.set_and_save(transition_angle_fw / (quadplane.transition_time_ms/2000.0f));
Expand Down
2 changes: 2 additions & 0 deletions ArduPlane/tiltrotor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,8 @@ void Tiltrotor::setup()
return;
}

quadplane.thrust_type = QuadPlane::ThrustType::THRUST_TYPE_TILTROTOR;

_is_vectored = tilt_mask != 0 && type == TILT_TYPE_VECTORED_YAW;

// true if a fixed forward motor is configured, either throttle, throttle left or throttle right.
Expand Down

0 comments on commit cd0403b

Please sign in to comment.