Format whitespace and group checkall() methods together in geofence class.

src/modules/navigator/geofence.cpp

@@ -75,9 +75,7 @@ void Geofence::updateFence()
 {
 	// Note: be aware that when calling this, it can block for quite some time, the duration of a geofence transfer.
 	// However this is currently not used
-	int ret = dm_lock(DM_KEY_FENCE_POINTS);
-
-	if (ret != 0) {
+	if (dm_lock(DM_KEY_FENCE_POINTS) != 0) {
 		PX4_ERR("lock failed");
 		return;
 	}
@@ -88,7 +86,6 @@ void Geofence::updateFence()
 
 void Geofence::_updateFence()
 {
-
 	// initialize fence points count
 	mission_stats_entry_s stats;
 	int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
@@ -190,16 +187,41 @@ bool Geofence::checkAll(const struct vehicle_global_position_s &global_position,
 	return checkAll(global_position.lat, global_position.lon, alt);
 }
 
+bool Geofence::checkAll(double lat, double lon, float altitude)
+{
+	bool inside_fence = isCloserThanMaxDistToHome(lat, lon, altitude);
+
+	inside_fence = inside_fence && isBelowMaxAltitude(altitude);
+
+	// to be inside the geofence both fences have to report being inside
+	// as they both report being inside when not enabled
+	inside_fence = inside_fence && isInsidePolygonOrCircle(lat, lon, altitude);
+
+	if (inside_fence) {
+		_outside_counter = 0;
+		return inside_fence;
+
+	} else {
+		_outside_counter++;
+
+		if (_outside_counter > _param_gf_count.get()) {
+			return inside_fence;
+
+		} else {
+			return true;
+		}
+	}
+}
+
 bool Geofence::check(const vehicle_global_position_s &global_position, const vehicle_gps_position_s &gps_position,
 		     const home_position_s home_pos, bool home_position_set)
 {
-	if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
+	if (_param_gf_altmode.get() == Geofence::GF_ALT_MODE_WGS84) {
 		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
 			return checkAll(global_position);
 
 		} else {
-			return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
-					(double)gps_position.alt * 1.0e-3);
+			return checkAll(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, gps_position.alt * 1.0e-3);
 		}
 
 	} else {
@@ -211,7 +233,7 @@ bool Geofence::check(const vehicle_global_position_s &global_position, const veh
 			return checkAll(global_position, baro_altitude_amsl);
 
 		} else {
-			return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, baro_altitude_amsl);
+			return checkAll(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, baro_altitude_amsl);
 		}
 	}
 }
@@ -283,33 +305,6 @@ bool Geofence::isBelowMaxAltitude(float altitude)
 	return inside_fence;
 }
 
-
-bool Geofence::checkAll(double lat, double lon, float altitude)
-{
-	bool inside_fence = isCloserThanMaxDistToHome(lat, lon, altitude);
-
-	inside_fence = inside_fence && isBelowMaxAltitude(altitude);
-
-	// to be inside the geofence both fences have to report being inside
-	// as they both report being inside when not enabled
-	inside_fence = inside_fence && isInsidePolygonOrCircle(lat, lon, altitude);
-
-	if (inside_fence) {
-		_outside_counter = 0;
-		return inside_fence;
-
-	} else {
-		_outside_counter++;
-
-		if (_outside_counter > _param_gf_count.get()) {
-			return inside_fence;
-
-		} else {
-			return true;
-		}
-	}
-}
-
 bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
 {
 	// the following uses dm_read, so first we try to lock all items. If that fails, it (most likely) means
@@ -346,9 +341,9 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
 	bool inside_inclusion = false;
 	bool had_inclusion_areas = false;
 
-	for (int polygon_idx = 0; polygon_idx < _num_polygons; ++polygon_idx) {
-		if (_polygons[polygon_idx].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) {
-			bool inside = insideCircle(_polygons[polygon_idx], lat, lon, altitude);
+	for (int polygon_index = 0; polygon_index < _num_polygons; ++polygon_index) {
+		if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) {
+			bool inside = insideCircle(_polygons[polygon_index], lat, lon, altitude);
 
 			if (inside) {
 				inside_inclusion = true;
@@ -356,17 +351,17 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
 
 			had_inclusion_areas = true;
 
-		} else if (_polygons[polygon_idx].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
-			bool inside = insideCircle(_polygons[polygon_idx], lat, lon, altitude);
+		} else if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
+			bool inside = insideCircle(_polygons[polygon_index], lat, lon, altitude);
 
 			if (inside) {
 				outside_exclusion = false;
 			}
 
 		} else { // it's a polygon
-			bool inside = insidePolygon(_polygons[polygon_idx], lat, lon, altitude);
+			bool inside = insidePolygon(_polygons[polygon_index], lat, lon, altitude);
 
-			if (_polygons[polygon_idx].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) {
+			if (_polygons[polygon_index].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) {
 				if (inside) {
 					inside_inclusion = true;
 				}
@@ -388,15 +383,15 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude)
 
 bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude)
 {
-
-	/* Adaptation of algorithm originally presented as
+	/**
+	 * Adaptation of algorithm originally presented as
 	 * PNPOLY - Point Inclusion in Polygon Test
 	 * W. Randolph Franklin (WRF)
 	 * Only supports non-complex polygons (not self intersecting)
 	 */
 
-	mission_fence_point_s temp_vertex_i;
-	mission_fence_point_s temp_vertex_j;
+	mission_fence_point_s temp_vertex_i{};
+	mission_fence_point_s temp_vertex_j{};
 	bool c = false;
 
 	for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) {
@@ -431,7 +426,7 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon,
 bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude)
 {
 
-	mission_fence_point_s circle_point;
+	mission_fence_point_s circle_point{};
 
 	if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index, &circle_point,
 		    sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) {
@@ -472,7 +467,7 @@ Geofence::loadFromFile(const char *filename)
 	int pointCounter = 0;
 	bool gotVertical = false;
 	const char commentChar = '#';
-	int rc = PX4_ERROR;
+	int ret_val = PX4_ERROR;
 
 	/* Make sure no data is left in the datamanager */
 	clearDm();
@@ -508,7 +503,7 @@ Geofence::loadFromFile(const char *filename)
 
 		if (gotVertical) {
 			/* Parse the line as a geofence point */
-			mission_fence_point_s vertex;
+			mission_fence_point_s vertex{};
 			vertex.frame = NAV_FRAME_GLOBAL;
 			vertex.nav_cmd = NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION;
 			vertex.vertex_count = 0; // this will be filled in a second pass
@@ -561,7 +556,7 @@ Geofence::loadFromFile(const char *filename)
 	if (gotVertical && pointCounter > 2) {
 		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t");
 		events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported");
-		rc = PX4_OK;
+		ret_val = PX4_OK;
 
 		/* do a second pass, now that we know the number of vertices */
 		for (int seq = 1; seq <= pointCounter; ++seq) {
@@ -576,7 +571,7 @@ Geofence::loadFromFile(const char *filename)
 
 		mission_stats_entry_s stats;
 		stats.num_items = pointCounter;
-		rc = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+		ret_val = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
 
 	} else {
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t");
@@ -587,7 +582,7 @@ Geofence::loadFromFile(const char *filename)
 
 error:
 	fclose(fp);
-	return rc;
+	return ret_val;
 }
 
 int Geofence::clearDm()

src/modules/navigator/geofence.h

@@ -98,6 +98,13 @@ public:
 	 */
 	bool check(const struct mission_item_s &mission_item);
 
+	/**
+	 * Check if a point passes the Geofence test.
+	 * In addition to checkPolygons(), this takes all additional parameters into account.
+	 *
+	 * @return false for a geofence violation
+	 */
+	bool checkAll(double lat, double lon, float altitude);
 
 	bool isCloserThanMaxDistToHome(double lat, double lon, float altitude);
 
@@ -132,35 +139,20 @@ public:
 
 	bool isEmpty() { return _num_polygons == 0; }
 
-	int getAltitudeMode() { return _param_gf_altmode.get(); }
 	int getSource() { return _param_gf_source.get(); }
 	int getGeofenceAction() { return _param_gf_action.get(); }
+
 	float getMaxHorDistanceHome() { return _param_gf_max_hor_dist.get(); }
 	float getMaxVerDistanceHome() { return _param_gf_max_ver_dist.get(); }
 
 	bool isHomeRequired();
 
-	/**
-	 * Check if a point passes the Geofence test.
-	 * In addition to checkPolygons(), this takes all additional parameters into account.
-	 *
-	 * @return false for a geofence violation
-	 */
-	bool checkAll(double lat, double lon, float altitude);
-
 	/**
 	 * print Geofence status to the console
 	 */
 	void printStatus();
 
 private:
-	Navigator	*_navigator{nullptr};
-
-	hrt_abstime _last_horizontal_range_warning{0};
-	hrt_abstime _last_vertical_range_warning{0};
-
-	float _altitude_min{0.0f};
-	float _altitude_max{0.0f};
 
 	struct PolygonInfo {
 		uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* (can also be a circular region)
@@ -170,20 +162,20 @@ private:
 			float circle_radius;
 		};
 	};
+
+	Navigator   *_navigator{nullptr};
 	PolygonInfo *_polygons{nullptr};
+
+	hrt_abstime _last_horizontal_range_warning{0};
+	hrt_abstime _last_vertical_range_warning{0};
+
+	float _altitude_min{0.0f};
+	float _altitude_max{0.0f};
+
 	int _num_polygons{0};
 
 	MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m]
 
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::GF_ACTION>) _param_gf_action,
-		(ParamInt<px4::params::GF_ALTMODE>) _param_gf_altmode,
-		(ParamInt<px4::params::GF_SOURCE>) _param_gf_source,
-		(ParamInt<px4::params::GF_COUNT>) _param_gf_count,
-		(ParamFloat<px4::params::GF_MAX_HOR_DIST>) _param_gf_max_hor_dist,
-		(ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist
-	)
-
 	uORB::SubscriptionData<vehicle_air_data_s> _sub_airdata;
 
 	int _outside_counter{0};
@@ -223,4 +215,13 @@ private:
 	 * @return true if within polygon the circle
 	 */
 	bool insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude);
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::GF_ACTION>)         _param_gf_action,
+		(ParamInt<px4::params::GF_ALTMODE>)        _param_gf_altmode,
+		(ParamInt<px4::params::GF_SOURCE>)         _param_gf_source,
+		(ParamInt<px4::params::GF_COUNT>)          _param_gf_count,
+		(ParamFloat<px4::params::GF_MAX_HOR_DIST>) _param_gf_max_hor_dist,
+		(ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist
+	)
 };