Merged beta into master

src/modules/commander/accelerometer_calibration.cpp

@@ -561,6 +561,7 @@ int do_level_calibration(int mavlink_fd) {
 	const unsigned cal_time = 5;
 	const unsigned cal_hz = 100;
 	const unsigned settle_time = 30;
+	bool success = false;
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
@@ -599,7 +600,15 @@ int do_level_calibration(int mavlink_fd) {
 	start = hrt_absolute_time();
 	// average attitude for 5 seconds
 	while(hrt_elapsed_time(&start) < cal_time * 1000000) {
-		px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+		if (pollret <= 0) {
+			// attitude estimator is not running
+			mavlink_and_console_log_critical(mavlink_fd, "attitude estimator not running - check system boot");
+			mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
+			goto out;
+		}
+
 		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 		roll_mean += att.roll;
 		pitch_mean += att.pitch;
@@ -608,7 +617,6 @@ int do_level_calibration(int mavlink_fd) {
 
 	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
 
-	bool success = false;
 	if (counter > (cal_time * cal_hz / 2 )) {
 		roll_mean /= counter;
 		pitch_mean /= counter;

src/modules/commander/airspeed_calibration.cpp

@@ -75,12 +75,12 @@ int do_airspeed_calibration(int mavlink_fd)
 {
 	int result = OK;
 	unsigned calibration_counter = 0;
-	const unsigned maxcount = 3000;
+	const unsigned maxcount = 2400;
 
 	/* give directions */
 	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
-	const unsigned calibration_count = 2000;
+	const unsigned calibration_count = (maxcount * 2) / 3;
 
 	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	struct differential_pressure_s diff_pres;
@@ -204,10 +204,10 @@ int do_airspeed_calibration(int mavlink_fd)
 	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
 	while (calibration_counter < maxcount) {
 
-        if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
-		goto error_return;
-        }
-        
+		if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
+			goto error_return;
+		}
+
 		/* wait blocking for new data */
 		px4_pollfd_struct_t fds[1];
 		fds[0].fd = diff_pres_sub;

src/modules/commander/calibration_routines.cpp

@@ -470,8 +470,8 @@ calibrate_return calibrate_from_orientation(int		mavlink_fd,
 		/* inform user about already handled side */
 		if (side_data_collected[orient]) {
 			orientation_failures++;
-			mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient));
-			mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side");
+			mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient));
+			mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side");
 			continue;
 		}
 		

src/modules/commander/commander.cpp

@@ -2151,6 +2151,19 @@ int commander_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
 
 			armed.timestamp = now;
+
+			/* set prearmed state if safety is off, or safety is not present and 5 seconds passed */
+			if (safety.safety_switch_available) {
+
+				/* safety is off, go into prearmed */
+				armed.prearmed = safety.safety_off;
+			} else {
+				/* safety is not present, go into prearmed
+				 * (all output drivers should be started / unlocked last in the boot process
+				 * when the rest of the system is fully initialized)
+				 */
+				armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000);
+			}
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
 

src/modules/commander/mag_calibration.cpp

@@ -67,9 +67,11 @@
 static const int ERROR = -1;
 
 static const char *sensor_name = "mag";
-static const unsigned max_mags = 3;
+static constexpr unsigned max_mags = 3;
 static constexpr float mag_sphere_radius = 0.2f;
-static const unsigned int calibration_sides = 6;
+static constexpr unsigned int calibration_sides = 6;			///< The total number of sides
+static constexpr unsigned int calibration_total_points = 240;		///< The total points per magnetometer
+static constexpr unsigned int calibraton_duration_seconds = 42; 	///< The total duration the routine is allowed to take
 
 calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
@@ -207,6 +209,10 @@ static bool reject_sample(float sx, float sy, float sz, float x[], float y[], fl
 	return false;
 }
 
+static unsigned progress_percentage(mag_worker_data_t* worker_data) {
+	return 100 * ((float)worker_data->done_count) / calibration_sides;
+}
+
 static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
 {
 	calibrate_return result = calibrate_return_ok;
@@ -226,7 +232,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 	 * for a good result, so we're not constraining the user more than we have to.
 	 */
 
-	hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 2;
+	hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5;
 	hrt_abstime last_gyro = 0;
 	float gyro_x_integral = 0.0f;
 	float gyro_y_integral = 0.0f;
@@ -347,8 +353,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 				// Progress indicator for side
 				mavlink_and_console_log_info(worker_data->mavlink_fd,
 							     "[cal] %s side calibration: progress <%u>",
-							     detect_orientation_str(orientation),
-							     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
+							     detect_orientation_str(orientation), progress_percentage(worker_data) +
+							     (unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
 			}
 		} else {
 			poll_errcount++;
@@ -365,7 +371,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 		mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
 		
 		worker_data->done_count++;
-		mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
+		mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data));
 	}
 	
 	return result;
@@ -379,8 +385,8 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	
 	worker_data.mavlink_fd = mavlink_fd;
 	worker_data.done_count = 0;
-	worker_data.calibration_points_perside = 40;
-	worker_data.calibration_interval_perside_seconds = 20;
+	worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
+	worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
 	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
 
 	// Collect: Right-side up, Left Side, Nose down
@@ -499,6 +505,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 		printf("RAW DATA:\n--------------------\n");
 		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
 
+			if (worker_data.calibration_counter_total[cur_mag] == 0) {
+				continue;
+			}
+
 			printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
 
 			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
@@ -514,6 +524,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 		printf("CALIBRATED DATA:\n--------------------\n");
 		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
 
+			if (worker_data.calibration_counter_total[cur_mag] == 0) {
+				continue;
+			}
+
 			printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]);
 
 			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {

src/modules/fw_att_control/fw_att_control_params.c

@@ -84,11 +84,11 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
  * This gain defines how much control response will result out of a steady
  * state error. It trims any constant error.
  *
- * @min 0.0
- * @max 50.0
+ * @min 0.005
+ * @max 0.5
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_PR_I, 0.005f);
+PARAM_DEFINE_FLOAT(FW_PR_I, 0.01f);
 
 /**
  * Maximum positive / up pitch rate.
@@ -157,11 +157,11 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
  * This gain defines how much control response will result out of a steady
  * state error. It trims any constant error.
  *
- * @min 0.0
- * @max 100.0
+ * @min 0.005
+ * @max 0.2
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_RR_I, 0.005f);
+PARAM_DEFINE_FLOAT(FW_RR_I, 0.01f);
 
 /**
  * Roll Integrator Anti-Windup

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1037,7 +1037,7 @@ bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
-	float dt = FLT_MIN; // Using non zero value to a avoid division by zero
+	float dt = 0.01; // Using non zero value to a avoid division by zero
 	if (_control_position_last_called > 0) {
 		dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
 	}
@@ -1045,19 +1045,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	bool setpoint = true;
 
-	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
-	calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
-
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
 	/* filter speed and altitude for controller */
 	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
-	if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) {
-		_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
+	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
+	bool in_air_alt_control = (!_vehicle_status.condition_landed &&
+		(_control_mode.flag_control_auto_enabled ||
+		 _control_mode.flag_control_velocity_enabled ||
+		 _control_mode.flag_control_altitude_enabled));
+
+	/* update TECS filters */
+	if (!_mTecs.getEnabled()) {
+		_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
+			accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
 	}
 
+	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
+	calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
+
 	/* define altitude error */
 	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -49,7 +49,11 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
 	_vehicleLocalPositionSub(-1),
 	_vehicleLocalPosition({}),
 	_airspeedSub(-1),
-	_airspeed({}),
+	_vehicleStatusSub(-1),
+	_armingSub(-1),
+	_airspeed{},
+	_vehicleStatus{},
+	_arming{},
 	_parameterSub(-1),
 	_velocity_xy_filtered(0.0f),
 	_velocity_z_filtered(0.0f),
@@ -66,6 +70,8 @@ void FixedwingLandDetector::initialize()
 	// Subscribe to local position and airspeed data
 	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
+	_vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status));
+	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 
 	updateParameterCache(true);
 }
@@ -74,6 +80,8 @@ void FixedwingLandDetector::updateSubscriptions()
 {
 	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
 	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+	orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus);
+	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
 }
 
 bool FixedwingLandDetector::update()
@@ -81,6 +89,11 @@ bool FixedwingLandDetector::update()
 	// First poll for new data from our subscriptions
 	updateSubscriptions();
 
+	// only trigger flight conditions if we are armed
+	if (!_arming.armed) {
+		return true;
+	}
+
 	const uint64_t now = hrt_absolute_time();
 	bool landDetected = false;
 

src/modules/land_detector/FixedwingLandDetector.h

@@ -44,7 +44,9 @@
 #include "LandDetector.h"
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
 #include <systemlib/param/param.h>
 
 class FixedwingLandDetector : public LandDetector
@@ -90,11 +92,15 @@ private:
 	} _params;
 
 private:
-	int                                     _vehicleLocalPositionSub;   /**< notification of local position */
-	struct vehicle_local_position_s         _vehicleLocalPosition;      /**< the result from local position subscription */
-	int                                     _airspeedSub;
-	struct airspeed_s                       _airspeed;
-	int 									_parameterSub;
+	int					_vehicleLocalPositionSub;	/**< notification of local position */
+	struct vehicle_local_position_s		_vehicleLocalPosition;		/**< the result from local position subscription */
+	int					_airspeedSub;
+	int					_vehicleStatusSub;
+	int					_armingSub;
+	struct airspeed_s			_airspeed;
+	struct vehicle_status_s 		_vehicleStatus;
+	struct actuator_armed_s			_arming;
+	int 					_parameterSub;
 
 	float _velocity_xy_filtered;
 	float _velocity_z_filtered;

src/modules/land_detector/LandDetector.cpp

@@ -46,8 +46,9 @@
 LandDetector::LandDetector() :
 	_landDetectedPub(0),
 	_landDetected({0, false}),
-	      _taskShouldExit(false),
-	      _taskIsRunning(false)
+	_arming_time(0),
+	_taskShouldExit(false),
+	_taskIsRunning(false)
 {
 	// ctor
 }

src/modules/land_detector/LandDetector.h

@@ -96,10 +96,12 @@ protected:
 
 	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000;  /**< usec that landing conditions have to hold
                                                                           before triggering a land */
+	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000;	/**< time interval in which wider acceptance thresholds are used after arming */
 
 protected:
-	uintptr_t                               _landDetectedPub;           /**< publisher for position in local frame */
-	struct vehicle_land_detected_s          _landDetected;              /**< local vehicle position */
+	uintptr_t				_landDetectedPub;		/**< publisher for position in local frame */
+	struct vehicle_land_detected_s		_landDetected;			/**< local vehicle position */
+	uint64_t				_arming_time;			/**< timestamp of arming time */
 
 private:
 	bool _taskShouldExit;                                               /**< true if it is requested that this task should exit */

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -53,12 +53,12 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_actuatorsSub(-1),
 	_armingSub(-1),
 	_parameterSub(-1),
-    _attitudeSub(-1),
-	_vehicleGlobalPosition({}),
-	_vehicleStatus({}),
-	_actuators({}),
-	_arming({}),
-    _vehicleAttitude({}),
+	_attitudeSub(-1),
+	_vehicleGlobalPosition{},
+	_vehicleStatus{},
+	_actuators{},
+	_arming{},
+	_vehicleAttitude{},
 	_landTimer(0)
 {
 	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
@@ -97,7 +97,10 @@ bool MulticopterLandDetector::update()
 
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
+		_arming_time = 0;
 		return true;
+	} else if (_arming_time == 0) {
+		_arming_time = hrt_absolute_time();
 	}
 
 	// return status based on armed state if no position lock is available
@@ -110,8 +113,18 @@ bool MulticopterLandDetector::update()
 
 	const uint64_t now = hrt_absolute_time();
 
-	// check if we are moving vertically
-	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
+	float armThresholdFactor = 1.0f;
+
+	// Widen acceptance thresholds for landed state right after arming
+	// so that motor spool-up and other effects do not trigger false negatives
+	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
+		armThresholdFactor = 2.5f;
+	}
+
+	// check if we are moving vertically - this might see a spike after arming due to
+	// throttle-up vibration. If accelerating fast the throttle thresholds will still give
+	// an accurate in-air indication
+	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor;
 
 	// check if we are moving horizontally
 	bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
@@ -119,9 +132,11 @@ bool MulticopterLandDetector::update()
 					&& _vehicleStatus.condition_global_position_valid;
 
 	// next look if all rotation angles are not moving
-	bool rotating = fabsf(_vehicleAttitude.rollspeed)  > _params.maxRotation ||
-				    fabsf(_vehicleAttitude.pitchspeed) > _params.maxRotation ||
-        			fabsf(_vehicleAttitude.yawspeed)   > _params.maxRotation;
+	float maxRotationScaled = _params.maxRotation * armThresholdFactor;
+
+	bool rotating = (fabsf(_vehicleAttitude.rollspeed)  > maxRotationScaled) ||
+				    (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
+				(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
 
 	// check if thrust output is minimal (about half of default)
 	bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;

src/modules/land_detector/MulticopterLandDetector.h

@@ -97,20 +97,20 @@ private:
 	} _params;
 
 private:
-	int _vehicleGlobalPositionSub;                                      /**< notification of global position */
+	int _vehicleGlobalPositionSub;						/**< notification of global position */
 	int _vehicleStatusSub;
 	int _actuatorsSub;
 	int _armingSub;
 	int _parameterSub;
 	int _attitudeSub;
 
-	struct vehicle_global_position_s          _vehicleGlobalPosition;   /**< the result from global position subscription */
-	struct vehicle_status_s 			      _vehicleStatus;
-	struct actuator_controls_s                _actuators;
-	struct actuator_armed_s                   _arming;
-	struct vehicle_attitude_s                 _vehicleAttitude;
+	struct vehicle_global_position_s	_vehicleGlobalPosition;		/**< the result from global position subscription */
+	struct vehicle_status_s 		_vehicleStatus;
+	struct actuator_controls_s		_actuators;
+	struct actuator_armed_s			_arming;
+	struct vehicle_attitude_s		_vehicleAttitude;
 
-	uint64_t _landTimer;                                                /**< timestamp in microseconds since a possible land was detected*/
+	uint64_t _landTimer;							/**< timestamp in microseconds since a possible land was detected*/
 };
 
 #endif //__MULTICOPTER_LAND_DETECTOR_H__

src/modules/land_detector/land_detector_params.c

@@ -49,7 +49,7 @@
  *
  * @group Land Detector
  */
-PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.60f);
+PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.70f);
 
 /**
  * Multicopter max horizontal velocity

src/modules/mavlink/mavlink_main.cpp

@@ -120,6 +120,8 @@ extern mavlink_system_t mavlink_system;
 
 static void usage(void);
 
+bool Mavlink::_boot_complete = false;
+
 Mavlink::Mavlink() :
 #ifndef __PX4_NUTTX
 	VDev("mavlink-log", MAVLINK_LOG_DEVICE),
@@ -1639,15 +1641,15 @@ Mavlink::task_main(int argc, char *argv[])
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
-		configure_stream("HIGHRES_IMU", 1.0f);
-		configure_stream("ATTITUDE", 10.0f);
+		configure_stream("HIGHRES_IMU", 2.0f);
+		configure_stream("ATTITUDE", 20.0f);
 		configure_stream("VFR_HUD", 8.0f);
 		configure_stream("GPS_RAW_INT", 1.0f);
 		configure_stream("GLOBAL_POSITION_INT", 3.0f);
 		configure_stream("LOCAL_POSITION_NED", 3.0f);
-		configure_stream("RC_CHANNELS", 1.0f);
+		configure_stream("RC_CHANNELS", 4.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
-		configure_stream("ATTITUDE_TARGET", 3.0f);
+		configure_stream("ATTITUDE_TARGET", 8.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
 		break;
@@ -2163,6 +2165,10 @@ int mavlink_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "stream")) {
 		return Mavlink::stream_command(argc, argv);
 
+	} else if (!strcmp(argv[1], "boot_complete")) {
+		Mavlink::set_boot_complete();
+		return 0;
+
 	} else {
 		usage();
 		return 1;

src/modules/mavlink/mavlink_main.h

@@ -163,6 +163,14 @@ public:
 
 	bool			get_forwarding_on() { return _forwarding_on; }
 
+	/**
+	 * Set the boot complete flag on all instances
+	 *
+	 * Setting the flag unblocks parameter transmissions, which are gated
+	 * beforehand to ensure that the system is fully initialized.
+	 */
+	static void		set_boot_complete() { _boot_complete = true; }
+
 	/**
 	 * Get the free space in the transmit buffer
 	 *
@@ -325,6 +333,8 @@ public:
 
 	int 			get_socket_fd () { return _socket_fd; };
 
+	static bool		boot_complete() { return _boot_complete; }
+
 protected:
 	Mavlink			*next;
 
@@ -333,6 +343,7 @@ private:
 
 	int			_mavlink_fd;
 	bool			_task_running;
+	static bool		_boot_complete;
 
 	/* states */
 	bool			_hil_enabled;		/**< Hardware In the Loop mode */

src/modules/mavlink/mavlink_parameters.cpp

@@ -193,7 +193,7 @@ void
 MavlinkParametersManager::send(const hrt_abstime t)
 {
 	/* send all parameters if requested, but only after the system has booted */
-	if (_send_all_index >= 0 && t > 4 * 1000 * 1000) {
+	if (_send_all_index >= 0 && _mavlink->boot_complete()) {
 
 		/* skip if no space is available */
 		if (_mavlink->get_free_tx_buf() < get_size()) {

src/modules/mavlink/mavlink_receiver.cpp

@@ -35,9 +35,9 @@
  * @file mavlink_receiver.cpp
  * MAVLink protocol message receive and dispatch
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Anton Babushkin <anton@px4.io>
+ * @author Thomas Gubler <thomas@px4.io>
  */
 
 /* XXX trim includes */
@@ -136,7 +136,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_rates_sp{},
 	_time_offset_avg_alpha(0.6),
 	_time_offset(0),
-	_orb_class_instance(-1)
+	_orb_class_instance(-1),
+	_mom_switch_pos{},
+	_mom_switch_state(0)
 {
 
 }
@@ -970,15 +972,45 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	}
 }
 
-static switch_pos_t decode_switch_pos(uint16_t buttons, int sw) {
+switch_pos_t
+MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
+{
+	// XXX non-standard 3 pos switch decoding
 	return (buttons >> (sw * 2)) & 3;
 }
 
-static int decode_switch_pos_n(uint16_t buttons, int sw) {
-	if (buttons & (1 << sw)) {
-		return 1;
+int
+MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw)
+{
+
+	bool on = (buttons & (1 << sw));
+
+	if (sw < MOM_SWITCH_COUNT) {
+
+		bool last_on = (_mom_switch_state & (1 << sw));
+
+		/* first switch is 2-pos, rest is 2 pos */
+		unsigned state_count = (sw == 0) ? 3 : 2;
+
+		/* only transition on low state */
+		if (!on && (on != last_on)) {
+
+			_mom_switch_pos[sw]++;
+			if (_mom_switch_pos[sw] == state_count) {
+				_mom_switch_pos[sw] = 0;
+			}
+		}
+
+		/* state_count - 1 is the number of intervals and 1000 is the range,
+		 * with 2 states 0 becomes 0, 1 becomes 1000. With
+		 * 3 states 0 becomes 0, 1 becomes 500, 2 becomes 1000,
+		 * and so on for more states.
+		 */
+		return (_mom_switch_pos[sw] * 1000) / (state_count - 1) + 1000;
+
 	} else {
-		return 0;
+		/* return the current state */
+		return on * 1000 + 1000;
 	}
 }
 
@@ -1075,12 +1107,18 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 			rc.values[3] = 1000;
 		}
 
-		rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000;
-		rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000;
-		rc.values[6] = decode_switch_pos_n(man.buttons, 2) * 1000 + 1000;
-		rc.values[7] = decode_switch_pos_n(man.buttons, 3) * 1000 + 1000;
-		rc.values[8] = decode_switch_pos_n(man.buttons, 4) * 1000 + 1000;
-		rc.values[9] = decode_switch_pos_n(man.buttons, 5) * 1000 + 1000;
+		/* decode all switches which fit into the channel mask */
+		unsigned max_switch = (sizeof(man.buttons) * 8);
+		unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0]));
+		if (max_switch > (max_channels - 4)) {
+			max_switch = (max_channels - 4);
+		}
+
+		/* fill all channels */
+		for (unsigned i = 0; i < max_switch; i++) {
+			rc.values[i + 4] = decode_switch_pos_n(man.buttons, i);
+		}
+		_mom_switch_state = man.buttons;
 
 		if (_rc_pub == nullptr) {
 			_rc_pub = orb_advertise(ORB_ID(input_rc), &rc);

src/modules/mavlink/mavlink_receiver.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,8 +35,8 @@
  * @file mavlink_orb_listener.h
  * MAVLink 1.0 uORB listener definition
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Anton Babushkin <anton@px4.io>
  */
 
 #pragma once
@@ -141,15 +141,26 @@ private:
 	void *receive_thread(void *arg);
 
 	/**
-	* Convert remote timestamp to local hrt time (usec)
-	* Use timesync if available, monotonic boot time otherwise
-	*/
+	 * Convert remote timestamp to local hrt time (usec)
+	 * Use timesync if available, monotonic boot time otherwise
+	 */
 	uint64_t sync_stamp(uint64_t usec);
+
 	/**
-	* Exponential moving average filter to smooth time offset
-	*/
+	 * Exponential moving average filter to smooth time offset
+	 */
 	void smooth_time_offset(uint64_t offset_ns);
 
+	/**
+	 * Decode a switch position from a bitfield
+	 */
+	switch_pos_t decode_switch_pos(uint16_t buttons, unsigned sw);
+
+	/**
+	 * Decode a switch position from a bitfield and state
+	 */
+	int decode_switch_pos_n(uint16_t buttons, unsigned sw);
+
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
 	struct vehicle_land_detected_s hil_land_detector;
@@ -195,6 +206,11 @@ private:
 	uint64_t _time_offset;
 	int	_orb_class_instance;
 
+	static constexpr unsigned MOM_SWITCH_COUNT = 8;
+
+	uint8_t _mom_switch_pos[MOM_SWITCH_COUNT];
+	uint16_t _mom_switch_state;
+
 	/* do not allow copying this class */
 	MavlinkReceiver(const MavlinkReceiver &);
 	MavlinkReceiver operator=(const MavlinkReceiver &);

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -83,6 +83,9 @@
 #include <mavlink/mavlink_log.h>
 #include <platforms/px4_defines.h>
 
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
 #define TILT_COS_MAX	0.7f
 #define SIGMA			0.000001f
 #define MIN_DIST		0.01f
@@ -95,7 +98,7 @@
  */
 extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
 
-class MulticopterPositionControl
+class MulticopterPositionControl : public control::SuperBlock
 {
 public:
 	/**
@@ -122,15 +125,15 @@ private:
 	int		_control_task;			/**< task handle for task */
 	int		_mavlink_fd;			/**< mavlink fd */
 
-	int 	_vehicle_status_sub;	/**< vehicle status subscription */
-	int		_att_sub;				/**< vehicle attitude subscription */
+	int		_vehicle_status_sub;		/**< vehicle status subscription */
+	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_params_sub;			/**< notification of parameter updates */
 	int		_manual_sub;			/**< notification of manual control updates */
 	int		_arming_sub;			/**< arming status of outputs */
 	int		_local_pos_sub;			/**< vehicle local position */
-	int		_pos_sp_triplet_sub;	/**< position setpoint triplet */
+	int		_pos_sp_triplet_sub;		/**< position setpoint triplet */
 	int		_local_pos_sp_sub;		/**< offboard local position setpoint */
 	int		_global_vel_sp_sub;		/**< offboard global velocity setpoint */
 
@@ -138,17 +141,19 @@ private:
 	orb_advert_t	_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
 	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
-	struct vehicle_status_s 			_vehicle_status; /**< vehicle status */
+	struct vehicle_status_s 			_vehicle_status; 	/**< vehicle status */
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct vehicle_control_mode_s			_control_mode;	/**< vehicle control mode */
+	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
 	struct actuator_armed_s				_arming;		/**< actuator arming status */
 	struct vehicle_local_position_s			_local_pos;		/**< vehicle local position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< vehicle global position setpoint triplet */
 	struct vehicle_local_position_setpoint_s	_local_pos_sp;		/**< vehicle local position setpoint */
-	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;	/**< vehicle global velocity setpoint */
+	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;		/**< vehicle global velocity setpoint */
 
+	control::BlockParamFloat _manual_thr_min;
+	control::BlockParamFloat _manual_thr_max;
 
 	struct {
 		param_t thr_min;
@@ -213,7 +218,7 @@ private:
 	/**
 	 * Update our local parameter cache.
 	 */
-	int			parameters_update(bool force);
+	int		parameters_update(bool force);
 
 	/**
 	 * Update control outputs
@@ -293,7 +298,7 @@ MulticopterPositionControl	*g_control;
 }
 
 MulticopterPositionControl::MulticopterPositionControl() :
-
+	SuperBlock(NULL, "MPC"),
 	_task_should_exit(false),
 	_control_task(-1),
 	_mavlink_fd(-1),
@@ -313,7 +318,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_att_sp_pub(nullptr),
 	_local_pos_sp_pub(nullptr),
 	_global_vel_sp_pub(nullptr),
-
+	_manual_thr_min(this, "MANTHR_MIN"),
+	_manual_thr_max(this, "MANTHR_MAX"),
 	_ref_alt(0.0f),
 	_ref_timestamp(0),
 
@@ -413,6 +419,10 @@ MulticopterPositionControl::parameters_update(bool force)
 	}
 
 	if (updated || force) {
+		/* update C++ param system */
+		updateParams();
+
+		/* update legacy C interface params */
 		param_get(_params_handles.thr_min, &_params.thr_min);
 		param_get(_params_handles.thr_max, &_params.thr_max);
 		param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
@@ -1416,11 +1426,11 @@ MulticopterPositionControl::task_main()
 
 			/* control throttle directly if no climb rate controller is active */
 			if (!_control_mode.flag_control_climb_rate_enabled) {
-				_att_sp.thrust = math::min(_manual.z, _params.thr_max);
+				_att_sp.thrust = math::min(_manual.z, _manual_thr_max.get());
 
 				/* enforce minimum throttle if not landed */
 				if (!_vehicle_status.condition_landed) {
-					_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min);
+					_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
 				}
 			}
 

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -41,7 +41,32 @@
 #include <systemlib/param/param.h>
 
 /**
- * Minimum thrust
+ * Minimum thrust in auto thrust control
+ *
+ * It's recommended to set it > 0 to avoid free fall with zero thrust.
+ *
+ * @min 0.05
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
+
+/**
+ * Maximum thrust in auto thrust control
+ *
+ * Limit max allowed thrust. Setting a value of one can put
+ * the system into actuator saturation as no spread between
+ * the motors is possible any more. A value of 0.8 - 0.9
+ * is recommended.
+ *
+ * @min 0.0
+ * @max 0.95
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
+
+/**
+ * Minimum manual thrust
  *
  * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
  *
@@ -49,10 +74,10 @@
  * @max 1.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
+PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.12f);
 
 /**
- * Maximum thrust
+ * Maximum manual thrust
  *
  * Limit max allowed thrust. Setting a value of one can put
  * the system into actuator saturation as no spread between
@@ -63,7 +88,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
  * @max 1.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
+PARAM_DEFINE_FLOAT(MPC_MANTHR_MAX, 0.9f);
 
 /**
  * Proportional gain for vertical position error

src/modules/sdlog2/sdlog2.c

@@ -157,7 +157,7 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
  * @max  1
  * @group SD Logging
  */
-PARAM_DEFINE_INT32(SDLOG_GPSTIME, 0);
+PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1);
 
 #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
 		log_msgs_written++; \

src/modules/sensors/sensor_params.c

@@ -680,7 +680,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0);
  * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
  * to fine tune the board offset in the event of misalignment.
  *
- * @unit radians
+ * @unit degrees
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
@@ -691,7 +691,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
  * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user
  * to fine tune the board offset in the event of misalignment.
  *
- * @unit radians
+ * @unit degrees
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
@@ -702,7 +702,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
  * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user
  * to fine tune the board offset in the event of misalignment.
  *
- * @unit radians
+ * @unit degrees
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
@@ -2273,7 +2273,7 @@ PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0);
  * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
  * THE SYSTEM TO PUT CHANGES INTO EFFECT.
  *
- * Set to 1000 for default or 900 to increase servo travel
+ * Set to 1000 for industry default or 900 to increase servo travel.
  *
  * @min 800
  * @max 1400
@@ -2289,7 +2289,7 @@ PARAM_DEFINE_INT32(PWM_MIN, 1000);
  * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
  * THE SYSTEM TO PUT CHANGES INTO EFFECT.
  *
- * Set to 2000 for default or 2100 to increase servo travel
+ * Set to 2000 for industry default or 2100 to increase servo travel.
  *
  * @min 1600
  * @max 2200