Browse Source

Copter: add optflow_position_ok and use for loiter

This allows entering Loiter flight mode with only optical flow based
position
mission-4.1.18
Randy Mackay 10 years ago
parent
commit
fd0b82f669
  1. 2
      ArduCopter/control_loiter.pde
  2. 16
      ArduCopter/system.pde

2
ArduCopter/control_loiter.pde

@ -7,7 +7,7 @@ @@ -7,7 +7,7 @@
// loiter_init - initialise loiter controller
static bool loiter_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
if (position_ok() || optflow_position_ok() || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();

16
ArduCopter/system.pde

@ -349,6 +349,22 @@ static bool position_ok() @@ -349,6 +349,22 @@ static bool position_ok()
}
}
// optflow_position_ok - returns true if optical flow based position estimate is ok
static bool optflow_position_ok()
{
#if OPTFLOW != ENABLED
return false;
#else
// return immediately if optflow is not enabled
if (!optflow.enabled()) {
return false;
}
// get filter status from inertial nav or EKF
return inertial_nav.get_filter_status().flags.horiz_pos_rel;
#endif
}
// update_auto_armed - update status of auto_armed flag
static void update_auto_armed()
{

Loading…
Cancel
Save