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

Plane: use dedicated quadplane function for stick mixing enable

This commit is contained in:
Iampete1 2023-04-16 01:18:05 +01:00 committed by Andrew Tridgell
parent 1415418d6e
commit 74e4466cb4
6 changed files with 45 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduPlane/Attitude.cpp
View File

@ -219,6 +219,11 @@ void Plane::stabilize_stick_mixing_direct()
if (!stick_mixing_enabled()) {
return;
}
#if HAL_QUADPLANE_ENABLED
if (!quadplane.allow_stick_mixing()) {
return;
}
#endif
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
aileron = channel_roll->stick_mixing(aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
@ -249,6 +254,7 @@ void Plane::stabilize_stick_mixing_fbw()
#if QAUTOTUNE_ENABLED
control_mode == &mode_qautotune ||
#endif
!quadplane.allow_stick_mixing() ||
#endif // HAL_QUADPLANE_ENABLED
control_mode == &mode_training) {
return;
@ -347,10 +353,9 @@ void Plane::stabilize()
}
uint32_t now = AP_HAL::millis();
bool allow_stick_mixing = true;
#if HAL_QUADPLANE_ENABLED
if (quadplane.available()) {
quadplane.transition->set_FW_roll_pitch(nav_pitch_cd, nav_roll_cd, allow_stick_mixing);
quadplane.transition->set_FW_roll_pitch(nav_pitch_cd, nav_roll_cd);
}
#endif
@ -392,9 +397,7 @@ void Plane::stabilize()
} else if (control_mode == &mode_stabilize) {
stabilize_roll();
stabilize_pitch();
if (allow_stick_mixing) {
stabilize_stick_mixing_direct();
}
stabilize_stick_mixing_direct();
stabilize_yaw();
#if HAL_QUADPLANE_ENABLED
} else if (control_mode->is_vtol_mode() && !quadplane.tailsitter.in_vtol_transition(now)) {
@ -412,7 +415,7 @@ void Plane::stabilize()
} else {
// Direct stick mixing functionality has been removed, so as not to remove all stick mixing from the user completely
// the old direct option is now used to enable fbw mixing, this is easier than doing a param conversion.
if (allow_stick_mixing && ((g.stick_mixing == StickMixing::FBW) || (g.stick_mixing == StickMixing::DIRECT_REMOVED))) {
if ((g.stick_mixing == StickMixing::FBW) || (g.stick_mixing == StickMixing::DIRECT_REMOVED)) {
stabilize_stick_mixing_fbw();
}
stabilize_roll();

13
ArduPlane/quadplane.cpp
View File

@ -4349,7 +4349,7 @@ MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const
}
// Set FW roll and pitch limits and keep TECS informed
void SLT_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing)
void SLT_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd)
{
if (quadplane.in_vtol_mode() || quadplane.in_vtol_airbrake()) {
// not in FW flight
@ -4528,4 +4528,15 @@ bool QuadPlane::abort_landing(void)
return true;
}
// Should we allow stick mixing from the pilot
bool QuadPlane::allow_stick_mixing() const
{
if (!available()) {
// Quadplane not enabled
return true;
}
// Ask transition logic
return transition->allow_stick_mixing();
}
#endif // HAL_QUADPLANE_ENABLED

3
ArduPlane/quadplane.h
View File

@ -180,6 +180,9 @@ public:
*/
bool in_vtol_land_descent(void) const;
// Should we allow stick mixing from the pilot
bool allow_stick_mixing() const;
private:
AP_AHRS &ahrs;

17
ArduPlane/tailsitter.cpp
View File

@ -880,7 +880,7 @@ bool Tailsitter_Transition::show_vtol_view() const
return show_vtol;
}
void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing)
void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd)
{
uint32_t now = AP_HAL::millis();
if (tailsitter.in_vtol_transition(now)) {
@ -892,7 +892,6 @@ void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& na
// multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees
nav_pitch_cd = constrain_float(vtol_transition_initial_pitch + (tailsitter.transition_rate_vtol * dt) * 0.1f, -8500, 8500);
nav_roll_cd = 0;
allow_stick_mixing = false;
} else if (transition_state == TRANSITION_DONE) {
// still in FW, reset transition starting point
@ -908,12 +907,24 @@ void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& na
} else {
nav_pitch_cd = pitch_limit_cd;
nav_roll_cd = 0;
allow_stick_mixing = false;
}
}
}
}
bool Tailsitter_Transition::allow_stick_mixing() const
{
// Transitioning into VTOL flight, inital pitch up
if (tailsitter.in_vtol_transition()) {
return false;
}
// Transitioning into fixed wing flight, leveling off
if ((transition_state == TRANSITION_DONE) && (fw_limit_start_ms != 0)) {
return false;
}
return true;
}
bool Tailsitter_Transition::set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd)
{
if (vtol_limit_start_ms == 0) {

4
ArduPlane/tailsitter.h
View File

@ -159,7 +159,9 @@ public:
bool show_vtol_view() const override;
void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) override;
void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd) override;
bool allow_stick_mixing() const override;
MAV_VTOL_STATE get_mav_vtol_state() const override;

6
ArduPlane/transition.h
View File

@ -40,7 +40,7 @@ public:
virtual bool show_vtol_view() const = 0;
virtual void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) {};
virtual void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd) {};
virtual bool set_FW_roll_limit(int32_t& roll_limit_cd) { return false; }
@ -56,6 +56,8 @@ public:
virtual void set_last_fw_pitch(void) {}
virtual bool allow_stick_mixing() const { return true; }
protected:
// refences for convenience
@ -87,7 +89,7 @@ public:
bool show_vtol_view() const override;
void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) override;
void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd) override;
bool set_FW_roll_limit(int32_t& roll_limit_cd) override;