Merge branch 'beta' into stable

ROMFS/px4fmu_common/init.d/3030_io_camflyer

@@ -17,11 +17,9 @@ then
 	param set FW_LND_TLALT 5
 	param set FW_THR_LND_MAX 0
 	param set FW_PR_FF 0.35
-	param set FW_PR_I 0.005
 	param set FW_PR_IMAX 0.4
 	param set FW_PR_P 0.08
 	param set FW_RR_FF 0.6
-	param set FW_RR_I 0.005
 	param set FW_RR_IMAX 0.2
 	param set FW_RR_P 0.04
 fi

ROMFS/px4fmu_common/init.d/3031_phantom

@@ -16,14 +16,12 @@ then
 	param set FW_L1_DAMPING 0.75
 	param set FW_L1_PERIOD 15
 	param set FW_PR_FF 0.2
-	param set FW_PR_I 0.005
 	param set FW_PR_IMAX 0.2
 	param set FW_PR_P 0.03
 	param set FW_P_LIM_MAX 50
 	param set FW_P_LIM_MIN -50
 	param set FW_P_ROLLFF 1
 	param set FW_RR_FF 0.5
-	param set FW_RR_I 0.02
 	param set FW_RR_IMAX 0.2
 	param set FW_RR_P 0.08
 	param set FW_R_LIM 50

ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha

@@ -22,11 +22,9 @@ then
 	param set FW_LND_TLALT 5
 	param set FW_THR_LND_MAX 0
 	param set FW_PR_FF 0.35
-	param set FW_PR_I 0.01
 	param set FW_PR_IMAX 0.4
 	param set FW_PR_P 0.08
 	param set FW_RR_FF 0.6
-	param set FW_RR_I 0.01
 	param set FW_RR_IMAX 0.2
 	param set FW_RR_P 0.04
 	param set SYS_COMPANION 157600

ROMFS/px4fmu_common/init.d/4010_dji_f330

@@ -21,7 +21,11 @@ then
 	param set MC_YAWRATE_P 0.2
 	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
+	param set PWM_DISARMED 900
+	param set PWM_MIN 1230
+	param set PWM_MAX 1950
 fi
 
-set PWM_MIN 1200
-set PWM_MAX 1950
+set PWM_DISARMED p:PWM_DISARMED
+set PWM_MIN p:PWM_MIN
+set PWM_MAX p:PWM_MAX

ROMFS/px4fmu_common/init.d/4011_dji_f450

@@ -22,7 +22,11 @@ then
 	param set MC_YAWRATE_P 0.3
 	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
+	param set PWM_DISARMED 900
+	param set PWM_MIN 1230
+	param set PWM_MAX 1950
 fi
 
-set PWM_MIN 1230
-set PWM_MAX 1950
+set PWM_DISARMED p:PWM_DISARMED
+set PWM_MIN p:PWM_MIN
+set PWM_MAX p:PWM_MAX

src/drivers/gps/gps.cpp

@@ -310,7 +310,7 @@ GPS::task_main()
 			_report_gps_pos.cog_rad = 0.0f;
 			_report_gps_pos.vel_ned_valid = true;				
 
-			//no time and satellite information simulated
+			/* no time and satellite information simulated */
 
 
 			if (!(_pub_blocked)) {
@@ -351,28 +351,46 @@ GPS::task_main()
 
 			unlock();
 
+			/* the Ashtech driver lies about successful configuration and the
+			 * MTK driver is not well tested, so we really only trust the UBX
+			 * driver for an advance publication
+			 */
 			if (_Helper->configure(_baudrate) == 0) {
 				unlock();
 
-				//Publish initial report that we have access to a GPS
-				//Make sure to clear any stale data in case driver is reset
+				/* reset report */
 				memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
-				_report_gps_pos.timestamp_position = hrt_absolute_time();
-				_report_gps_pos.timestamp_variance = hrt_absolute_time();
-				_report_gps_pos.timestamp_velocity = hrt_absolute_time();
-				_report_gps_pos.timestamp_time = hrt_absolute_time();
 
-				if (!(_pub_blocked)) {
-					if (_report_gps_pos_pub > 0) {
-						orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
+				if (_mode == GPS_DRIVER_MODE_UBX) {
+					/* Publish initial report that we have access to a GPS,
+					 * but set all critical state fields to indicate we have
+					 * no valid position lock
+					 */
 
-					} else {
-						_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
+					_report_gps_pos.timestamp_time = hrt_absolute_time();
+
+					/* reset the timestamps for data, because we have no data yet */
+					_report_gps_pos.timestamp_position = 0;
+					_report_gps_pos.timestamp_variance = 0;
+					_report_gps_pos.timestamp_velocity = 0;
+
+					/* set a massive variance */
+					_report_gps_pos.eph = 10000.0f;
+					_report_gps_pos.epv = 10000.0f;
+					_report_gps_pos.fix_type = 0;
+
+					if (!(_pub_blocked)) {
+						if (_report_gps_pos_pub > 0) {
+							orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
+
+						} else {
+							_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
+						}
 					}
-				}
 
-				// GPS is obviously detected successfully, reset statistics
-				_Helper->reset_update_rates();
+					/* GPS is obviously detected successfully, reset statistics */
+					_Helper->reset_update_rates();
+				}
 
 				int helper_ret;
 				while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {

src/modules/commander/airspeed_calibration.cpp

@@ -72,12 +72,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;
@@ -201,10 +201,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 */
 		struct pollfd fds[1];
 		fds[0].fd = diff_pres_sub;

src/modules/commander/calibration_routines.cpp

@@ -469,8 +469,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/mag_calibration.cpp

@@ -64,9 +64,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]);
 
@@ -204,6 +206,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;
@@ -223,7 +229,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;
+	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;
@@ -344,8 +350,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++;
@@ -362,7 +368,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;
@@ -376,8 +382,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
@@ -496,6 +502,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", cur_mag, worker_data.calibration_counter_total[cur_mag]);
 
 			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
@@ -511,6 +521,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", cur_mag, 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/mavlink/mavlink_main.cpp

@@ -1440,15 +1440,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_RAW", 1.0f);
+		configure_stream("RC_CHANNELS_RAW", 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;

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -119,15 +119,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 */
 
@@ -135,16 +135,16 @@ 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 */
 
 
 	struct {
@@ -210,7 +210,7 @@ private:
 	/**
 	 * Update our local parameter cache.
 	 */
-	int			parameters_update(bool force);
+	int		parameters_update(bool force);
 
 	/**
 	 * Update control outputs

src/modules/navigator/mission.cpp

@@ -70,8 +70,8 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_dist_1wp(this, "MIS_DIST_1WP", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
 	_param_yawmode(this, "MIS_YAWMODE", false),
-	_onboard_mission({0}),
-	_offboard_mission({0}),
+	_onboard_mission{},
+	_offboard_mission{},
 	_current_onboard_mission_index(-1),
 	_current_offboard_mission_index(-1),
 	_need_takeoff(true),
@@ -110,32 +110,29 @@ Mission::on_inactive()
 			update_offboard_mission();
 		}
 
-		/* check if the home position became valid in the meantime */
-		if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) &&
-			!_home_inited && _navigator->home_position_valid()) {
+	} else {
 
-			dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+		/* load missions from storage */
+		mission_s mission_state;
 
-			_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
-					dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-					_navigator->get_home_position()->alt, _navigator->home_position_valid(),
-					_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-					_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
+		dm_lock(DM_KEY_MISSION_STATE);
 
-			_navigator->increment_mission_instance_count();
-			_navigator->set_mission_result_updated();
+		/* read current state */
+		int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
 
-			_home_inited = true;
+		dm_unlock(DM_KEY_MISSION_STATE);
+
+		if (read_res == sizeof(mission_s)) {
+			_offboard_mission.dataman_id = mission_state.dataman_id;
+			_offboard_mission.count = mission_state.count;
+			_current_offboard_mission_index = mission_state.current_seq;
 		}
 
-	} else {
-		/* read mission topics on initialization */
 		_inited = true;
-
-		update_onboard_mission();
-		update_offboard_mission();
 	}
 
+	check_mission_valid();
+
 	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
 		_need_takeoff = true;
@@ -151,6 +148,8 @@ Mission::on_activation()
 void
 Mission::on_active()
 {
+	check_mission_valid();
+
 	/* check if anything has changed */
 	bool onboard_updated = false;
 	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@@ -272,6 +271,8 @@ Mission::update_offboard_mission()
 		_offboard_mission.count = 0;
 		_offboard_mission.current_seq = 0;
 		_current_offboard_mission_index = 0;
+
+		warnx("mission check failed");
 	}
 
 	set_current_offboard_mission_item();
@@ -652,6 +653,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 
 		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
 			/* mission item index out of bounds */
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
 			return false;
 		}
 
@@ -795,3 +797,26 @@ Mission::set_mission_finished()
 	_navigator->get_mission_result()->finished = true;
 	_navigator->set_mission_result_updated();
 }
+
+bool
+Mission::check_mission_valid()
+{
+	/* check if the home position became valid in the meantime */
+	if (!_home_inited && _navigator->home_position_valid()) {
+
+		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+
+		_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
+				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
+				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
+				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+				_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
+
+		_navigator->increment_mission_instance_count();
+		_navigator->set_mission_result_updated();
+
+		_home_inited = true;
+	}
+
+	return _navigator->get_mission_result()->valid;
+}

src/modules/navigator/mission.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -39,6 +39,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Ban Siesta <bansiesta@gmail.com>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -165,6 +166,11 @@ private:
 	 */
 	void set_mission_finished();
 
+	/**
+	 * Check wether a mission is ready to go
+	 */
+	bool check_mission_valid();
+
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;

src/modules/sdlog2/sdlog2.c

@@ -156,7 +156,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++; \