Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/ArduPlane/VTOL_Assist.cpp
Views: 1798
#include "Plane.h"12#if HAL_QUADPLANE_ENABLED34// Assistance hysteresis helpers56// Reset state7void VTOL_Assist::Assist_Hysteresis::reset()8{9start_ms = 0;10last_ms = 0;11active = false;12}1314// Update state, return true when first triggered15bool 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)16{17bool ret = false;1819if (trigger) {20last_ms = now_ms;21if (start_ms == 0) {22start_ms = now_ms;23}24if ((now_ms - start_ms) > trigger_delay_ms) {25// trigger delay has elapsed26if (!active) {27// return true on first trigger28ret = true;29}30active = true;31}3233} else if (active) {34if ((last_ms == 0) || ((now_ms - last_ms) > clear_delay_ms)) {35// Clear delay passed36reset();37}3839} else {40reset();41}4243return ret;44}4546// Assistance not needed, reset any state47void VTOL_Assist::reset()48{49force_assist = false;50speed_assist = false;51angle_error.reset();52alt_error.reset();53}5455/*56return true if the quadplane should provide stability assistance57*/58bool VTOL_Assist::should_assist(float aspeed, bool have_airspeed)59{60if (!plane.arming.is_armed_and_safety_off() || (state == STATE::ASSIST_DISABLED) || quadplane.tailsitter.is_control_surface_tailsitter()) {61// disarmed or disabled by aux switch or because a control surface tailsitter62reset();63return false;64}6566if (!quadplane.tailsitter.enabled() && !( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed)67|| is_positive(plane.get_throttle_input())68|| plane.is_flying() ) ) {69// not in a flight mode and condition where it would be safe to turn on vertial lift motors70// skip this check for tailsitters because the forward and vertial motors are the same and are controled directly by throttle imput unlike other quadplanes71reset();72return false;73}7475if (plane.flare_mode != Plane::FlareMode::FLARE_DISABLED) {76// Never active in fixed wing flare77reset();78return false;79}8081force_assist = state == STATE::FORCE_ENABLED;8283if (speed <= 0) {84// all checks disabled via speed threshold, still allow force enabled85speed_assist = false;86alt_error.reset();87angle_error.reset();88return force_assist;89}9091// assistance due to Q_ASSIST_SPEED92// if option bit is enabled only allow assist with real airspeed sensor93speed_assist = (have_airspeed && aspeed < speed) &&94(!quadplane.option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || plane.ahrs.using_airspeed_sensor());9596const uint32_t now_ms = AP_HAL::millis();97const uint32_t tigger_delay_ms = delay * 1000;98const uint32_t clear_delay_ms = tigger_delay_ms * 2;99100/*101optional assistance when altitude is too close to the ground102*/103if (alt <= 0) {104// Alt assist disabled105alt_error.reset();106107} else {108const float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);109if (alt_error.update(height_above_ground < alt, now_ms, tigger_delay_ms, clear_delay_ms)) {110gcs().send_text(MAV_SEVERITY_WARNING, "Alt assist %.1fm", height_above_ground);111}112}113114if (angle <= 0) {115// Angle assist disabled116angle_error.reset();117118} else {119120/*121now check if we should provide assistance due to attitude error122*/123const uint16_t allowed_envelope_error_cd = 500U;124const bool inside_envelope = (labs(plane.ahrs.roll_sensor) <= (plane.aparm.roll_limit*100 + allowed_envelope_error_cd)) &&125(plane.ahrs.pitch_sensor < (plane.aparm.pitch_limit_max*100 + allowed_envelope_error_cd)) &&126(plane.ahrs.pitch_sensor > (plane.aparm.pitch_limit_min*100 - allowed_envelope_error_cd));127128const int32_t max_angle_cd = 100U*angle;129const bool inside_angle_error = (labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) < max_angle_cd) &&130(labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) < max_angle_cd);131132if (angle_error.update(!inside_envelope && !inside_angle_error, now_ms, tigger_delay_ms, clear_delay_ms)) {133gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d",134(int)(plane.ahrs.roll_sensor/100),135(int)(plane.ahrs.pitch_sensor/100));136}137}138139return force_assist || speed_assist || alt_error.is_active() || angle_error.is_active();140}141142#endif // HAL_QUADPLANE_ENABLED143144145