added option for esc calibrationESC_calibration

1 files changed, 12 insertions, 3 deletions

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;