diff --git a/src/integration_tests/mission.cpp b/src/integration_tests/mission.cpp index 1e2cd4a689..4e10894850 100644 --- a/src/integration_tests/mission.cpp +++ b/src/integration_tests/mission.cpp @@ -286,7 +286,7 @@ Mission::MissionItem add_mission_item( float gimbal_pitch_deg, float gimbal_yaw_deg, float loiter_time_s, - float acceptance_radius_m + float acceptance_radius_m, Mission::MissionItem::CameraAction camera_action) { Mission::MissionItem new_item{}; @@ -299,7 +299,7 @@ Mission::MissionItem add_mission_item( new_item.gimbal_yaw_deg = gimbal_yaw_deg; new_item.loiter_time_s = loiter_time_s; new_item.camera_action = camera_action; - new_item.acceptance_radius_m = camera_action; + new_item.acceptance_radius_m = acceptance_radius_m; // In order to test setting the interval, add it here. if (camera_action == Mission::MissionItem::CameraAction::StartPhotoInterval) { diff --git a/src/plugins/mission/include/plugins/mission/mission.h b/src/plugins/mission/include/plugins/mission/mission.h index e261955b49..66cfe7225a 100644 --- a/src/plugins/mission/include/plugins/mission/mission.h +++ b/src/plugins/mission/include/plugins/mission/mission.h @@ -100,7 +100,8 @@ class Mission : public PluginBase { float loiter_time_s{float(NAN)}; /**< @brief Loiter time (in seconds) */ double camera_photo_interval_s{ 1.0}; /**< @brief Camera photo interval to use after this mission item (in seconds) */ - float acceptance_radius_m{float(NAN)}; /**< @brief Radius for completing a mission item (in metres) */ + float acceptance_radius_m{ + float(NAN)}; /**< @brief Radius for completing a mission item (in metres) */ }; /** diff --git a/src/plugins/mission/mission.cpp b/src/plugins/mission/mission.cpp index d31572ba3b..7add56ed63 100644 --- a/src/plugins/mission/mission.cpp +++ b/src/plugins/mission/mission.cpp @@ -152,7 +152,9 @@ bool operator==(const Mission::MissionItem& lhs, const Mission::MissionItem& rhs ((std::isnan(rhs.loiter_time_s) && std::isnan(lhs.loiter_time_s)) || rhs.loiter_time_s == lhs.loiter_time_s) && ((std::isnan(rhs.camera_photo_interval_s) && std::isnan(lhs.camera_photo_interval_s)) || - rhs.camera_photo_interval_s == lhs.camera_photo_interval_s); + rhs.camera_photo_interval_s == lhs.camera_photo_interval_s) && + ((std::isnan(rhs.acceptance_radius_m) && std::isnan(lhs.acceptance_radius_m)) || + rhs.acceptance_radius_m == lhs.acceptance_radius_m); } std::ostream& operator<<(std::ostream& str, Mission::MissionItem const& mission_item) @@ -169,6 +171,7 @@ std::ostream& operator<<(std::ostream& str, Mission::MissionItem const& mission_ str << " camera_action: " << mission_item.camera_action << '\n'; str << " loiter_time_s: " << mission_item.loiter_time_s << '\n'; str << " camera_photo_interval_s: " << mission_item.camera_photo_interval_s << '\n'; + str << " acceptance_radius_m: " << mission_item.acceptance_radius_m << '\n'; str << '}'; return str; } diff --git a/src/plugins/mission/mission_impl.cpp b/src/plugins/mission/mission_impl.cpp index a31c1f248a..30e657e38c 100644 --- a/src/plugins/mission/mission_impl.cpp +++ b/src/plugins/mission/mission_impl.cpp @@ -269,8 +269,7 @@ float MissionImpl::acceptance_radius(const MissionItem& item) float acceptance_radius_m; if (std::isfinite(item.acceptance_radius_m)) { acceptance_radius_m = item.acceptance_radius_m; - } - else if (item.is_fly_through) { + } else if (item.is_fly_through) { // _acceptance_radius_m is 0, determine the radius using fly_through acceptance_radius_m = 3.0f; } else {