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

src/modules/commander/commander.cpp

@@ -546,24 +546,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 
-#if 0
 	/* Flight termination */
-	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
-
-			//XXX: to enable the parachute, a param needs to be set
-			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
-			if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
-				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
-				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
-
+	case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
+			if (cmd->param1 > 0.5f) {
+				//XXX update state machine?
+				armed_local->force_failsafe = true;
+				warnx("forcing failsafe");
 			} else {
-				/* reject parachute depoyment not armed */
-				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				armed_local->force_failsafe = false;
+				warnx("disabling failsafe");
 			}
-
+			cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 		}
 		break;
-#endif
 
 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
@@ -1421,8 +1416,12 @@ int commander_thread_main(int argc, char *argv[])
 				arming_state_changed = true;
 
 			} else if (arming_ret == TRANSITION_DENIED) {
-				/* DENIED here indicates bug in the commander */
-				mavlink_log_critical(mavlink_fd, "arming state transition denied");
+				/*
+				 * the arming transition can be denied to a number of reasons:
+				 *  - pre-flight check failed (sensors not ok or not calibrated)
+				 *  - safety not disabled
+				 *  - system not in manual mode
+				 */
 				tune_negative(true);
 			}
 

src/modules/commander/state_machine_helper.cpp

@@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
 
 	transition_result_t ret = TRANSITION_DENIED;
-
 	arming_state_t current_arming_state = status->arming_state;
+	bool feedback_provided = false;
 
 	/* only check transition if the new state is actually different from the current one */
 	if (new_arming_state == current_arming_state) {
@@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 
 					// Fail transition if pre-arm check fails
 					if (prearm_ret) {
+						/* the prearm check already prints the reject reason */
+						feedback_provided = true;
 						valid_transition = false;
 
 					// Fail transition if we need safety switch press
 					} else if (safety->safety_switch_available && !safety->safety_off) {
 
-						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
-
+						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
+						feedback_provided = true;
 						valid_transition = false;
 					}
 
@@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 						if (!status->condition_power_input_valid) {
 
 							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
+							feedback_provided = true;
 							valid_transition = false;
 						}
 
@@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 							(status->avionics_power_rail_voltage < 4.9f))) {
 
 							mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
+							feedback_provided = true;
 							valid_transition = false;
 						}
 					}
@@ -200,6 +204,8 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 
 		/* Sensors need to be initialized for STANDBY state */
 		if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
+			feedback_provided = true;
 			valid_transition = false;
 		}
 
@@ -216,11 +222,14 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	}
 
 	if (ret == TRANSITION_DENIED) {
-		static const char *errMsg = "INVAL: %s - %s";
+		const char * str = "INVAL: %s - %s";
+		/* only print to console here by default as this is too technical to be useful during operation */
+		warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
 
-		mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
-
-		warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+		/* print to MAVLink if we didn't provide any feedback yet */
+		if (!feedback_provided) {
+			mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
+		}
 	}
 
 	return ret;
@@ -648,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
 
 		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
-			mavlink_log_critical(mavlink_fd, "hold still while arming");
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
 			/* this is frickin' fatal */
 			failed = true;
 			goto system_eval;

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -212,9 +212,6 @@ private:
 
 		float throttle_land_max;
 
-		float heightrate_p;
-		float speedrate_p;
-
 		float land_slope_angle;
 		float land_H1_virt;
 		float land_flare_alt_relative;
@@ -242,9 +239,6 @@ private:
 
 		param_t throttle_land_max;
 
-		param_t heightrate_p;
-		param_t speedrate_p;
-
 		param_t land_slope_angle;
 		param_t land_H1_virt;
 		param_t land_flare_alt_relative;
@@ -494,9 +488,6 @@ FixedwingPositionControl::parameters_update()
 
 	param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
 
-	param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
-	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
-
 	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
 	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
 	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
@@ -756,8 +747,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		if (!_was_pos_control_mode) {
 			/* reset integrators */
-			_mTecs.resetIntegrators();
-			_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			}
 		}
 
 		_was_pos_control_mode = true;

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -49,6 +49,7 @@ namespace fwPosctrl {
 mTecs::mTecs() :
 	SuperBlock(NULL, "MT"),
 	/* Parameters */
+	_mTecsEnabled(this, "ENABLED"),
 	_airspeedMin(this, "FW_AIRSPD_MIN", false),
 	/* Publications */
 	_status(&getPublications(), ORB_ID(tecs_status)),

src/modules/fw_pos_control_l1/mtecs/mTecs.h

@@ -90,12 +90,14 @@ public:
 	void resetDerivatives(float airspeed);
 
 	/* Accessors */
+	bool getEnabled() { return _mTecsEnabled.get() > 0; }
 	float getThrottleSetpoint() { return _throttleSp; }
 	float getPitchSetpoint() { return _pitchSp; }
 	float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
 
 protected:
 	/* parameters */
+	control::BlockParamInt _mTecsEnabled;		/**< 1 if mTecs is enabled */
 	control::BlockParamFloat _airspeedMin;		/**< minimal airspeed */
 
 	/* Publications */

src/modules/fw_pos_control_l1/mtecs/mTecs_params.c

@@ -46,6 +46,17 @@
  * Controller parameters, accessible via MAVLink
  */
 
+/**
+ * mTECS enabled
+ *
+ * Set to 1 to enable mTECS
+ *
+ * @min 0
+ * @max 1
+ * @group mTECS
+ */
+PARAM_DEFINE_INT32(MT_ENABLED, 1);
+
 /**
  * Total Energy Rate Control Feedforward
  * Maps the total energy rate setpoint to the throttle setpoint

src/modules/position_estimator_inav/position_estimator_inav_params.c

@@ -40,22 +40,196 @@
 
 #include "position_estimator_inav_params.h"
 
+/**
+ * Z axis weight for barometer
+ *
+ * Weight (cutoff frequency) for barometer altitude measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
+
+/**
+ * Z axis weight for GPS
+ *
+ * Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
+
+/**
+ * Z axis weight for sonar
+ *
+ * Weight (cutoff frequency) for sonar measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
+
+/**
+ * XY axis weight for GPS position
+ *
+ * Weight (cutoff frequency) for GPS position measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
+
+/**
+ * XY axis weight for GPS velocity
+ *
+ * Weight (cutoff frequency) for GPS velocity measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
+
+/**
+ * XY axis weight for optical flow
+ *
+ * Weight (cutoff frequency) for optical flow (velocity) measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
+
+/**
+ * XY axis weight for resetting velocity
+ *
+ * When velocity sources lost slowly decrease estimated horizontal velocity with this weight.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
+
+/**
+ * XY axis weight factor for GPS when optical flow available
+ *
+ * When optical flow data available, multiply GPS weights (for position and velocity) by this factor.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
+
+/**
+ * Accelerometer bias estimation weight
+ *
+ * Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
+ *
+ * @min 0.0
+ * @max 0.1
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
+
+/**
+ * Optical flow scale factor
+ *
+ * Factor to convert raw optical flow (in pixels) to radians [rad/px].
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit rad/px
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
+
+/**
+ * Minimal acceptable optical flow quality
+ *
+ * 0 - lowest quality, 1 - best quality.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
+
+/**
+ * Weight for sonar filter
+ *
+ * Sonar filter detects spikes on sonar measurements and used to detect new surface level.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
+
+/**
+ * Sonar maximal error for new surface
+ *
+ * If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable).
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit m
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
+
+/**
+ * Land detector time
+ *
+ * Vehicle assumed landed if no altitude changes happened during this time on low throttle.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @unit s
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
+
+/**
+ * Land detector altitude dispersion threshold
+ *
+ * Dispersion threshold for triggering land detector.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @unit m
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
+
+/**
+ * Land detector throttle threshold
+ *
+ * Value should be lower than minimal hovering thrust. Half of it is good choice.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
+
+/**
+ * GPS delay
+ *
+ * GPS delay compensation
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit s
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)

src/modules/px4iofirmware/mixer.cpp

@@ -111,7 +111,7 @@ mixer_tick(void)
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
 	}
 
-	/* default to failsafe mixing */
+	/* default to failsafe mixing - it will be forced below if flag is set */
 	source = MIX_FAILSAFE;
 
 	/*
@@ -154,6 +154,13 @@ mixer_tick(void)
 		}
 	}
 
+	/*
+	 * Check if we should force failsafe - and do it if we have to
+	 */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
+		source = MIX_FAILSAFE;
+	}
+
 	/*
 	 * Set failsafe status flag depending on mixing source
 	 */

src/modules/px4iofirmware/protocol.h

@@ -179,6 +179,7 @@
 #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
 #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
 #define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
+#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE	(1 << 8) /* If set, the system will always output the failsafe values */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
@@ -253,6 +254,10 @@ enum {							/* DSM bind states */
 /* PWM failsafe values - zero disables the output */
 #define PX4IO_PAGE_FAILSAFE_PWM			55		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_SENSORS			56		/**< Sensors connected to PX4IO */
+#define PX4IO_P_SENSORS_ALTITUDE		0		/**< Altitude of an external sensor (HoTT or S.BUS2) */
+
 /* Debug and test page - not used in normal operation */
 #define PX4IO_PAGE_TEST				127
 #define PX4IO_P_TEST_LED			0		/**< set the amber LED on/off */

src/modules/px4iofirmware/registers.c

@@ -189,7 +189,8 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
 					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
 					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
-					 PX4IO_P_SETUP_ARMING_LOCKDOWN)
+					 PX4IO_P_SETUP_ARMING_LOCKDOWN | \
+					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 

src/modules/uORB/topics/actuator_armed.h

@@ -56,6 +56,7 @@ struct actuator_armed_s {
 	bool		armed;		/**< Set to true if system is armed */
 	bool		ready_to_arm;	/**< Set to true if system is ready to be armed */
 	bool		lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+	bool		force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */
 };
 
 /**
@@ -65,4 +66,4 @@ struct actuator_armed_s {
 /* register this as object request broker structure */
 ORB_DECLARE(actuator_armed);
 
-#endif
+#endif

src/modules/uORB/topics/vehicle_command.h

@@ -76,6 +76,7 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */