Merge pull request #2101 from PX4/ESC_calibration

Esc calibration

6 files changed, 226 insertions, 3 deletions

diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg
index b83adb8f2..c8098724e 100644
--- a/msg/actuator_armed.msg
+++ b/msg/actuator_armed.msg
@@ -4,3 +4,4 @@ bool armed		# Set to true if system is armed
 bool ready_to_arm	# Set to true if system is ready to be armed
 bool lockdown		# Set to true if actuators are forced to being disabled (due to emergency or HIL)
 bool force_failsafe	# Set to true if the actuators are forced to the failsafe position
+bool in_esc_calibration_mode # IO/FMU should ignore messages from the actuator controls topics
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index e5636ff0f..5a3104fa5 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -302,6 +302,7 @@ private:
 	float			_battery_mamphour_total;///< amp hours consumed so far
 	uint64_t		_battery_last_timestamp;///< last amp hour calculation timestamp
 	bool			_cb_flighttermination;	///< true if the flight termination circuit breaker is enabled
+	bool 			_in_esc_calibration_mode;	///< do not send control outputs to IO (used for esc calibration)
 
 	int32_t			_rssi_pwm_chan; ///< RSSI PWM input channel
 	int32_t			_rssi_pwm_max; ///< max RSSI input on PWM channel
@@ -529,6 +530,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_mamphour_total(0),
 	_battery_last_timestamp(0),
 	_cb_flighttermination(true),
+	_in_esc_calibration_mode(false),
 	_rssi_pwm_chan(0),
 	_rssi_pwm_max(0),
 	_rssi_pwm_min(0)
@@ -1167,9 +1169,14 @@ PX4IO::io_set_control_state(unsigned group)
 		break;
 	}
 
-	if (!changed) {
+	if (!changed && (!_in_esc_calibration_mode || group != 0)) {
 		return -1;
 	}
+	else if (_in_esc_calibration_mode && group == 0) {
+		// modify controls to get max pwm (full thrust) on every esc
+		memset(&controls, 0, sizeof(controls));
+		controls.control[3] = 1.0f;		// set maximum thrust
+	}
 
 	for (unsigned i = 0; i < _max_controls; i++) {
 		regs[i] = FLOAT_TO_REG(controls.control[i]);
@@ -1188,12 +1195,14 @@ PX4IO::io_set_arming_state()
 
 	int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
 	int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
+	_in_esc_calibration_mode = armed.in_esc_calibration_mode;
 
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-        if (have_armed == OK) {
-		if (armed.armed) {
+    if (have_armed == OK) {
+		_in_esc_calibration_mode = armed.in_esc_calibration_mode;
+		if (armed.armed || _in_esc_calibration_mode) {
 			set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 		} else {
 			clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 7aaf5e5cd..5caf36e19 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -114,6 +114,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++ */
@@ -2708,6 +2709,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) {
@@ -2717,6 +2728,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) {
diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp
new file mode 100644
index 000000000..541aca053
--- /dev/null
+++ b/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;
+ }
\ No newline at end of file
diff --git a/src/modules/commander/esc_calibration.h b/src/modules/commander/esc_calibration.h
new file mode 100644
index 000000000..5539955eb
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index 4437041e2..5331428c2 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -47,6 +47,7 @@ SRCS		 	= commander.cpp \
 			baro_calibration.cpp \
 			rc_calibration.cpp \
 			airspeed_calibration.cpp \
+			esc_calibration.cpp \
 			PreflightCheck.cpp
 
 MODULE_STACKSIZE = 5000