Merge branch 'navigator_new' of github.com:PX4/Firmware into navigator_new

src/lib/launchdetection/LaunchDetector.cpp

@@ -43,7 +43,8 @@
 #include <systemlib/err.h>
 
 LaunchDetector::LaunchDetector() :
-	launchdetection_on(NULL, "LAUN_ALL_ON", false)
+	launchdetection_on(NULL, "LAUN_ALL_ON", false),
+	throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
 {
 	/* init all detectors */
 	launchMethods[0] = new CatapultLaunchMethod();
@@ -83,10 +84,11 @@ bool LaunchDetector::getLaunchDetected()
 void LaunchDetector::updateParams() {
 
 	warnx(" LaunchDetector::updateParams()");
-	//launchdetection_on.update();
+	launchdetection_on.update();
+	throttlePreTakeoff.update();
 
 	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
-		//launchMethods[i]->updateParams();
+		launchMethods[i]->updateParams();
 		warnx("updating component %d", i);
 	}
 

src/lib/launchdetection/LaunchDetector.h

@@ -59,11 +59,14 @@ public:
 	void updateParams();
 	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
 
+	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
+
 //	virtual bool getLaunchDetected();
 protected:
 private:
 	LaunchMethod* launchMethods[1];
 	control::BlockParamInt launchdetection_on;
+	control::BlockParamFloat throttlePreTakeoff;
 
 
 };

src/lib/launchdetection/launchdetection_params.c

@@ -33,11 +33,11 @@
  ****************************************************************************/
 
 /**
- * @file fw_pos_control_l1_params.c
+ * @file launchdetection_params.c
  *
- * Parameters defined by the L1 position control task
+ * Parameters for launchdetection
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -65,3 +65,8 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
 // @Description		LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection
 // @Range		> 0, in seconds
 PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
+
+// @DisplayName		Throttle setting while detecting the launch
+// @Description		The throttle is set to this value while the system is waiting for the takeoff
+// @Range		0 to 1
+PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -174,6 +174,7 @@ private:
 
 	/* takeoff/launch states */
 	bool launch_detected;
+	bool usePreTakeoffThrust;
 	bool launch_detection_message_sent;
 
 	/* Landingslope object */
@@ -396,6 +397,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_mavlink_fd(-1),
 	launchDetector(),
 	launch_detected(false),
+	usePreTakeoffThrust(false),
 	launch_detection_message_sent(false)
 {
 	/* safely initialize structs */
@@ -1004,6 +1006,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
 			if (launch_detected) {
+				usePreTakeoffThrust = false;
 
 				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
 				if (altitude_error > 15.0f) {
@@ -1028,7 +1031,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				}
 
 			} else {
-				throttle_max = 0.0f;
+				usePreTakeoffThrust = true;
 			}
 		}
 
@@ -1053,6 +1056,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		/* reset takeoff/launch state */
 		if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
 			launch_detected = false;
+			usePreTakeoffThrust = false;
 			launch_detection_message_sent = false;
 		}
 
@@ -1150,8 +1154,14 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		setpoint = false;
 	}
 
+	if (usePreTakeoffThrust) {
+		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
+	}
+	else {
+		_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
+	}
 	_att_sp.pitch_body = _tecs.get_pitch_demand();
-	_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
+
 
 	return setpoint;
 }

src/modules/navigator/geofence.cpp

@@ -57,9 +57,12 @@ static const int ERROR = -1;
 
 Geofence::Geofence() : _fence_pub(-1),
 		_altitude_min(0),
-		_altitude_max(0)
+		_altitude_max(0),
+		_verticesCount(0),
+		param_geofence_on(NULL, "GF_ON", false)
 {
-	memset(&_fence, 0, sizeof(_fence));
+	/* Load initial params */
+	updateParams();
 }
 
 Geofence::~Geofence()
@@ -70,57 +73,75 @@ Geofence::~Geofence()
 
 bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
 {
-
-	/* Adaptation of algorithm originally presented as
-	 * PNPOLY - Point Inclusion in Polygon Test
-	 * W. Randolph Franklin (WRF) */
-
-	unsigned int i, j, vertices = _fence.count;
-	bool c = false;
 	double lat = vehicle->lat / 1e7d;
 	double lon = vehicle->lon / 1e7d;
+	float	alt = vehicle->alt;
 
-	// skip vertex 0 (return point)
-	for (i = 0, j = vertices - 1; i < vertices; j = i++)
-		if (((_fence.vertices[i].lon) >= lon != (_fence.vertices[j].lon >= lon)) &&
-		    (lat <= (_fence.vertices[j].lat - _fence.vertices[i].lat) * (lon - _fence.vertices[i].lon) /
-		     (_fence.vertices[j].lon - _fence.vertices[i].lon) + _fence.vertices[i].lat))
-			c = !c;
-	return c;
+	return inside(lat, lon, vehicle->alt);
 }
 
-bool
-Geofence::loadFromDm(unsigned vertices)
+bool Geofence::inside(double lat, double lon, float altitude)
 {
-	struct fence_s temp_fence;
+	/* Return true if geofence is disabled */
+	if (param_geofence_on.get() != 1)
+		return true;
 
-	unsigned i;
-	for (i = 0; i < vertices; i++) {
-		if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
-			break;
+	if (valid()) {
+
+		if (!isEmpty()) {
+			/* Vertical check */
+			if (altitude > _altitude_max || altitude < _altitude_min)
+				return false;
+
+			/*Horizontal check */
+			/* Adaptation of algorithm originally presented as
+			 * PNPOLY - Point Inclusion in Polygon Test
+			 * W. Randolph Franklin (WRF) */
+
+			bool c = false;
+
+			struct fence_vertex_s temp_vertex_i;
+			struct fence_vertex_s temp_vertex_j;
+
+			/* Red until fence is finished */
+			for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
+				if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+					break;
+				}
+				if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+					break;
+				}
+
+				// skip vertex 0 (return point)
+				if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
+							(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
+							 (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
+							c = !c;
+				}
+
+			}
+
+			return c;
+		} else {
+			/* Empty fence --> accept all points */
+			return true;
 		}
+
+	} else {
+		/* Invalid fence --> accept all points */
+		return true;
 	}
-
-	temp_fence.count = i;
-
-	if (valid())
-		memcpy(&_fence, &temp_fence, sizeof(_fence));
-	else
-		warnx("Invalid fence file, ignored!");
-
-	return _fence.count != 0;
 }
 
 bool
 Geofence::valid()
 {
 	// NULL fence is valid
-	if (_fence.count == 0) {
+	if (isEmpty())
 		return true;
-	}
 
 	// Otherwise
-	if ((_fence.count < 4) || (_fence.count > GEOFENCE_MAX_VERTICES)) {
+	if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
 		warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
 		return false;
 	}
@@ -214,12 +235,28 @@ Geofence::loadFromFile(const char *filename)
 			/* Parse the line as a geofence point */
 			struct fence_vertex_s vertex;
 
-			if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
-				return ERROR;
+			/* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */
+			if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') {
+				/* Handle degree minute second format */
+				float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
 
+				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
+					return ERROR;
+
+//				warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
+
+				vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f;
+				vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f;
+
+			} else {
+				/* Handle decimal degree format */
+
+				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
+					return ERROR;
+			}
 
 			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
-				return ERROR;
+								return ERROR;
 
 			warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter,  (double)vertex.lat, (double)vertex.lon);
 
@@ -239,17 +276,11 @@ Geofence::loadFromFile(const char *filename)
 
 	fclose(fp);
 
-	/* Re-Load imported geofence from DM */
+	/* Check if import was successful */
 	if(gotVertical && pointCounter > 0)
 	{
-		bool fence_valid = loadFromDm(GEOFENCE_MAX_VERTICES);
-		if (fence_valid) {
-			warnx("Geofence: imported and loaded successfully");
-			return OK;
-		} else {
-			warnx("Geofence: datamanager read error");
-			return ERROR;
-		}
+		_verticesCount = pointCounter;
+		warnx("Geofence: imported successfully");
 	} else {
 		warnx("Geofence: import error");
 	}
@@ -261,3 +292,8 @@ int Geofence::clearDm()
 {
 	dm_clear(DM_KEY_FENCE_POINTS);
 }
+
+void Geofence::updateParams()
+{
+	param_geofence_on.update();
+}

src/modules/navigator/geofence.h

@@ -41,16 +41,21 @@
 #define GEOFENCE_H_
 
 #include <uORB/topics/fence.h>
+#include <controllib/block/BlockParam.hpp>
 
 #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
 
 class Geofence {
 private:
-	struct fence_s	_fence;
 	orb_advert_t	_fence_pub;			/**< publish fence topic */
 
 	float			_altitude_min;
 	float			_altitude_max;
+
+	unsigned 			_verticesCount;
+
+	/* Params */
+	control::BlockParamInt param_geofence_on;
 public:
 	Geofence();
 	~Geofence();
@@ -64,12 +69,7 @@ public:
 	 * @return true: craft is inside fence, false:craft is outside fence
 	 */
 	bool inside(const struct vehicle_global_position_s *craft);
-
-
-	/**
-	 * Load fence parameters.
-	 */
-	bool loadFromDm(unsigned vertices);
+	bool inside(double lat, double lon, float altitude);
 
 	int clearDm();
 
@@ -83,6 +83,10 @@ public:
 	void publishFence(unsigned vertices);
 
 	int loadFromFile(const char *filename);
+
+	bool isEmpty() {return _verticesCount == 0;}
+
+	void updateParams();
 };
 
 

src/modules/navigator/geofence_params.c

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file geofence_params.c
+ *
+ * Parameters for geofence
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * geofence parameters, accessible via MAVLink
+ *
+ */
+
+// @DisplayName		Switch to enable geofence
+// @Description		if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present
+// @Range		0 or 1
+PARAM_DEFINE_INT32(GF_ON, 1);

src/modules/navigator/mission_feasibility_checker.cpp

@@ -97,7 +97,24 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
 
 bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
 {
-	//xxx: check geofence
+	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
+	if (geofence.valid()) {
+		for (size_t i = 0; i < nMissionItems; i++) {
+			static struct mission_item_s missionitem;
+			const ssize_t len = sizeof(struct mission_item_s);
+
+			if (dm_read(dm_current, i, &missionitem, len) != len) {
+				/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+				return false;
+			}
+
+			if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude
+				mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
+				return false;
+			}
+		}
+	}
+
 	return true;
 }
 

src/modules/navigator/module.mk

@@ -41,6 +41,7 @@ SRCS		= navigator_main.cpp \
 		  navigator_params.c \
 		  navigator_mission.cpp \
 		  mission_feasibility_checker.cpp \
-		  geofence.cpp
+		  geofence.cpp \
+		  geofence_params.c
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink

src/modules/navigator/navigator_main.cpp

@@ -160,6 +160,8 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 	
 	Geofence					_geofence;
+	bool						_geofence_violation_warning_sent;
+
 	bool						_fence_valid;		/**< flag if fence is valid */
         bool						_inside_fence;		/**< vehicle is inside fence */
 
@@ -393,7 +395,8 @@ Navigator::Navigator() :
 	_time_first_inside_orbit(0),
 	_set_nav_state_timestamp(0),
 	_need_takeoff(true),
-	_do_takeoff(false)
+	_do_takeoff(false),
+	_geofence_violation_warning_sent(false)
 {
 	_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
 	_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
@@ -458,6 +461,8 @@ Navigator::parameters_update()
 	param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt));
 
 	_mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled);
+
+	_geofence.updateParams();
 }
 
 void
@@ -547,8 +552,6 @@ Navigator::task_main()
 
 	mavlink_log_info(_mavlink_fd, "[navigator] started");
 
-	_fence_valid = _geofence.loadFromDm(GEOFENCE_MAX_VERTICES);
-
 	/* Try to load the geofence:
 	 * if /fs/microsd/etc/geofence.txt load from this file
 	 * else clear geofence data in datamanager
@@ -766,6 +769,21 @@ Navigator::task_main()
 					on_mission_item_reached();
 				}
 			}
+
+			/* Check geofence violation */
+			if(!_geofence.inside(&_global_pos)) {
+				//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)
+
+				/* Issue a warning about the geofence violation once */
+				if (!_geofence_violation_warning_sent)
+				{
+					mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
+					_geofence_violation_warning_sent = true;
+				}
+			} else {
+				/* Reset the _geofence_violation_warning_sent field */
+				_geofence_violation_warning_sent = false;
+			}
 		}
 
 		/* notify user about state changes */
@@ -1515,7 +1533,7 @@ void Navigator::load_fence_from_file(const char *filename)
 
 static void usage()
 {
-	errx(1, "usage: navigator {start|stop|status|fence}");
+	errx(1, "usage: navigator {start|stop|status|fence|fencefile}");
 }
 
 int navigator_main(int argc, char *argv[])