Merge branch 'fmuv2_bringup' of github.com:cvg/Firmware_Private into fmuv2_bringup

6 files changed, 104 insertions, 73 deletions

diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330
index 4107fab4f..07e70993d 100644
--- a/ROMFS/px4fmu_common/init.d/10_io_f330
+++ b/ROMFS/px4fmu_common/init.d/10_io_f330
@@ -15,20 +15,19 @@ then
 	# Set all params here, then disable autoconfig
 	param set SYS_AUTOCONFIG 0
 
-	param set MC_ATTRATE_D	         0.007
+	param set MC_ATTRATE_D	         0.005
 	param set MC_ATTRATE_I	         0.0
-	param set MC_ATTRATE_P	         0.13
+	param set MC_ATTRATE_P	         0.1
 	param set MC_ATT_D	         0.0
 	param set MC_ATT_I	         0.0
-	param set MC_ATT_P	         7.0
-	param set MC_POS_P	         0.1
+	param set MC_ATT_P	         4.5
 	param set MC_RCLOSS_THR	         0.0
 	param set MC_YAWPOS_D	         0.0
-	param set MC_YAWPOS_I	         0.5
-	param set MC_YAWPOS_P	         1.0
+	param set MC_YAWPOS_I	         0.3
+	param set MC_YAWPOS_P	         0.6
 	param set MC_YAWRATE_D	         0.0
 	param set MC_YAWRATE_I	         0.0
-	param set MC_YAWRATE_P	         0.2
+	param set MC_YAWRATE_P	         0.1
 
 	param save /fs/microsd/params
 fi
@@ -128,7 +127,7 @@ multirotor_att_control start
 #
 # Start logging
 #
-sdlog2 start -r 20 -a -b 14
+sdlog2 start -r 20 -a -b 16
  
 #
 # Start system state
diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_init.c b/src/drivers/boards/px4fmuv2/px4fmu2_init.c
index 535e075b2..13829d5c4 100644
--- a/src/drivers/boards/px4fmuv2/px4fmu2_init.c
+++ b/src/drivers/boards/px4fmuv2/px4fmu2_init.c
@@ -193,8 +193,8 @@ __EXPORT int nsh_archinitialize(void)
 		       NULL);
 
 	/* initial LED state */
-	//drv_led_start();
-	up_ledoff(LED_AMBER);
+	drv_led_start();
+	led_off(LED_AMBER);
 
 	/* Configure SPI-based devices */
 
diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmuv2/px4fmu2_led.c
index 85177df56..11a5c7211 100644
--- a/src/drivers/boards/px4fmuv2/px4fmu2_led.c
+++ b/src/drivers/boards/px4fmuv2/px4fmu2_led.c
@@ -68,7 +68,7 @@ __EXPORT void led_init()
 
 __EXPORT void led_on(int led)
 {
-	if (led == 0)
+	if (led == 1)
 	{
 		/* Pull down to switch on */
 		stm32_gpiowrite(GPIO_LED1, false);
@@ -77,7 +77,7 @@ __EXPORT void led_on(int led)
 
 __EXPORT void led_off(int led)
 {
-	if (led == 0)
+	if (led == 1)
 	{
 		/* Pull up to switch off */
 		stm32_gpiowrite(GPIO_LED1, true);
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 122cf0cec..d9268c0b3 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -111,9 +111,8 @@ protected:
 
 	ms5611::prom_s		_prom;
 
-	struct hrt_call		_baro_call;
-
-	unsigned _call_baro_interval;
+	struct work_s		_work;
+	unsigned		_measure_ticks;
 
 	unsigned		_num_reports;
 	volatile unsigned	_next_report;
@@ -144,12 +143,12 @@ protected:
 	 * @note This function is called at open and error time.  It might make sense
 	 *       to make it more aggressive about resetting the bus in case of errors.
 	 */
-	void			start();
+	void			start_cycle();
 
 	/**
 	 * Stop the automatic measurement state machine.
 	 */
-	void			stop();
+	void			stop_cycle();
 
 	/**
 	 * Perform a poll cycle; collect from the previous measurement
@@ -167,11 +166,12 @@ protected:
 	void			cycle();
 
 	/**
-	 * Static trampoline; XXX is this needed?
+	 * Static trampoline from the workq context; because we don't have a
+	 * generic workq wrapper yet.
 	 *
 	 * @param arg		Instance pointer for the driver that is polling.
 	 */
-	void			cycle_trampoline(void *arg);
+	static void		cycle_trampoline(void *arg);
 
 	/**
 	 * Issue a measurement command for the current state.
@@ -184,7 +184,6 @@ protected:
 	 * Collect the result of the most recent measurement.
 	 */
 	virtual int		collect();
-
 };
 
 /*
@@ -196,7 +195,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
 	CDev("MS5611", BARO_DEVICE_PATH),
 	_interface(interface),
 	_prom(prom_buf.s),
-	_call_baro_interval(0),
+	_measure_ticks(0),
 	_num_reports(0),
 	_next_report(0),
 	_oldest_report(0),
@@ -213,13 +212,14 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
 	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
 {
-
+	// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
 }
 
 MS5611::~MS5611()
 {
 	/* make sure we are truly inactive */
-	stop();
+	stop_cycle();
 
 	/* free any existing reports */
 	if (_reports != nullptr)
@@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		return -ENOSPC;
 
 	/* if automatic measurement is enabled */
-	if (_call_baro_interval > 0) {
+	if (_measure_ticks > 0) {
 
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
@@ -298,13 +298,41 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 
 	/* manual measurement - run one conversion */
 	/* XXX really it'd be nice to lock against other readers here */
-	_measure_phase = 0;
-	_oldest_report = _next_report = 0;
-	measure();
+	do {
+		_measure_phase = 0;
+		_oldest_report = _next_report = 0;
+
+		/* do temperature first */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		usleep(MS5611_CONVERSION_INTERVAL);
+
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* now do a pressure measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
 
-	/* state machine will have generated a report, copy it out */
-	memcpy(buffer, _reports, sizeof(*_reports));
-	ret = sizeof(*_reports);
+		usleep(MS5611_CONVERSION_INTERVAL);
+
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		memcpy(buffer, _reports, sizeof(*_reports));
+		ret = sizeof(*_reports);
+
+	} while (0);
 
 	return ret;
 }
@@ -319,8 +347,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 				/* switching to manual polling */
 			case SENSOR_POLLRATE_MANUAL:
-				stop();
-				_call_baro_interval = 0;
+				stop_cycle();
+				_measure_ticks = 0;
 				return OK;
 
 				/* external signalling not supported */
@@ -334,15 +362,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT: {
 					/* do we need to start internal polling? */
-					bool want_start = (_call_baro_interval == 0);
-
-					/* set interval to minimum legal value */
-					_baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL;
+					bool want_start = (_measure_ticks == 0);
 
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start)
-						start();
+						start_cycle();
 
 					return OK;
 				}
@@ -350,18 +377,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* adjust to a legal polling interval in Hz */
 			default: {
 					/* do we need to start internal polling? */
-					bool want_start = (_call_baro_interval == 0);
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (1000000/arg < MS5611_CONVERSION_INTERVAL)
+					if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
 						return -EINVAL;
 
-					/* update interval */
-					_baro_call.period = _call_baro_interval =  1000000/arg;
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start)
-						start();
+						start_cycle();
 
 					return OK;
 				}
@@ -369,10 +399,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		}
 
 	case SENSORIOCGPOLLRATE:
-		if (_call_baro_interval == 0)
+		if (_measure_ticks == 0)
 			return SENSOR_POLLRATE_MANUAL;
 
-		return (1000 / _call_baro_interval);
+		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
 			/* add one to account for the sentinel in the ring */
@@ -389,11 +419,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 				return -ENOMEM;
 
 			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
+			stop_cycle();
 			delete[] _reports;
 			_num_reports = arg;
 			_reports = buf;
-			start();
+			start_cycle();
 
 			return OK;
 		}
@@ -427,32 +457,30 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 }
 
 void
-MS5611::start()
+MS5611::start_cycle()
 {
-	/* make sure we are stopped first */
-	stop();
 
-	/* reset the report ring */
-	_oldest_report = _next_report = 0;
-	/* reset to first measure phase */
+	/* reset the report ring and state machine */
+	_collect_phase = false;
 	_measure_phase = 0;
+	_oldest_report = _next_report = 0;
 
 	/* schedule a cycle to start things */
-	hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this);
+	work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
 }
 
 void
-MS5611::stop()
+MS5611::stop_cycle()
 {
-	hrt_cancel(&_baro_call);
+	work_cancel(HPWORK, &_work);
 }
 
 void
 MS5611::cycle_trampoline(void *arg)
 {
-	MS5611 *dev = (MS5611 *)arg;
+	MS5611 *dev = reinterpret_cast<MS5611 *>(arg);
 
-	cycle();
+	dev->cycle();
 }
 
 void
@@ -476,7 +504,7 @@ MS5611::cycle()
 				//log("collection error %d", ret);
 			}
 			/* reset the collection state machine and try again */
-			start();
+			start_cycle();
 			return;
 		}
 
@@ -489,16 +517,14 @@ MS5611::cycle()
 		 * doing pressure measurements at something close to the desired rate.
 		 */
 		if ((_measure_phase != 0) &&
-		    (_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
-
-			// XXX maybe do something appropriate as well
+		    (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
 
-//			/* schedule a fresh cycle call when we are ready to measure again */
-//			work_queue(HPWORK,
-//				   &_work,
-//				   (worker_t)&MS5611::cycle_trampoline,
-//				   this,
-//				   _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&MS5611::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
 
 			return;
 		}
@@ -509,12 +535,19 @@ MS5611::cycle()
 	if (ret != OK) {
 		//log("measure error %d", ret);
 		/* reset the collection state machine and try again */
-		start();
+		start_cycle();
 		return;
 	}
 
 	/* next phase is collection */
 	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&MS5611::cycle_trampoline,
+		   this,
+		   USEC2TICK(MS5611_CONVERSION_INTERVAL));
 }
 
 int
@@ -663,14 +696,13 @@ MS5611::collect()
 	return OK;
 }
 
-
 void
 MS5611::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
-	printf("poll interval:  %u ticks\n", _call_baro_interval);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
 	printf("report queue:   %u (%u/%u @ %p)\n",
 	       _num_reports, _oldest_report, _next_report, _reports);
 	printf("TEMP:           %d\n", _TEMP);
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c
index dc653a079..6a304896a 100644
--- a/src/modules/commander/accelerometer_calibration.c
+++ b/src/modules/commander/accelerometer_calibration.c
@@ -268,8 +268,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
 	/* EMA time constant in seconds*/
 	float ema_len = 0.2f;
-	/* set "still" threshold to 0.1 m/s^2 */
-	float still_thr2 = pow(0.1f, 2);
+	/* set "still" threshold to 0.25 m/s^2 */
+	float still_thr2 = pow(0.25f, 2);
 	/* set accel error threshold to 5m/s^2 */
 	float accel_err_thr = 5.0f;
 	/* still time required in us */
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b2a6c6a79..5722d2fdf 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -391,7 +391,7 @@ namespace sensors
 #endif
 static const int ERROR = -1;
 
-Sensors	*g_sensors;
+Sensors	*g_sensors = nullptr;
 }
 
 Sensors::Sensors() :