Merge remote-tracking branch 'upstream/master' into linux

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Conflicts:
	makefiles/firmware.mk
	src/modules/commander/module.mk
	src/modules/mavlink/mavlink_ftp.h
	src/modules/mavlink/mavlink_tests/module.mk

src/modules/commander/calibration_routines.cpp

@@ -47,12 +47,12 @@
 #include <float.h>
 #include <poll.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_accel.h>
 #include <mavlink/mavlink_log.h>
 #include <geo/geo.h>
 #include <string.h>
 
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/sensor_combined.h>
 
 #include "calibration_routines.h"
 #include "calibration_messages.h"
@@ -237,7 +237,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 {
 	const unsigned ndim = 3;
 	
-	struct accel_report sensor;
+	struct sensor_combined_s sensor;
 	float		accel_ema[ndim] = { 0.0f };		// exponential moving average of accel
 	float		accel_disp[3] = { 0.0f, 0.0f, 0.0f };	// max-hold dispersion of accel
 	float		ema_len = 0.5f;				// EMA time constant in seconds
@@ -265,7 +265,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 		int poll_ret = px4_poll(fds, 1, 1000);
 		
 		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_accel), accel_sub, &sensor);
+			orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
 			t = hrt_absolute_time();
 			float dt = (t - t_prev) / 1000000.0f;
 			t_prev = t;
@@ -276,13 +276,13 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 				float di = 0.0f;
 				switch (i) {
 					case 0:
-						di = sensor.x;
+						di = sensor.accelerometer_m_s2[0];
 						break;
 					case 1:
-						di = sensor.y;
+						di = sensor.accelerometer_m_s2[1];
 						break;
 					case 2:
-						di = sensor.z;
+						di = sensor.accelerometer_m_s2[2];
 						break;
 				}
 				
@@ -411,7 +411,7 @@ calibrate_return calibrate_from_orientation(int		mavlink_fd,
 	
 	// Setup subscriptions to onboard accel sensor
 	
-	int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
+	int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
 	if (sub_accel < 0) {
 		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
 		return calibrate_return_error;

src/modules/commander/commander.cpp

@@ -119,6 +119,7 @@
 #include "baro_calibration.h"
 #include "rc_calibration.h"
 #include "airspeed_calibration.h"
+#include "esc_calibration.h"
 #include "PreflightCheck.h"
 
 /* oddly, ERROR is not defined for c++ */
@@ -1166,7 +1167,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* initialize low priority thread */
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2000);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2600);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -2717,6 +2718,16 @@ void *commander_low_prio_loop(void *arg)
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 						calib_ret = do_airspeed_calibration(mavlink_fd);
 
+					} else if ((int)(cmd.param7) == 1) {
+						/* do esc calibration */
+						calib_ret = check_if_batt_disconnected(mavlink_fd);
+						if(calib_ret == OK) {
+							answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED);
+							armed.in_esc_calibration_mode = true;
+							calib_ret = do_esc_calibration(mavlink_fd);
+							armed.in_esc_calibration_mode = false;
+						}
+
 					} else if ((int)(cmd.param4) == 0) {
 						/* RC calibration ended - have we been in one worth confirming? */
 						if (status.rc_input_blocked) {
@@ -2726,6 +2737,8 @@ void *commander_low_prio_loop(void *arg)
 							mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
                             calib_ret = OK;
 						}
+						/* this always succeeds */
+						calib_ret = OK;
 					}
 
 					if (calib_ret == OK) {

src/modules/commander/esc_calibration.cpp

@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file esc_calibration.cpp
+ *
+ * Definition of esc calibration
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
+#include "esc_calibration.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <systemlib/err.h>
+#include <fcntl.h>
+#include <poll.h>
+#include "drivers/drv_pwm_output.h"
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/uORB.h>
+#include <drivers/drv_hrt.h>
+#include <mavlink/mavlink_log.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int check_if_batt_disconnected(int mavlink_fd) {
+	struct battery_status_s battery;
+	memset(&battery,0,sizeof(battery));
+	int batt_sub = orb_subscribe(ORB_ID(battery_status));
+	orb_copy(ORB_ID(battery_status), batt_sub, &battery);
+
+	if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) {
+		mavlink_log_info(mavlink_fd, "Please disconnect battery and try again!");
+		return ERROR;
+	}
+	return OK;
+}
+
+
+int do_esc_calibration(int mavlink_fd) {
+
+	int fd = open(PWM_OUTPUT0_DEVICE_PATH, 0);
+	int ret;
+
+	if(fd < 0) {
+		err(1,"Can't open %s", PWM_OUTPUT0_DEVICE_PATH);
+	}
+
+	/* tell IO/FMU that its ok to disable its safety with the switch */
+	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_SET_ARM_OK");
+	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
+	ret = ioctl(fd, PWM_SERVO_ARM, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_ARM");
+	/* tell IO to switch off safety without using the safety switch */
+	ret = ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
+	if(ret!=0) {
+		err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF");
+	}
+
+	mavlink_log_info(mavlink_fd,"Please connect battery now");
+
+	struct battery_status_s battery;
+	memset(&battery,0,sizeof(battery));
+	int batt_sub = orb_subscribe(ORB_ID(battery_status));
+	orb_copy(ORB_ID(vehicle_command),batt_sub, &battery);
+	bool updated = false;
+
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	orb_copy(ORB_ID(vehicle_command),cmd_sub, &cmd);
+
+	/* wait for one of the following events:
+		1) user has pressed the button in QGroundControl
+		2) timeout of 5 seconds is reached
+	*/
+	hrt_abstime start_time = hrt_absolute_time();
+
+	while(true) {
+		orb_check(batt_sub,&updated);
+		if(updated) {
+			orb_copy(ORB_ID(battery_status), batt_sub, &battery);
+		}
+		// user has connected battery
+		if(battery.voltage_filtered_v > 3.0f) {
+			orb_check(cmd_sub,&updated);
+			if(updated) {
+				orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+			}
+			if((int)(cmd.param7) == 2 && cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION) {
+				break;
+			} else if (hrt_absolute_time() - start_time > 5000000) {
+				// waited for 5 seconds, switch to low pwm
+				break;
+			}
+		}
+		else {
+			start_time = hrt_absolute_time();
+		}
+		usleep(50000);
+	}
+
+	/* disarm */
+	ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_DISARM");
+
+	mavlink_log_info(mavlink_fd,"ESC calibration finished");
+	return OK;
+ }

src/modules/commander/esc_calibration.h

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file esc_calibration.h
+ *
+ * Definition of esc calibration
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
+#ifndef ESC_CALIBRATION_H_
+#define ESC_CALIBRATION_H_
+int check_if_batt_disconnected(int mavlink_fd);
+int do_esc_calibration(int mavlink_fd);
+
+#endif

src/modules/commander/module.mk

@@ -38,16 +38,17 @@
 MODULE_COMMAND	 	= commander
 SRCS		 	= commander.cpp \
 			commander_params.c \
+			state_machine_helper.cpp \
 			commander_helper.cpp \
 			calibration_routines.cpp \
-			mag_calibration.cpp \
-			gyro_calibration.cpp \
-			baro_calibration.cpp \
 			accelerometer_calibration.cpp \
+			gyro_calibration.cpp \
+			mag_calibration.cpp \
+			baro_calibration.cpp \
 			rc_calibration.cpp \
 			airspeed_calibration.cpp \
-			PreflightCheck.cpp \
-			state_machine_helper.cpp 
+			esc_calibration.cpp \
+			PreflightCheck.cpp
 
 MODULE_STACKSIZE = 5000