Skip to content

Commit 7d84a81

Browse files
committed
AP_Common: catch invalid combination of terrain_alt and relative_alt
1 parent 6a974e8 commit 7d84a81

File tree

1 file changed

+8
-0
lines changed

1 file changed

+8
-0
lines changed

libraries/AP_Common/Location.cpp

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -90,6 +90,11 @@ bool Location::change_alt_frame(AltFrame desired_frame)
9090
Location::AltFrame Location::get_alt_frame() const
9191
{
9292
if (terrain_alt) {
93+
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
94+
if (!relative_alt) {
95+
AP_HAL::panic("terrain loc must be relative_alt1");
96+
}
97+
#endif
9398
return AltFrame::ABOVE_TERRAIN;
9499
}
95100
if (origin_alt) {
@@ -108,6 +113,9 @@ bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const
108113
if (!initialised()) {
109114
AP_HAL::panic("Should not be called on invalid location: Location cannot be (0, 0, 0)");
110115
}
116+
if (terrain_alt && !relative_alt) {
117+
AP_HAL::panic("terrain loc must be relative_alt2");
118+
}
111119
#endif
112120
Location::AltFrame frame = get_alt_frame();
113121

0 commit comments

Comments
 (0)