Merge 'master'

Makefile

@@ -37,6 +37,7 @@ TARGETS	:= nuttx posix qurt
 EXPLICIT_TARGET	:= $(filter $(TARGETS),$(MAKECMDGOALS))
 ifneq ($(EXPLICIT_TARGET),)
     export PX4_TARGET_OS=$(EXPLICIT_TARGET)
+    export GOALS := $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS))
 endif
 
 #
@@ -277,14 +278,12 @@ testbuild:
 	$(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8)
 	$(Q) (zip -r Firmware.zip $(PX4_BASE)/Images)
 
-nuttx: 
-	make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS))
-
-posix: 
-	make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS))
-
-qurt: 
-	make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS))
+nuttx posix qurt: 
+ifeq ($(GOALS),)
+	make PX4_TARGET_OS=$@ $(GOALS)
+else
+	export PX4_TARGET_OS=$@
+endif
 
 posixrun:
 	Tools/posix_run.sh

msg/vehicle_status.msg

@@ -91,6 +91,7 @@ uint8 nav_state				# set navigation state machine to specified value
 uint8 arming_state			# current arming state
 uint8 hil_state				# current hil state
 bool failsafe				# true if system is in failsafe state
+bool calibration_enabled		# true if current calibrating parts of the system. Also sets the system to ARMING_STATE_INIT.
 
 int32 system_type			# system type, inspired by MAVLink's VEHICLE_TYPE enum
 int32 system_id				# system id, inspired by MAVLink's system ID field

src/drivers/device/vdev.cpp

@@ -64,7 +64,7 @@ private:
 	px4_dev_t() {}
 };
 
-#define PX4_MAX_DEV 30
+#define PX4_MAX_DEV 50
 static px4_dev_t *devmap[PX4_MAX_DEV];
 
 /*
@@ -99,7 +99,7 @@ VDev::~VDev()
 int
 VDev::register_class_devname(const char *class_devname)
 {
-	PX4_DEBUG("VDev::register_class_devname");
+	PX4_DEBUG("VDev::register_class_devname %s", class_devname);
 	if (class_devname == nullptr) {
 		return -EINVAL;
 	}
@@ -124,7 +124,7 @@ VDev::register_class_devname(const char *class_devname)
 int
 VDev::register_driver(const char *name, void *data)
 {
-	PX4_DEBUG("VDev::register_driver");
+	PX4_DEBUG("VDev::register_driver %s", name);
 	int ret = -ENOSPC;
 
 	if (name == NULL || data == NULL)
@@ -145,14 +145,17 @@ VDev::register_driver(const char *name, void *data)
 			break;
 		}
 	}
+	if (ret != PX4_OK) {
+		PX4_ERR("No free devmap entries - increase PX4_MAX_DEV");
+	}
 	return ret;
 }
 
 int
 VDev::unregister_driver(const char *name)
 {
-	PX4_DEBUG("VDev::unregister_driver");
-	int ret = -ENOSPC;
+	PX4_DEBUG("VDev::unregister_driver %s", name);
+	int ret = -EINVAL;
 
 	if (name == NULL)
 		return -EINVAL;

src/modules/commander/accelerometer_calibration.cpp

@@ -559,7 +559,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
 
 int do_level_calibration(int mavlink_fd) {
 	const unsigned cal_time = 5;
-	const unsigned cal_hz = 250;
+	const unsigned cal_hz = 100;
 	const unsigned settle_time = 30;
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	struct vehicle_attitude_s att;
@@ -614,6 +614,8 @@ int do_level_calibration(int mavlink_fd) {
 		pitch_mean /= counter;
 	} else {
 		mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken");
+		success = false;
+		goto out;
 	}
 
 	if (fabsf(roll_mean) > 0.8f ) {
@@ -628,6 +630,7 @@ int do_level_calibration(int mavlink_fd) {
 		success = true;
 	}
 
+out:
 	if (success) {
 		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level");
 		return 0;

src/modules/commander/commander.cpp

@@ -1611,7 +1611,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* End battery voltage check */
 
 		/* If in INIT state, try to proceed to STANDBY state */
-		if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
+		if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 			arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
 							     mavlink_fd);
 
@@ -2698,6 +2698,8 @@ void *commander_low_prio_loop(void *arg)
 							false /* fRunPreArmChecks */, mavlink_fd)) {
 						answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 						break;
+					} else {
+						status.calibration_enabled = true;
 					}
 
 					if ((int)(cmd.param1) == 1) {
@@ -2757,12 +2759,14 @@ void *commander_low_prio_loop(void *arg)
 							/* enable RC control input */
 							status.rc_input_blocked = false;
 							mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
-                            calib_ret = OK;
+							calib_ret = OK;
 						}
 						/* this always succeeds */
 						calib_ret = OK;
 					}
 
+					status.calibration_enabled = false;
+
 					if (calib_ret == OK) {
 						tune_positive(true);
 
@@ -2780,7 +2784,7 @@ void *commander_low_prio_loop(void *arg)
 
 						status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
 
-						arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+						arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);
 
 					} else {
 						tune_negative(true);

src/modules/commander/state_machine_helper.cpp

@@ -219,23 +219,13 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			valid_transition = true;
 		}
 
-		// Sensors need to be initialized for STANDBY state, except for HIL
-		if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
-			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
-			(!status->condition_system_sensors_initialized)) {
-			mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
-			feedback_provided = true;
-			valid_transition = false;
-			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
-		}
-
 		// Check if we are trying to arm, checks look good but we are in STANDBY_ERROR
 		if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
 
 			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 
 				if (status->condition_system_sensors_initialized) {
-					mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
+					mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming");
 				} else {
 					mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
 				}
@@ -243,11 +233,24 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 			} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
 					status->condition_system_sensors_initialized) {
-				mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete");
+				mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete");
 				feedback_provided = true;
 			} else {
-
+				// Silent ignore
+				feedback_provided = true;
 			}
+
+		// Sensors need to be initialized for STANDBY state, except for HIL
+		} else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
+			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
+			(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
+			(!status->condition_system_sensors_initialized)) {
+			if (!fRunPreArmChecks) {
+				mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
+			}
+			feedback_provided = true;
+			valid_transition = false;
+			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
 		}
 
 		// Finish up the state transition

src/modules/dataman/dataman.c

@@ -131,8 +131,8 @@ static sem_t g_sys_state_mutex;
 
 /* The data manager store file handle and file name */
 static int g_fd = -1, g_task_fd = -1;
-// FIXME - need a configurable path that is not OS specific
-static const char *k_data_manager_device_path = "/fs/microsd/dataman";
+static const char *default_device_path = "/fs/microsd/dataman";
+static char *k_data_manager_device_path = NULL;
 
 /* The data manager work queues */
 
@@ -671,7 +671,12 @@ task_main(int argc, char *argv[])
 	}
 
 	/* Open or create the data manager file */
-	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, 0x0777);
+	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY
+#ifdef __PX4_LINUX
+			// Open with read/write permission for user
+			, S_IRUSR | S_IWUSR
+#endif
+			);
 
 	if (g_task_fd < 0) {
 		warnx("Could not open data manager file %s", k_data_manager_device_path);
@@ -831,7 +836,7 @@ stop(void)
 static void
 usage(void)
 {
-	warnx("usage: dataman {start|stop|status|poweronrestart|inflightrestart}");
+	warnx("usage: dataman {start [-f datafile]|stop|status|poweronrestart|inflightrestart}");
 }
 
 int
@@ -848,11 +853,20 @@ dataman_main(int argc, char *argv[])
 			warnx("dataman already running");
 			return -1;
 		}
+		if (argc == 4 && strcmp(argv[2],"-f") == 0) {
+			k_data_manager_device_path = strdup(argv[3]);
+			warnx("dataman file set to: %s\n", k_data_manager_device_path);
+		}
+		else {
+			k_data_manager_device_path = strdup(default_device_path);
+		}
 
 		start();
 
 		if (g_fd < 0) {
 			warnx("dataman start failed");
+			free(k_data_manager_device_path);
+			k_data_manager_device_path = NULL;
 			return -1;
 		}
 
@@ -866,8 +880,11 @@ dataman_main(int argc, char *argv[])
 		return -1;
 	}
 
-	if (!strcmp(argv[1], "stop"))
+	if (!strcmp(argv[1], "stop")) {
 		stop();
+		free(k_data_manager_device_path);
+		k_data_manager_device_path = NULL;
+	}
 	else if (!strcmp(argv[1], "status"))
 		status();
 	else if (!strcmp(argv[1], "poweronrestart"))

src/modules/navigator/mission.cpp

@@ -207,6 +207,8 @@ Mission::update_onboard_mission()
 void
 Mission::update_offboard_mission()
 {
+	bool failed = true;
+
 	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
 		warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq);
 		/* determine current index */
@@ -228,12 +230,15 @@ Mission::update_offboard_mission()
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
+		failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt);
 
 	} else {
 		warnx("offboard mission update failed");
+	}
+
+	if (failed) {
 		_offboard_mission.count = 0;
 		_offboard_mission.current_seq = 0;
 		_current_offboard_mission_index = 0;

src/modules/navigator/mission_feasibility_checker.cpp

@@ -65,6 +65,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
 
 bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
 {
+	bool failed = false;
 	/* Init if not done yet */
 	init();
 
@@ -74,11 +75,16 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
 		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	}
 
+	// check if all mission item commands are supported
+	failed |= !checkMissionItemValidity(dm_current, nMissionItems);
+
 
 	if (isRotarywing)
-		return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
+		failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
 	else
-		return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
+		failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
+
+	return !failed;
 }
 
 bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
@@ -163,6 +169,38 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 	return true;
 }
 
+bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) {
+	// do not allow mission if we find unsupported item
+	for (size_t i = 0; i < nMissionItems; i++) {
+		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.
+			mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access mission from SD card");
+			return false;
+		}
+
+		// check if we find unsupported item and reject mission if so
+		if (missionitem.nav_cmd != NAV_CMD_IDLE &&
+			missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
+			missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
+			missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
+			missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
+			missionitem.nav_cmd != NAV_CMD_LAND &&
+			missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
+			missionitem.nav_cmd != NAV_CMD_ROI &&
+			missionitem.nav_cmd != NAV_CMD_PATHPLANNING &&
+			missionitem.nav_cmd != NAV_CMD_DO_JUMP) {
+
+			mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1));
+			return false;
+		}
+	}
+	mavlink_log_critical(_mavlink_fd, "Mission is valid!");
+	return true;
+}
+
 bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
 {
 	/* Go through all mission items and search for a landing waypoint

src/modules/navigator/mission_feasibility_checker.h

@@ -62,6 +62,7 @@ private:
 	/* Checks for all airframes */
 	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
 	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false);
+	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
 
 	/* Checks specific to fixedwing airframes */
 	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);

src/modules/sensors/sensors.cpp

@@ -922,16 +922,16 @@ Sensors::gyro_init()
 		warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH);
 		return ERROR;
 
-	} else {
-
-		/* set the gyro internal sampling rate to default rate */
-		px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT);
-
-		/* set the driver to poll at default rate */
-		px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
-
 	}
 
+	/* set the gyro internal sampling rate to default rate */
+	px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT);
+
+	/* set the driver to poll at default rate */
+	px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
+
+	px4_close(fd);
+
 	return OK;
 }
 
@@ -2086,8 +2086,12 @@ Sensors::task_main()
 	} while (0);
 
 	if (ret) {
+		warnx("Sensor initialization failed");
 		_sensors_task = -1;
-		_exit(ret);
+		if (_fd_adc >=0) {
+			close(_fd_adc);
+			_fd_adc = -1;
+		}
 		return;
 	}
 

src/modules/simulator/simulator.cpp

@@ -63,6 +63,9 @@ Simulator *Simulator::getInstance()
 
 bool Simulator::getMPUReport(uint8_t *buf, int len)
 {
+	// Reads are paced from reading gyrosim and if
+	// we don't delay here we read too fast
+	usleep(50000);
 	return _mpu.copyData(buf, len);
 }
 

src/platforms/posix/px4_layer/work_thread.c

@@ -138,10 +138,11 @@ static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
 
           work_unlock(lock_id);
 	  if (!worker) {
-             printf("MESSED UP: worker = 0\n");
+             PX4_ERR("MESSED UP: worker = 0");
           }
-          else
+          else {
             worker(arg);
+          }
 
           /* Now, unfortunately, since we re-enabled interrupts we don't
            * know the state of the work list and we will have to start

src/platforms/px4_log.h

@@ -50,7 +50,6 @@
 	printf(__VA_ARGS__);\
 	printf(" (file %s line %d)\n", __FILE__, __LINE__);\
 }
-
 #if defined(__PX4_QURT)
 #include <stdio.h>
 
@@ -61,8 +60,14 @@
 
 #elif defined(__PX4_LINUX)
 #include <stdio.h>
+#include <pthread.h>
+
+#define __px4_log_threads(level, ...)   { \
+	printf("%-5s %ld ", level, pthread_self());\
+	printf(__VA_ARGS__);\
+	printf(" (file %s line %d)\n", __FILE__, __LINE__);\
+}
 
-//#define PX4_DEBUG(...)	{ }
 #define PX4_DEBUG(...) 	__px4_log_omit("DEBUG", __VA_ARGS__);
 #define PX4_INFO(...) 	__px4_log("INFO",  __VA_ARGS__);
 #define PX4_WARN(...) 	__px4_log_verbose("WARN",  __VA_ARGS__);