mirror of https://github.com/ArduPilot/ardupilot
Copter: add collective stick low flag to catch bad setups for land detector
This commit is contained in:
parent
d2c37daa66
commit
8ef88fb98e
|
@ -553,6 +553,7 @@ private:
|
||||||
uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms)
|
uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms)
|
||||||
uint8_t inverted_flight : 1; // 1 // true for inverted flight mode
|
uint8_t inverted_flight : 1; // 1 // true for inverted flight mode
|
||||||
uint8_t in_autorotation : 1; // 2 // true when heli is in autorotation
|
uint8_t in_autorotation : 1; // 2 // true when heli is in autorotation
|
||||||
|
bool coll_stk_low ; // 3 // true when collective stick is on lower limit
|
||||||
} heli_flags_t;
|
} heli_flags_t;
|
||||||
heli_flags_t heli_flags;
|
heli_flags_t heli_flags;
|
||||||
|
|
||||||
|
@ -761,6 +762,7 @@ private:
|
||||||
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
#if MODE_AUTOROTATE_ENABLED == ENABLED
|
||||||
void heli_set_autorotation(bool autotrotation);
|
void heli_set_autorotation(bool autotrotation);
|
||||||
#endif
|
#endif
|
||||||
|
void update_collective_low_flag(int16_t throttle_control);
|
||||||
// inertia.cpp
|
// inertia.cpp
|
||||||
void read_inertia();
|
void read_inertia();
|
||||||
|
|
||||||
|
|
|
@ -136,6 +136,7 @@ bool Copter::should_use_landing_swash() const
|
||||||
void Copter::heli_update_landing_swash()
|
void Copter::heli_update_landing_swash()
|
||||||
{
|
{
|
||||||
motors->set_collective_for_landing(should_use_landing_swash());
|
motors->set_collective_for_landing(should_use_landing_swash());
|
||||||
|
update_collective_low_flag(channel_throttle->get_control_in());
|
||||||
}
|
}
|
||||||
|
|
||||||
// convert motor interlock switch's position to desired rotor speed expressed as a value from 0 to 1
|
// convert motor interlock switch's position to desired rotor speed expressed as a value from 0 to 1
|
||||||
|
@ -237,4 +238,19 @@ void Copter::heli_set_autorotation(bool autorotation)
|
||||||
motors->set_in_autorotation(autorotation);
|
motors->set_in_autorotation(autorotation);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// update collective low flag. Use a debounce time of 400 milliseconds.
|
||||||
|
void Copter::update_collective_low_flag(int16_t throttle_control)
|
||||||
|
{
|
||||||
|
static uint32_t last_nonzero_collective_ms = 0;
|
||||||
|
uint32_t tnow_ms = millis();
|
||||||
|
|
||||||
|
if (throttle_control > 0) {
|
||||||
|
last_nonzero_collective_ms = tnow_ms;
|
||||||
|
heli_flags.coll_stk_low = false;
|
||||||
|
} else if (tnow_ms - last_nonzero_collective_ms > 400) {
|
||||||
|
heli_flags.coll_stk_low = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
#endif // FRAME_CONFIG == HELI_FRAME
|
#endif // FRAME_CONFIG == HELI_FRAME
|
||||||
|
|
|
@ -61,11 +61,12 @@ void Copter::update_land_detector()
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
#if FRAME_CONFIG == HELI_FRAME
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
// check for both manual collective modes and modes that use altitude hold. For manual collective (called throttle
|
// check for both manual collective modes and modes that use altitude hold. For manual collective (called throttle
|
||||||
// because multi's use throttle), check that collective pitch is below mid collective (zero thrust) position. For modes
|
// because multi's use throttle), check that collective pitch is below land min collective position or throttle stick is zero.
|
||||||
|
// Including the throttle zero check will ensure the conditions where stabilize stick zero position was not below collective min. For modes
|
||||||
// that use altitude hold, check that the pilot is commanding a descent and collective is at min allowed for altitude hold modes.
|
// that use altitude hold, check that the pilot is commanding a descent and collective is at min allowed for altitude hold modes.
|
||||||
bool motor_at_lower_limit = ((flightmode->has_manual_throttle() && motors->get_below_land_min_coll() && fabsf(ahrs.get_roll()) < M_PI/2.0f)
|
bool motor_at_lower_limit = ((flightmode->has_manual_throttle() && (motors->get_below_land_min_coll() || heli_flags.coll_stk_low) &&
|
||||||
|| (motors->limit.throttle_lower && pos_control->get_vel_desired_cms().z < 0.0f));
|
fabsf(ahrs.get_roll()) < M_PI/2.0f) || (motors->limit.throttle_lower && pos_control->get_vel_desired_cms().z < 0.0f));
|
||||||
#else
|
#else
|
||||||
// check that the average throttle output is near minimum (less than 12.5% hover throttle)
|
// check that the average throttle output is near minimum (less than 12.5% hover throttle)
|
||||||
bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min();
|
bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min();
|
||||||
|
|
Loading…
Reference in New Issue