ros: euroc master bringup/merge work

Makefile

@@ -1,5 +1,5 @@
 #
-#   Copyright (c) 2012, 2013 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
@@ -161,7 +161,7 @@ $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config))))
 #
 NUTTX_ARCHIVES		 = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export)
 .PHONY:			archives
-archives:		$(NUTTX_ARCHIVES)
+archives:		checksubmodules $(NUTTX_ARCHIVES)
 
 # We cannot build these parallel; note that we also force -j1 for the
 # sub-make invocations.
@@ -211,8 +211,7 @@ menuconfig:
 	@$(ECHO) ""
 endif
 
-$(NUTTX_SRC):
-	$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
+$(NUTTX_SRC): checksubmodules
 
 $(UAVCAN_DIR):
 	$(Q) (./Tools/check_submodules.sh)

Tools/check_submodules.sh

@@ -12,17 +12,16 @@ if [ -d NuttX/nuttx ];
 	if [ -z "$STATUSRETVAL" ]; then
 		echo "Checked NuttX submodule, correct version found"
 	else
+		echo ""
+		echo ""
+		echo "New commits required:"
+		echo "$(git submodule summary)"
 		echo ""
 		echo ""
 		echo "   NuttX sub repo not at correct version. Try 'git submodule update'"
 		echo "   or follow instructions on http://pixhawk.org/dev/git/submodules"
 		echo ""
 		echo "   DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!"
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
 		exit 1
 	fi
 else
@@ -37,15 +36,14 @@ if [ -d mavlink/include/mavlink/v1.0 ];
 	if [ -z "$STATUSRETVAL" ]; then
 		echo "Checked mavlink submodule, correct version found"
 	else
-		echo ""
-		echo ""
-		echo "mavlink sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
 		echo ""
 		echo ""
 		echo "New commits required:"
 		echo "$(git submodule summary)"
 		echo ""
+		echo ""
+		echo "mavlink sub repo not at correct version. Try 'git submodule update'"
+		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
 		exit 1
 	fi
 else
@@ -61,15 +59,58 @@ then
 	then
 		echo "Checked uavcan submodule, correct version found"
 	else
+		echo ""
+		echo ""
+		echo "New commits required:"
+		echo "$(git submodule summary)"
 		echo ""
 		echo ""
 		echo "uavcan sub repo not at correct version. Try 'git submodule update'"
 		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
+		exit 1
+	fi
+else
+	git submodule init;
+	git submodule update;
+fi
+
+if [ -d Tools/gencpp ]
+then
+	STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<")
+	if [ -z "$STATUSRETVAL" ]
+	then
+		echo "Checked gencpp submodule, correct version found"
+	else
 		echo ""
 		echo ""
 		echo "New commits required:"
 		echo "$(git submodule summary)"
 		echo ""
+		echo ""
+		echo "gencpp sub repo not at correct version. Try 'git submodule update'"
+		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
+		exit 1
+	fi
+else
+	git submodule init;
+	git submodule update;
+fi
+
+if [ -d Tools/genmsg ]
+then
+	STATUSRETVAL=$(git submodule summary | grep -A20 -i genmsg | grep "<")
+	if [ -z "$STATUSRETVAL" ]
+	then
+		echo "Checked genmsg submodule, correct version found"
+	else
+		echo ""
+		echo ""
+		echo "New commits required:"
+		echo "$(git submodule summary)"
+		echo ""
+		echo ""
+		echo "genmsg sub repo not at correct version. Try 'git submodule update'"
+		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
 		exit 1
 	fi
 else

Tools/px_generate_uorb_topic_headers.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 #############################################################################
 #
-#   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

launch/ardrone.launch

@@ -9,6 +9,7 @@
 	<param name="MPC_XY_VEL_I" type="double" value="0.0" />
 	<param name="MPC_XY_VEL_D" type="double" value="0.01" />
 	<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
+	<param name="vehicle_model" type="string" value="ardrone" />
 </group>
 
 </launch>

launch/iris.launch

@@ -10,6 +10,7 @@
 	<param name="MPC_XY_VEL_I" type="double" value="0.0" />
 	<param name="MPC_XY_VEL_D" type="double" value="0.01" />
 	<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
+	<param name="vehicle_model" type="string" value="iris" />
 </group>
 
 </launch>

src/drivers/drv_accel.h

@@ -97,6 +97,8 @@ ORB_DECLARE(sensor_accel);
 /** set the accel internal sample rate to at least (arg) Hz */
 #define ACCELIOCSSAMPLERATE	_ACCELIOC(0)
 
+#define ACCEL_SAMPLERATE_DEFAULT    1000003	/**< default sample rate */
+
 /** return the accel internal sample rate in Hz */
 #define ACCELIOCGSAMPLERATE	_ACCELIOC(1)
 

src/drivers/drv_gyro.h

@@ -93,6 +93,8 @@ ORB_DECLARE(sensor_gyro);
 /** set the gyro internal sample rate to at least (arg) Hz */
 #define GYROIOCSSAMPLERATE	_GYROIOC(0)
 
+#define GYRO_SAMPLERATE_DEFAULT    1000003	/**< default sample rate */
+
 /** return the gyro internal sample rate in Hz */
 #define GYROIOCGSAMPLERATE	_GYROIOC(1)
 

src/drivers/l3gd20/l3gd20.cpp

@@ -784,8 +784,9 @@ L3GD20::set_samplerate(unsigned frequency)
 {
 	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
 
-	if (frequency == 0)
+	if (frequency == 0 || frequency == GYRO_SAMPLERATE_DEFAULT) {
 		frequency = _is_l3g4200d ? 800 : 760;
+	}
 
 	/*
 	 * Use limits good for H or non-H models. Rates are slightly different

src/drivers/lsm303d/lsm303d.cpp

@@ -1307,8 +1307,9 @@ LSM303D::accel_set_samplerate(unsigned frequency)
 	uint8_t setbits = 0;
 	uint8_t clearbits = REG1_RATE_BITS_A;
 
-	if (frequency == 0)
+	if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) {
 		frequency = 1600;
+	}
 
 	if (frequency <= 100) {
 		setbits |= REG1_RATE_100HZ_A;

src/drivers/mpu6000/mpu6000.cpp

@@ -402,7 +402,7 @@ class MPU6000 : public device::SPI
 	/*
 	  set sample rate (approximate) - 1kHz to 5Hz
 	*/
-	void _set_sample_rate(uint16_t desired_sample_rate_hz);
+	void _set_sample_rate(unsigned desired_sample_rate_hz);
 
 	/*
 	  check that key registers still have the right value
@@ -794,13 +794,19 @@ MPU6000::probe()
   set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
 */
 void
-MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
+MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz)
 {
-  uint8_t div = 1000 / desired_sample_rate_hz;
-  if(div>200) div=200;
-  if(div<1) div=1;
-  write_checked_reg(MPUREG_SMPLRT_DIV, div-1);
-  _sample_rate = 1000 / div;
+	if (desired_sample_rate_hz == 0 ||
+			desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT ||
+			desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) {
+		desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE;
+	}
+
+	uint8_t div = 1000 / desired_sample_rate_hz;
+	if(div>200) div=200;
+	if(div<1) div=1;
+	write_checked_reg(MPUREG_SMPLRT_DIV, div-1);
+	_sample_rate = 1000 / div;
 }
 
 /*

src/modules/commander/accelerometer_calibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 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
@@ -221,17 +221,17 @@ int do_accel_calibration(int mavlink_fd)
 		accel_scale.z_scale = accel_T_rotated(2, 2);
 
 		/* set parameters */
-		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
-		    || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
-		    || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
-		    || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
-		    || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
-		    || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
+		if (param_set(param_find("CAL_ACC0_XOFF"), &(accel_scale.x_offset))
+		    || param_set(param_find("CAL_ACC0_YOFF"), &(accel_scale.y_offset))
+		    || param_set(param_find("CAL_ACC0_ZOFF"), &(accel_scale.z_offset))
+		    || param_set(param_find("CAL_ACC0_XSCALE"), &(accel_scale.x_scale))
+		    || param_set(param_find("CAL_ACC0_YSCALE"), &(accel_scale.y_scale))
+		    || param_set(param_find("CAL_ACC0_ZSCALE"), &(accel_scale.z_scale))) {
 			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			res = ERROR;
 		}
 
-		if (param_set(param_find("SENS_ACC_ID"), &(device_id))) {
+		if (param_set(param_find("CAL_ACC0_ID"), &(device_id))) {
 				res = ERROR;
 		}
 	}

src/modules/commander/gyro_calibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 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
@@ -51,6 +51,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
+#include <systemlib/mcu_version.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -80,6 +81,13 @@ int do_gyro_calibration(int mavlink_fd)
 
 	int res = OK;
 
+	/* store board ID */
+	uint32_t mcu_id[3];
+	mcu_unique_id(&mcu_id[0]);
+
+	/* store last 32bit number - not unique, but unique in a given set */
+	param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
+
 	/* reset all offsets to zero and all scales to one */
 	int fd = open(GYRO_DEVICE_PATH, 0);
 
@@ -149,9 +157,9 @@ int do_gyro_calibration(int mavlink_fd)
 
 	if (res == OK) {
 		/* set offset parameters to new values */
-		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
-		    || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
-		    || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
+		if (param_set(param_find("CAL_GYRO0_XOFF"), &(gyro_scale.x_offset))
+		    || param_set(param_find("CAL_GYRO0_YOFF"), &(gyro_scale.y_offset))
+		    || param_set(param_find("CAL_GYRO0_ZOFF"), &(gyro_scale.z_offset))) {
 			mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
 			res = ERROR;
 		}
@@ -275,13 +283,13 @@ int do_gyro_calibration(int mavlink_fd)
 
 	if (res == OK) {
 		/* set scale parameters to new values */
-		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
-		    || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
-		    || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
+		if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale))
+		    || param_set(param_find("CAL_GYRO0_YSCALE"), &(gyro_scale.y_scale))
+		    || param_set(param_find("CAL_GYRO0_ZSCALE"), &(gyro_scale.z_scale))) {
 			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
 			res = ERROR;
 		}
-		if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) {
+		if (param_set(param_find("CAL_GYRO0_ID"), &(device_id))) {
 				res = ERROR;
 			}
 	}

src/modules/commander/mag_calibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 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
@@ -263,30 +263,30 @@ int do_mag_calibration(int mavlink_fd)
 
 		if (res == OK) {
 			/* set parameters */
-			if (param_set(param_find("SENS_MAG_ID"), &(device_id))) {
+			if (param_set(param_find("CAL_MAG0_ID"), &(device_id))) {
 				res = ERROR;
 			}
-			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
+			if (param_set(param_find("CAL_MAG0_XOFF"), &(mscale.x_offset))) {
 				res = ERROR;
 			}
 
-			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
+			if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) {
 				res = ERROR;
 			}
 
-			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
+			if (param_set(param_find("CAL_MAG0_ZOFF"), &(mscale.z_offset))) {
 				res = ERROR;
 			}
 
-			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
+			if (param_set(param_find("CAL_MAG0_XSCALE"), &(mscale.x_scale))) {
 				res = ERROR;
 			}
 
-			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
+			if (param_set(param_find("CAL_MAG0_YSCALE"), &(mscale.y_scale))) {
 				res = ERROR;
 			}
 
-			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
+			if (param_set(param_find("CAL_MAG0_ZSCALE"), &(mscale.z_scale))) {
 				res = ERROR;
 			}
 

src/modules/navigator/navigator_main.cpp

@@ -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
@@ -247,9 +247,6 @@ Navigator::task_main_trampoline(int argc, char *argv[])
 void
 Navigator::task_main()
 {
-	/* inform about start */
-	warnx("Initializing..");
-
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	_geofence.setMavlinkFd(_mavlink_fd);
 
@@ -263,7 +260,7 @@ Navigator::task_main()
 		_geofence.loadFromFile(GEOFENCE_FILENAME);
 
 	} else {
-		mavlink_log_critical(_mavlink_fd, "#audio: No geofence file");
+		mavlink_log_info(_mavlink_fd, "No geofence set");
 		if (_geofence.clearDm() > 0)
 			warnx("Geofence cleared");
 		else