Merge remote-tracking branch 'upstream/master' into obcfailsafe

Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

src/lib/launchdetection/CatapultLaunchMethod.cpp

@@ -49,9 +49,10 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
 	SuperBlock(parent, "CAT"),
 	last_timestamp(hrt_absolute_time()),
 	integrator(0.0f),
-	launchDetected(false),
-	threshold_accel(this, "A"),
-	threshold_time(this, "T")
+	state(LAUNCHDETECTION_RES_NONE),
+	thresholdAccel(this, "A"),
+	thresholdTime(this, "T"),
+	motorDelay(this, "MDEL")
 {
 
 }
@@ -65,34 +66,56 @@ void CatapultLaunchMethod::update(float accel_x)
 	float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
 	last_timestamp = hrt_absolute_time();
 
-	if (accel_x > threshold_accel.get()) {
-		integrator += dt;
-//		warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
-//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
-		if (integrator > threshold_time.get()) {
-			launchDetected = true;
+	switch (state) {
+	case LAUNCHDETECTION_RES_NONE:
+		/* Detect a acceleration that is longer and stronger as the minimum given by the params */
+		if (accel_x > thresholdAccel.get()) {
+			integrator += dt;
+			if (integrator > thresholdTime.get()) {
+				if (motorDelay.get() > 0.0f) {
+					state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
+					warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
+							" throttle", (double)motorDelay.get());
+				} else {
+					/* No motor delay set: go directly to enablemotors state */
+					state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+					warnx("Launch detected: state: enablemotors (delay not activated)");
+				}
+			}
+		} else {
+			/* reset */
+			reset();
 		}
+		break;
+
+	case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
+		/* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is
+		 * over to allow full throttle */
+		motorDelayCounter += dt;
+		if (motorDelayCounter > motorDelay.get()) {
+			warnx("Launch detected: state enablemotors");
+			state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+		}
+		break;
+
+	default:
+		break;
 
-	} else {
-//		warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
-//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
-		/* reset integrator */
-		integrator = 0.0f;
-		launchDetected = false;
 	}
 
 }
 
-bool CatapultLaunchMethod::getLaunchDetected()
+LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const
 {
-	return launchDetected;
+	return state;
 }
 
 
 void CatapultLaunchMethod::reset()
 {
 	integrator = 0.0f;
-	launchDetected = false;
+	motorDelayCounter = 0.0f;
+	state = LAUNCHDETECTION_RES_NONE;
 }
 
 }

src/lib/launchdetection/CatapultLaunchMethod.h

@@ -57,16 +57,19 @@ public:
 	~CatapultLaunchMethod();
 
 	void update(float accel_x);
-	bool getLaunchDetected();
+	LaunchDetectionResult getLaunchDetected() const;
 	void reset();
 
 private:
 	hrt_abstime last_timestamp;
 	float integrator;
-	bool launchDetected;
+	float motorDelayCounter;
 
-	control::BlockParamFloat threshold_accel;
-	control::BlockParamFloat threshold_time;
+	LaunchDetectionResult state;
+
+	control::BlockParamFloat thresholdAccel;
+	control::BlockParamFloat thresholdTime;
+	control::BlockParamFloat motorDelay;
 
 };
 

src/lib/launchdetection/LaunchDetector.cpp

@@ -46,6 +46,7 @@ namespace launchdetection
 
 LaunchDetector::LaunchDetector() :
 	SuperBlock(NULL, "LAUN"),
+	activeLaunchDetectionMethodIndex(-1),
 	launchdetection_on(this, "ALL_ON"),
 	throttlePreTakeoff(this, "THR_PRE")
 {
@@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector()
 void LaunchDetector::reset()
 {
 	/* Reset all detectors */
-	launchMethods[0]->reset();
+	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+		launchMethods[i]->reset();
+	}
+
+	/* Reset active launchdetector */
+	activeLaunchDetectionMethodIndex  = -1;
+
+
 }
 
 void LaunchDetector::update(float accel_x)
@@ -77,17 +85,24 @@ void LaunchDetector::update(float accel_x)
 	}
 }
 
-bool LaunchDetector::getLaunchDetected()
+LaunchDetectionResult LaunchDetector::getLaunchDetected()
 {
 	if (launchdetection_on.get() == 1) {
-		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
-			if(launchMethods[i]->getLaunchDetected()) {
-				return true;
+		if (activeLaunchDetectionMethodIndex < 0) {
+		/* None of the active launchmethods has detected a launch, check all launchmethods */
+			for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+				if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
+					warnx("selecting launchmethod %d", i);
+					activeLaunchDetectionMethodIndex = i; // from now on only check this method
+					return launchMethods[i]->getLaunchDetected();
+				}
 			}
+		} else {
+			return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
 		}
 	}
 
-	return false;
+	return LAUNCHDETECTION_RES_NONE;
 }
 
 }

src/lib/launchdetection/LaunchDetector.h

@@ -59,7 +59,7 @@ public:
 	void reset();
 
 	void update(float accel_x);
-	bool getLaunchDetected();
+	LaunchDetectionResult getLaunchDetected();
 	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
 
 	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@@ -67,6 +67,12 @@ public:
 //	virtual bool getLaunchDetected();
 protected:
 private:
+	int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods
+					       which detected a Launch. If no launchMethod has detected a launch yet the
+					       value is -1. Once one launchMetthod has detected a launch only this
+					       method is checked for further adavancing in the state machine (e.g. when
+					       to power up the motors) */
+
 	LaunchMethod* launchMethods[1];
 	control::BlockParamInt launchdetection_on;
 	control::BlockParamFloat throttlePreTakeoff;

src/lib/launchdetection/LaunchMethod.h

@@ -44,11 +44,22 @@
 namespace launchdetection
 {
 
+enum LaunchDetectionResult {
+	LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */
+	LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should
+							  control the attitude. However any motors should not throttle
+							  up and still be set to 'throttlePreTakeoff'.
+							  For instance this is used to have a delay for the motor
+							  when launching a fixed wing aircraft from a bungee */
+	LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control
+							attitude and also throttle up the motors. */
+};
+
 class LaunchMethod
 {
 public:
 	virtual void update(float accel_x) = 0;
-	virtual bool getLaunchDetected() = 0;
+	virtual LaunchDetectionResult getLaunchDetected() const = 0;
 	virtual void reset() = 0;
 
 protected:

src/lib/launchdetection/launchdetection_params.c

@@ -77,6 +77,17 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
  */
 PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
 
+/**
+ * Motor delay
+ *
+ * Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller)
+ * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate
+ *
+ * @unit seconds
+ * @min 0
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f);
 /**
  * Throttle setting while detecting launch.
  *

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -102,6 +102,8 @@ static int	_control_task = -1;			/**< task handle for sensor task */
  */
 extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
 
+using namespace launchdetection;
+
 class FixedwingPositionControl
 {
 public:
@@ -173,8 +175,7 @@ private:
 	bool land_onslope;
 
 	/* takeoff/launch states */
-	bool launch_detected;
-	bool usePreTakeoffThrust;
+	LaunchDetectionResult launch_detection_state;
 
 	bool last_manual;				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
 
@@ -447,8 +448,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_stayonground(false),
 	land_motor_lim(false),
 	land_onslope(false),
-	launch_detected(false),
-	usePreTakeoffThrust(false),
+	launch_detection_state(LAUNCHDETECTION_RES_NONE),
 	last_manual(false),
 	landingslope(),
 	flare_curve_alt_rel_last(0.0f),
@@ -1102,39 +1102,38 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
 
 			/* Perform launch detection */
-			if(!launch_detected) { //do not do further checks once a launch was detected
-				if (launchDetector.launchDetectionEnabled()) {
-					static hrt_abstime last_sent = 0;
-					if(hrt_absolute_time() - last_sent > 4e6) {
-						mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
-						last_sent = hrt_absolute_time();
-					}
-
-					/* Tell the attitude controller to stop integrating while we are waiting
-					 * for the launch */
-					_att_sp.roll_reset_integral = true;
-					_att_sp.pitch_reset_integral = true;
-					_att_sp.yaw_reset_integral = true;
-
-					/* Detect launch */
-					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
-					if (launchDetector.getLaunchDetected()) {
-						launch_detected = true;
-						mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
-					}
-				} else	{
-					/* no takeoff detection --> fly */
-					launch_detected = true;
-					warnx("launchdetection off");
+			if (launchDetector.launchDetectionEnabled() &&
+					launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+				/* Inform user that launchdetection is running */
+				static hrt_abstime last_sent = 0;
+				if(hrt_absolute_time() - last_sent > 4e6) {
+					mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
+					last_sent = hrt_absolute_time();
 				}
+
+				/* Detect launch */
+				launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+
+				/* update our copy of the laucn detection state */
+				launch_detection_state = launchDetector.getLaunchDetected();
+			} else	{
+				/* no takeoff detection --> fly */
+				launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
 			}
 
-			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
-			_att_sp.roll_body = _l1_control.nav_roll();
-			_att_sp.yaw_body = _l1_control.nav_bearing();
+			/* Set control values depending on the detection state */
+			if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
+				/* Launch has been detected, hence we have to control the plane. */
 
-			if (launch_detected) {
-				usePreTakeoffThrust = false;
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+				_att_sp.roll_body = _l1_control.nav_roll();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+
+				/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
+				 * full throttle, otherwise we use the preTakeOff Throttle */
+				float takeoff_throttle = launch_detection_state !=
+					LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
+					launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
 
 				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
 				if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
@@ -1145,7 +1144,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 							eas2tas,
 							math::radians(_parameters.pitch_limit_min),
 							math::radians(_parameters.pitch_limit_max),
-							_parameters.throttle_min, _parameters.throttle_max,
+							_parameters.throttle_min, takeoff_throttle,
 							_parameters.throttle_cruise,
 							true,
 							math::max(math::radians(pos_sp_triplet.current.pitch_min),
@@ -1155,7 +1154,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 							TECS_MODE_TAKEOFF);
 
 					/* limit roll motion to ensure enough lift */
-					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
+							math::radians(15.0f));
 
 				} else {
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
@@ -1164,17 +1164,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 								math::radians(_parameters.pitch_limit_min),
 								math::radians(_parameters.pitch_limit_max),
 								_parameters.throttle_min,
-								_parameters.throttle_max,
+								takeoff_throttle,
 								_parameters.throttle_cruise,
 								false,
 								math::radians(_parameters.pitch_limit_min),
 								_global_pos.alt,
 								ground_speed);
 				}
-
 			} else {
-				usePreTakeoffThrust = true;
+				/* Tell the attitude controller to stop integrating while we are waiting
+				 * for the launch */
+				_att_sp.roll_reset_integral = true;
+				_att_sp.pitch_reset_integral = true;
+				_att_sp.yaw_reset_integral = true;
+
+				/* Set default roll and pitch setpoints during detection phase */
+				_att_sp.roll_body = 0.0f;
+				_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
+						math::radians(10.0f));
 			}
+
 		}
 
 		/* reset landing state */
@@ -1211,13 +1220,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
 		/* Set thrrust to 0 to minimize damage */
 		_att_sp.thrust = 0.0f;
-	}
-	else if (usePreTakeoffThrust) {
+	} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+		 /* making sure again that the correct thrust is used,
+		 * without depending on library calls */
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
 	} else {
-		_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max);
+		/* Copy thrust and pitch values from tecs */
+		_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
+				_tecs.get_throttle_demand(), throttle_max);
+	}
+
+	/* During a takeoff waypoint while waiting for launch the pitch sp is set
+	 * already (not by tecs) */
+	if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+			launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
+		_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 	}
-	_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 
 	if (_control_mode.flag_control_position_enabled) {
 		last_manual = false;
@@ -1377,8 +1396,7 @@ FixedwingPositionControl::task_main()
 
 void FixedwingPositionControl::reset_takeoff_state()
 {
-	launch_detected = false;
-	usePreTakeoffThrust = false;
+	launch_detection_state = LAUNCHDETECTION_RES_NONE;
 	launchDetector.reset();
 }
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -54,6 +54,7 @@
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
+#include <drivers/drv_range_finder.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -102,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_battery_pub(-1),
 	_cmd_pub(-1),
 	_flow_pub(-1),
+	_range_pub(-1),
 	_offboard_control_sp_pub(-1),
 	_local_pos_sp_pub(-1),
 	_global_vel_sp_pub(-1),
@@ -200,6 +202,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			handle_message_hil_state_quaternion(msg);
 			break;
 
+		case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
+			handle_message_hil_optical_flow(msg);
+			break;
+
 		default:
 			break;
 		}
@@ -363,6 +369,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
+{
+	/* optical flow */
+	mavlink_hil_optical_flow_t flow;
+	mavlink_msg_hil_optical_flow_decode(msg, &flow);
+
+	struct optical_flow_s f;
+	memset(&f, 0, sizeof(f));
+
+	f.timestamp = hrt_absolute_time();
+	f.flow_timestamp = flow.time_usec;
+	f.flow_raw_x = flow.flow_x;
+	f.flow_raw_y = flow.flow_y;
+	f.flow_comp_x_m = flow.flow_comp_m_x;
+	f.flow_comp_y_m = flow.flow_comp_m_y;
+	f.ground_distance_m = flow.ground_distance;
+	f.quality = flow.quality;
+	f.sensor_id = flow.sensor_id;
+
+	if (_flow_pub < 0) {
+		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+
+	} else {
+		orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
+	}
+
+	/* Use distance value for range finder report */
+	struct range_finder_report r;
+	memset(&r, 0, sizeof(f));
+
+	r.timestamp = hrt_absolute_time();
+	r.error_count = 0;
+	r.type = RANGE_FINDER_TYPE_LASER;
+	r.distance = flow.ground_distance;
+	r.minimum_distance = 0.0f;
+	r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
+	r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
+
+	if (_range_pub < 0) {
+		_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
+	} else {
+		orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
+	}
+}
+
 void
 MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 {
@@ -444,7 +496,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	vision_position.vx = 0.0f;
 	vision_position.vy = 0.0f;
 	vision_position.vz = 0.0f;
-	
+
 	math::Quaternion q;
 	q.from_euler(pos.roll, pos.pitch, pos.yaw);
 

src/modules/mavlink/mavlink_receiver.h

@@ -112,6 +112,7 @@ private:
 	void handle_message_command_long(mavlink_message_t *msg);
 	void handle_message_command_int(mavlink_message_t *msg);
 	void handle_message_optical_flow(mavlink_message_t *msg);
+	void handle_message_hil_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
@@ -142,6 +143,7 @@ private:
 	orb_advert_t _battery_pub;
 	orb_advert_t _cmd_pub;
 	orb_advert_t _flow_pub;
+	orb_advert_t _range_pub;
 	orb_advert_t _offboard_control_sp_pub;
 	orb_advert_t _local_pos_sp_pub;
 	orb_advert_t _global_vel_sp_pub;