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

Plane: Quadplane: move Q Assist check into new VTOL assist files.

This commit is contained in:
Iampete1 2024-03-07 23:20:36 +00:00 committed by Andrew Tridgell
parent 8df49d72f3
commit 2ee5cdd792
7 changed files with 223 additions and 209 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -143,6 +143,7 @@ public:
friend class Tiltrotor;
friend class SLT_Transition;
friend class Tailsitter_Transition;
friend class VTOL_Assist;
friend class Mode;
friend class ModeCircle;

6
ArduPlane/RC_Channel.cpp
View File

@ -61,17 +61,17 @@ void RC_Channel_Plane::do_aux_function_q_assist_state(AuxSwitchPos ch_flag)
switch(ch_flag) {
case AuxSwitchPos::HIGH:
gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Force enabled");
plane.quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::FORCE_ENABLED);
plane.quadplane.assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED);
break;
case AuxSwitchPos::MIDDLE:
gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Enabled");
plane.quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::ASSIST_ENABLED);
plane.quadplane.assist.set_state(VTOL_Assist::STATE::ASSIST_ENABLED);
break;
case AuxSwitchPos::LOW:
gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Disabled");
plane.quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::ASSIST_DISABLED);
plane.quadplane.assist.set_state(VTOL_Assist::STATE::ASSIST_DISABLED);
break;
}
}

143
ArduPlane/VTOL_Assist.cpp Normal file
View File

@ -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

72
ArduPlane/VTOL_Assist.h Normal file
View File

@ -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;
};

137
ArduPlane/quadplane.cpp
View File

@ -1451,143 +1451,6 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const
return yaw_rate;
}
// Assistance not needed, reset any state
void QuadPlane::VTOL_Assist::reset()
{
force_assist = false;
speed_assist = false;
angle_error.reset();
alt_error.reset();
}
// Assistance hysteresis helpers
// Reset state
void QuadPlane::VTOL_Assist::Assist_Hysteresis::reset()
{
start_ms = 0;
last_ms = 0;
active = false;
}
bool QuadPlane::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;
}
/*
return true if the quadplane should provide stability assistance
*/
bool QuadPlane::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();
}
/*
update for transition from quadplane to fixed wing mode
*/

71
ArduPlane/quadplane.h
View File

@ -26,6 +26,7 @@
#include "tailsitter.h"
#include "tiltrotor.h"
#include "transition.h"
#include "VTOL_Assist.h"
/*
QuadPlane specific functionality
@ -45,6 +46,7 @@ public:
friend class Tiltrotor;
friend class SLT_Transition;
friend class Tailsitter_Transition;
friend class VTOL_Assist;
friend class Mode;
friend class ModeManual;
@ -326,75 +328,8 @@ private:
AP_Int16 rc_speed;
// VTOL assistance in a forward flight mode
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;
} assist {*this};
VTOL_Assist assist {*this};
// landing speed in m/s
AP_Float land_final_speed;

2
ArduPlane/tailsitter.cpp
View File

@ -233,7 +233,7 @@ void Tailsitter::setup()
// Setup for control surface less operation
if (enable == 2) {
quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::FORCE_ENABLED);
quadplane.assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED);
quadplane.air_mode = AirMode::ASSISTED_FLIGHT_ONLY;
// Do not allow arming in forward flight modes