Merge branch 'master'

src/modules/commander/calibration_messages.h

@@ -52,6 +52,7 @@
 #define CAL_QGC_STARTED_MSG			"[cal] calibration started: 1 %s"
 #define CAL_QGC_DONE_MSG			"[cal] calibration done: %s"
 #define CAL_QGC_FAILED_MSG			"[cal] calibration failed: %s"
+#define CAL_QGC_WARNING_MSG			"[cal] calibration warning: %s"
 #define CAL_QGC_CANCELLED_MSG			"[cal] calibration cancelled"
 #define CAL_QGC_PROGRESS_MSG			"[cal] progress <%u>"
 #define CAL_QGC_ORIENTATION_DETECTED_MSG	"[cal] %s orientation detected"

src/modules/commander/commander.cpp

@@ -326,6 +326,8 @@ int commander_main(int argc, char *argv[])
 				calib_ret = do_gyro_calibration(mavlink_fd);
 			} else if (!strcmp(argv[2], "level")) {
 				calib_ret = do_level_calibration(mavlink_fd);
+			} else if (!strcmp(argv[2], "esc")) {
+				calib_ret = do_esc_calibration(mavlink_fd, &armed);
 			} else {
 				warnx("argument %s unsupported.", argv[2]);
 			}
@@ -2744,14 +2746,9 @@ void *commander_low_prio_loop(void *arg)
 
 					} 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;
-						}
-
+						answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED);
+						calib_ret = do_esc_calibration(mavlink_fd, &armed);
+						
 					} else if ((int)(cmd.param4) == 0) {
 						/* RC calibration ended - have we been in one worth confirming? */
 						if (status.rc_input_blocked) {

src/modules/commander/esc_calibration.cpp

@@ -40,6 +40,8 @@
  */
 
 #include "esc_calibration.h"
+#include "calibration_messages.h"
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -63,91 +65,122 @@
 #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;
+int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
+{
+	int	return_code = OK;
+	
+	int	fd = -1;
+	
+	struct	battery_status_s battery;
+	int	batt_sub = -1;
+	bool	batt_updated = false;
+	bool	batt_connected = false;
+	
+	hrt_abstime battery_connect_wait_timeout = 20000000;
+	hrt_abstime pwm_high_timeout = 5000000;
+	hrt_abstime timeout_start;
+	
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "esc");
+	
+	batt_sub = orb_subscribe(ORB_ID(battery_status));
+	if (batt_sub < 0) {
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Subscribe to battery");
+		goto Error;
 	}
-	return OK;
-}
 
+	// Make sure battery is disconnected
+	orb_copy(ORB_ID(battery_status), batt_sub, &battery);
+	if (battery.voltage_filtered_v > 3.0f) {
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
+		goto Error;
+	}
+	
+	armed->in_esc_calibration_mode = true;
+	
+	fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
 
-int do_esc_calibration(int mavlink_fd) {
-
-	int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
-	int ret;
-
-	if(fd < 0) {
-		err(1,"Can't open %s", PWM_OUTPUT0_DEVICE_PATH);
+	if (fd < 0) {
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Can't open PWM device");
+		goto Error;
 	}
 
 	/* tell IO/FMU that its ok to disable its safety with the switch */
-	ret = px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
-	if (ret != OK)
-		err(1, "PWM_SERVO_SET_ARM_OK");
+	if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
+		goto Error;
+	}
+	
 	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
-	ret = px4_ioctl(fd, PWM_SERVO_ARM, 0);
-	if (ret != OK)
-		err(1, "PWM_SERVO_ARM");
+	if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to arm system");
+		goto Error;
+	}
+	
 	/* tell IO to switch off safety without using the safety switch */
-	ret = px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
-	if(ret!=0) {
-		err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF");
+	if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
+		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to force safety off");
+		goto Error;
 	}
 
-	mavlink_log_info(mavlink_fd,"Please connect battery now");
+	mavlink_and_console_log_info(mavlink_fd, "[cal] Connect battery now");
+	
+	timeout_start = hrt_absolute_time();
 
-	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;
+	while (true) {
+		// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
+		// sit high.
+		hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
+        
+		if (hrt_absolute_time() - timeout_start > timeout_wait) {
+			if (!batt_connected) {
+				mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
+				goto Error;
 			}
+			
+			// PWM was high long enough
+			break;
 		}
-		else {
-			start_time = hrt_absolute_time();
+		
+		if (!batt_connected) {
+			orb_check(batt_sub, &batt_updated);
+			if (batt_updated) {
+				orb_copy(ORB_ID(battery_status), batt_sub, &battery);
+				if (battery.voltage_filtered_v > 3.0f) {
+					// Battery is connected, signal to user and start waiting again
+					batt_connected = true;
+					timeout_start = hrt_absolute_time();
+					mavlink_and_console_log_info(mavlink_fd, "[cal] Battery connected");
+				}
+			}
 		}
 		usleep(50000);
 	}
 
-	/* disarm */
-	ret = px4_ioctl(fd, PWM_SERVO_DISARM, 0);
-	if (ret != OK)
-		err(1, "PWM_SERVO_DISARM");
-
-	mavlink_log_info(mavlink_fd,"ESC calibration finished");
-	return OK;
- }
+Out:
+	if (batt_sub != -1) {
+		orb_unsubscribe(batt_sub);
+	}
+	if (fd != -1) {
+		if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) {
+			mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still off");
+		}
+		if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) {
+			mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Servos still armed");
+		}
+		if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) {
+			mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
+		}
+		px4_close(fd);
+	}
+	armed->in_esc_calibration_mode = false;
+	
+	if (return_code == OK) {
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "esc");
+	}
+	
+	return return_code;
+	
+Error:
+	return_code = ERROR;
+	goto Out;
+}

src/modules/commander/esc_calibration.h

@@ -41,7 +41,9 @@
 
 #ifndef ESC_CALIBRATION_H_
 #define ESC_CALIBRATION_H_
-int check_if_batt_disconnected(int mavlink_fd);
-int do_esc_calibration(int mavlink_fd);
+
+#include <uORB/topics/actuator_armed.h>
+
+int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed);
 
 #endif

src/modules/mavlink/mavlink_main.cpp

@@ -69,6 +69,8 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/mcu_version.h>
+#include <systemlib/git_version.h>
 #include <geo/geo.h>
 #include <dataman/dataman.h>
 //#include <mathlib/mathlib.h>
@@ -949,6 +951,47 @@ Mavlink::send_statustext(unsigned char severity, const char *string)
 	mavlink_logbuffer_write(&_logbuffer, &logmsg);
 }
 
+void Mavlink::send_autopilot_capabilites() {
+	struct vehicle_status_s status;
+
+	MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status));
+
+	if (status_sub->update(&status)) {
+		mavlink_autopilot_version_t msg = {};
+
+		msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET;
+		msg.flight_sw_version = 0;
+		msg.middleware_sw_version = 0;
+		msg.os_sw_version = 0;
+		msg.board_version = 0;
+		memcpy(&msg.flight_custom_version, &px4_git_version_binary, sizeof(msg.flight_custom_version));
+		memcpy(&msg.middleware_custom_version, &px4_git_version_binary, sizeof(msg.middleware_custom_version));
+		memset(&msg.os_custom_version, 0, sizeof(msg.os_custom_version));
+		#ifdef CONFIG_CDCACM_VENDORID
+		msg.vendor_id = CONFIG_CDCACM_VENDORID;
+		#else
+		msg.vendor_id = 0;
+		#endif
+		#ifdef CONFIG_CDCACM_PRODUCTID
+		msg.product_id = CONFIG_CDCACM_PRODUCTID;
+		#else
+		msg.product_id = 0;
+		#endif
+		uint32_t uid[3];
+		mcu_unique_id(uid);
+		msg.uid = (((uint64_t)uid[1]) << 32) | uid[2];
+
+		this->send_message(MAVLINK_MSG_ID_AUTOPILOT_VERSION, &msg);
+	}
+}
+
 MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance)
 {
 	/* check if already subscribed to this topic */
@@ -1482,6 +1525,8 @@ Mavlink::task_main(int argc, char *argv[])
 	/* now the instance is fully initialized and we can bump the instance count */
 	LL_APPEND(_mavlink_instances, this);
 
+	send_autopilot_capabilites();
+
 	while (!_task_should_exit) {
 		/* main loop */
 		usleep(_main_loop_delay);

src/modules/mavlink/mavlink_main.h

@@ -241,6 +241,7 @@ public:
 	 * @param severity the log level
 	 */
 	void			send_statustext(unsigned char severity, const char *string);
+	void 			send_autopilot_capabilites();
 
 	MavlinkStream *		get_streams() const { return _streams; }
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -285,7 +285,9 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 
 			/* terminate other threads and this thread */
 			_mavlink->_task_should_exit = true;
-
+		} else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
+			/* send autopilot version message */
+			_mavlink->send_autopilot_capabilites();
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {

src/modules/sdlog2/module.mk

@@ -49,5 +49,3 @@ MAXOPTIMIZATION	 = -Os
 ifeq ($(PX4_TARGET_OS),nuttx)
 EXTRACFLAGS = -Wframe-larger-than=1400
 endif
-
-EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"'

src/modules/sdlog2/sdlog2.c

@@ -105,6 +105,7 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
+#include <systemlib/git_version.h>
 #include <version/version.h>
 
 #include <mavlink/mavlink_log.h>
@@ -798,7 +799,7 @@ int write_version(int fd)
 	};
 
 	/* fill version message and write it */
-	strncpy(log_msg_VER.body.fw_git, GIT_VERSION, sizeof(log_msg_VER.body.fw_git));
+	strncpy(log_msg_VER.body.fw_git, px4_git_version, sizeof(log_msg_VER.body.fw_git));
 	strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch));
 	return write(fd, &log_msg_VER, sizeof(log_msg_VER));
 }

src/modules/sensors/sensors.cpp

@@ -133,7 +133,7 @@
 #endif
 
 #define BATT_V_LOWPASS			0.001f
-#define BATT_V_IGNORE_THRESHOLD		4.8f
+#define BATT_V_IGNORE_THRESHOLD		2.5f
 
 /**
  * HACK - true temperature is much less than indicated temperature in baro,

src/modules/systemlib/git_version.h

@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   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 git_version.h
+ *
+ * GIT repository version
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+__EXPORT extern const char* px4_git_version;
+__EXPORT extern const uint64_t px4_git_version_binary;
+
+__END_DECLS

src/modules/systemlib/module.mk

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012-2014 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
@@ -52,7 +52,8 @@ SRCS		 = \
 		   mcu_version.c \
 		   bson/tinybson.c \
 		   circuit_breaker.cpp \
-		   circuit_breaker_params.c 
+		   circuit_breaker_params.c \
+		   $(BUILD_DIR)git_version.c
 
 ifeq ($(PX4_TARGET_OS),nuttx)
 SRCS		+= err.c \

src/modules/uavcan/module.mk

@@ -63,7 +63,7 @@ SRCS += $(subst  $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC))
 INCLUDE_DIRS += $(LIBUAVCAN_INC)
 # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
 # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
-override EXTRADEFINES := $(EXTRADEFINES) -DGIT_VERSION='"$(GIT_DESC)"' -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
+override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
 
 
 #

src/modules/uavcan/uavcan_main.cpp

@@ -42,6 +42,7 @@
 #include <systemlib/mixer/mixer.h>
 #include <systemlib/board_serial.h>
 #include <systemlib/scheduling_priorities.h>
+#include <systemlib/git_version.h>
 #include <version/version.h>
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
@@ -212,7 +213,7 @@ void UavcanNode::fill_node_info()
 
 	// Extracting the first 8 hex digits of GIT_VERSION and converting them to int
 	char fw_git_short[9] = {};
-	std::memmove(fw_git_short, GIT_VERSION, 8);
+	std::memmove(fw_git_short, px4_git_version, 8);
 	assert(fw_git_short[8] == '\0');
 	char *end = nullptr;
 	swver.vcs_commit = std::strtol(fw_git_short, &end, 16);