Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Plane: Quadplane: move Q Assist check into new VTOL assist files.
- Loading branch information
Showing
7 changed files
with
223 additions
and
209 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,143 @@ | ||
| #include "Plane.h" | ||
|
|
||
| #if HAL_QUADPLANE_ENABLED | ||
|
|
||
| // Assistance hysteresis helpers | ||
|
|
||
| // Reset state | ||
| void VTOL_Assist::Assist_Hysteresis::reset() | ||
| { | ||
| start_ms = 0; | ||
| last_ms = 0; | ||
| active = false; | ||
| } | ||
|
|
||
| // Update state, return true when first triggered | ||
| bool VTOL_Assist::Assist_Hysteresis::update(const bool trigger, const uint32_t &now_ms, const uint32_t &trigger_delay_ms, const uint32_t &clear_delay_ms) | ||
| { | ||
| bool ret = false; | ||
|
|
||
| if (trigger) { | ||
| last_ms = now_ms; | ||
| if (start_ms == 0) { | ||
| start_ms = now_ms; | ||
| } | ||
| if ((now_ms - start_ms) > trigger_delay_ms) { | ||
| // trigger delay has elapsed | ||
| if (!active) { | ||
| // return true on first trigger | ||
| ret = true; | ||
| } | ||
| active = true; | ||
| } | ||
|
|
||
| } else if (active) { | ||
| if ((last_ms == 0) || ((now_ms - last_ms) > clear_delay_ms)) { | ||
| // Clear delay passed | ||
| reset(); | ||
| } | ||
|
|
||
| } else { | ||
| reset(); | ||
| } | ||
|
|
||
| return ret; | ||
| } | ||
|
|
||
| // Assistance not needed, reset any state | ||
| void VTOL_Assist::reset() | ||
| { | ||
| force_assist = false; | ||
| speed_assist = false; | ||
| angle_error.reset(); | ||
| alt_error.reset(); | ||
| } | ||
|
|
||
| /* | ||
| return true if the quadplane should provide stability assistance | ||
| */ | ||
| bool VTOL_Assist::should_assist(float aspeed, bool have_airspeed) | ||
| { | ||
| if (!plane.arming.is_armed_and_safety_off() || (state == STATE::ASSIST_DISABLED) || quadplane.tailsitter.is_control_surface_tailsitter()) { | ||
| // disarmed or disabled by aux switch or because a control surface tailsitter | ||
| reset(); | ||
| return false; | ||
| } | ||
|
|
||
| if (!quadplane.tailsitter.enabled() && !( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed) | ||
| || is_positive(plane.get_throttle_input()) | ||
| || plane.is_flying() ) ) { | ||
| // not in a flight mode and condition where it would be safe to turn on vertial lift motors | ||
| // skip this check for tailsitters because the forward and vertial motors are the same and are controled directly by throttle imput unlike other quadplanes | ||
| reset(); | ||
| return false; | ||
| } | ||
|
|
||
| if (plane.flare_mode != Plane::FlareMode::FLARE_DISABLED) { | ||
| // Never active in fixed wing flare | ||
| reset(); | ||
| return false; | ||
| } | ||
|
|
||
| force_assist = state == STATE::FORCE_ENABLED; | ||
|
|
||
| if (speed <= 0) { | ||
| // all checks disabled via speed threshold, still allow force enabled | ||
| speed_assist = false; | ||
| alt_error.reset(); | ||
| angle_error.reset(); | ||
| return force_assist; | ||
| } | ||
|
|
||
| // assistance due to Q_ASSIST_SPEED | ||
| // if option bit is enabled only allow assist with real airspeed sensor | ||
| speed_assist = (have_airspeed && aspeed < speed) && | ||
| (!quadplane.option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || plane.ahrs.using_airspeed_sensor()); | ||
|
|
||
| const uint32_t now_ms = AP_HAL::millis(); | ||
| const uint32_t tigger_delay_ms = delay * 1000; | ||
| const uint32_t clear_delay_ms = tigger_delay_ms * 2; | ||
|
|
||
| /* | ||
| optional assistance when altitude is too close to the ground | ||
| */ | ||
| if (alt <= 0) { | ||
| // Alt assist disabled | ||
| alt_error.reset(); | ||
|
|
||
| } else { | ||
| const float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); | ||
| if (alt_error.update(height_above_ground < alt, now_ms, tigger_delay_ms, clear_delay_ms)) { | ||
| gcs().send_text(MAV_SEVERITY_WARNING, "Alt assist %.1fm", height_above_ground); | ||
| } | ||
| } | ||
|
|
||
| if (angle <= 0) { | ||
| // Angle assist disabled | ||
| angle_error.reset(); | ||
|
|
||
| } else { | ||
|
|
||
| /* | ||
| now check if we should provide assistance due to attitude error | ||
| */ | ||
| const uint16_t allowed_envelope_error_cd = 500U; | ||
| const bool inside_envelope = (labs(plane.ahrs.roll_sensor) <= (plane.aparm.roll_limit*100 + allowed_envelope_error_cd)) && | ||
| (plane.ahrs.pitch_sensor < (plane.aparm.pitch_limit_max*100 + allowed_envelope_error_cd)) && | ||
| (plane.ahrs.pitch_sensor > (plane.aparm.pitch_limit_min*100 - allowed_envelope_error_cd)); | ||
|
|
||
| const int32_t max_angle_cd = 100U*angle; | ||
| const bool inside_angle_error = (labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) < max_angle_cd) && | ||
| (labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) < max_angle_cd); | ||
|
|
||
| if (angle_error.update(!inside_envelope && !inside_angle_error, now_ms, tigger_delay_ms, clear_delay_ms)) { | ||
| gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d", | ||
| (int)(plane.ahrs.roll_sensor/100), | ||
| (int)(plane.ahrs.pitch_sensor/100)); | ||
| } | ||
| } | ||
|
|
||
| return force_assist || speed_assist || alt_error.is_active() || angle_error.is_active(); | ||
| } | ||
|
|
||
| #endif // HAL_QUADPLANE_ENABLED |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,72 @@ | ||
| #pragma once | ||
|
|
||
| // VTOL assistance in a forward flight mode | ||
|
|
||
| class QuadPlane; | ||
| class VTOL_Assist { | ||
| public: | ||
| VTOL_Assist(QuadPlane& _quadplane):quadplane(_quadplane) {}; | ||
|
|
||
| // check for assistance needed | ||
| bool should_assist(float aspeed, bool have_airspeed); | ||
|
|
||
| // Assistance not needed, reset any state | ||
| void reset(); | ||
|
|
||
| // speed below which quad assistance is given | ||
| AP_Float speed; | ||
|
|
||
| // angular error at which quad assistance is given | ||
| AP_Int8 angle; | ||
|
|
||
| // altitude to trigger assistance | ||
| AP_Int16 alt; | ||
|
|
||
| // Time hysteresis for triggering of assistance | ||
| AP_Float delay; | ||
|
|
||
| // State from pilot | ||
| enum class STATE { | ||
| ASSIST_DISABLED, | ||
| ASSIST_ENABLED, | ||
| FORCE_ENABLED, | ||
| }; | ||
| void set_state(STATE _state) { state = _state; } | ||
|
|
||
| // Logging getters for assist types | ||
| bool in_force_assist() const { return force_assist; } | ||
| bool in_speed_assist() const { return speed_assist; } | ||
| bool in_alt_assist() const { return alt_error.is_active(); } | ||
| bool in_angle_assist() const { return angle_error.is_active(); } | ||
|
|
||
| private: | ||
|
|
||
| // Default to enabled | ||
| STATE state = STATE::ASSIST_ENABLED; | ||
|
|
||
| class Assist_Hysteresis { | ||
| public: | ||
| // Reset state | ||
| void reset(); | ||
|
|
||
| // Update state, return true when first triggered | ||
| bool update(const bool trigger, const uint32_t &now_ms, const uint32_t &trigger_delay_ms, const uint32_t &clear_delay_ms); | ||
|
|
||
| // Return true if the output is active | ||
| bool is_active() const { return active; } | ||
|
|
||
| private: | ||
| uint32_t start_ms; | ||
| uint32_t last_ms; | ||
| bool active; | ||
| }; | ||
| Assist_Hysteresis angle_error; | ||
| Assist_Hysteresis alt_error; | ||
|
|
||
| // Force and speed assist have no hysteresis | ||
| bool force_assist; | ||
| bool speed_assist; | ||
|
|
||
| // Reference to access quadplane | ||
| QuadPlane& quadplane; | ||
| }; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.