Skip to content

Commit 8ff846d

Browse files
committed
Blimp: remove far_from_EKF_origin sanity checks
some flawed implementations, and the extreme-ardupilot project means these checks are no longer required
1 parent 6b90dc5 commit 8ff846d

File tree

2 files changed

+0
-20
lines changed

2 files changed

+0
-20
lines changed

Blimp/Blimp.h

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -312,7 +312,6 @@ class Blimp : public AP_Vehicle
312312
void set_home_to_current_location_inflight();
313313
bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
314314
bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
315-
bool far_from_EKF_origin(const Location& loc);
316315

317316
// ekf_check.cpp
318317
void ekf_check();

Blimp/commands.cpp

Lines changed: 0 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -58,11 +58,6 @@ bool Blimp::set_home(const Location& loc, bool lock)
5858
return false;
5959
}
6060

61-
// check home is close to EKF origin
62-
if (far_from_EKF_origin(loc)) {
63-
return false;
64-
}
65-
6661
// set ahrs home (used for RTL)
6762
if (!ahrs.set_home(loc)) {
6863
return false;
@@ -76,17 +71,3 @@ bool Blimp::set_home(const Location& loc, bool lock)
7671
// return success
7772
return true;
7873
}
79-
80-
// far_from_EKF_origin - checks if a location is too far from the EKF origin
81-
// returns true if too far
82-
bool Blimp::far_from_EKF_origin(const Location& loc)
83-
{
84-
// check distance to EKF origin
85-
Location ekf_origin;
86-
if (ahrs.get_origin(ekf_origin) && ((ekf_origin.get_distance(loc) > EKF_ORIGIN_MAX_DIST_M) || (labs(ekf_origin.alt - loc.alt) > EKF_ORIGIN_MAX_DIST_M))) {
87-
return true;
88-
}
89-
90-
// close enough to origin
91-
return false;
92-
}

0 commit comments

Comments
 (0)