Browse Source

Plane: fixed is_flying() for VTOL flight

otherwise we may disarm mid-flight!
master
Andrew Tridgell 9 years ago
parent
commit
0aa1ae048d
  1. 3
      ArduPlane/is_flying.cpp
  2. 9
      ArduPlane/quadplane.cpp
  3. 3
      ArduPlane/quadplane.h

3
ArduPlane/is_flying.cpp

@ -170,6 +170,9 @@ void Plane::update_is_flying_5Hz(void)
bool Plane::is_flying(void) bool Plane::is_flying(void)
{ {
if (hal.util->get_soft_armed()) { if (hal.util->get_soft_armed()) {
if (quadplane.is_flying_vtol()) {
return true;
}
// when armed, assume we're flying unless we probably aren't // when armed, assume we're flying unless we probably aren't
return (isFlyingProbability >= 0.1f); return (isFlyingProbability >= 0.1f);
} }

9
ArduPlane/quadplane.cpp

@ -553,6 +553,15 @@ bool QuadPlane::should_relax(void)
return relax_loiter; return relax_loiter;
} }
// see if we are flying in vtol
bool QuadPlane::is_flying_vtol(void)
{
if (in_vtol_mode() && millis() - motors_lower_limit_start_ms > 5000) {
return true;
}
return false;
}
/* /*
smooth out descent rate for landing to prevent a jerk as we get to smooth out descent rate for landing to prevent a jerk as we get to
land_final_alt. land_final_alt.

3
ArduPlane/quadplane.h

@ -63,6 +63,9 @@ public:
int8_t forward_throttle_pct(void); int8_t forward_throttle_pct(void);
float get_weathervane_yaw_rate_cds(void); float get_weathervane_yaw_rate_cds(void);
// see if we are flying from vtol point of view
bool is_flying_vtol(void);
struct PACKED log_QControl_Tuning { struct PACKED log_QControl_Tuning {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;

Loading…
Cancel
Save