VTOL preserve PWM min and max when enabling/disabling actuators

src/modules/vtol_att_control/standard.cpp

@@ -48,7 +48,6 @@
 
 Standard::Standard(VtolAttitudeControl *attc) :
 	VtolType(attc),
-	_flag_enable_mc_motors(true),
 	_pusher_throttle(0.0f),
 	_reverse_output(0.0f),
 	_airspeed_trans_blend_margin(0.0f)
@@ -300,11 +299,9 @@ void Standard::update_transition_state()
 
 		}
 
-		// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
+		// in back transition we need to start the MC motors again
 		if (_flag_enable_mc_motors) {
-			set_max_mc(2000);
+			_flag_enable_mc_motors = !enable_mc_motors();
-			set_idle_mc();
-			_flag_enable_mc_motors = false;
 		}
 	}
 
@@ -322,9 +319,7 @@ void Standard::update_mc_state()
 
 	// enable MC motors here in case we transitioned directly to MC mode
 	if (_flag_enable_mc_motors) {
-		set_max_mc(2000);
+		_flag_enable_mc_motors = !enable_mc_motors();
-		set_idle_mc();
-		_flag_enable_mc_motors = false;
 	}
 
 	// if the thrust scale param is zero or the drone is on manual mode,
@@ -403,11 +398,9 @@ void Standard::update_fw_state()
 {
 	VtolType::update_fw_state();
 
-	// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
+	// stop MC motors in FW mode
 	if (!_flag_enable_mc_motors) {
-		set_max_mc(950);
+		_flag_enable_mc_motors = disable_mc_motors();
-		set_idle_fw();  // force them to stop, not just idle
-		_flag_enable_mc_motors = true;
 	}
 }
 
@@ -495,36 +488,3 @@ Standard::waiting_on_tecs()
 	// keep thrust from transition
 	_v_att_sp->thrust = _pusher_throttle;
 };
-
-/**
-* Disable all multirotor motors when in fw mode.
-*/
-void
-Standard::set_max_mc(unsigned pwm_value)
-{
-	int ret;
-	unsigned servo_count;
-	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
-	int fd = px4_open(dev, 0);
-
-	if (fd < 0) {
-		PX4_WARN("can't open %s", dev);
-	}
-
-	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
-	struct pwm_output_values pwm_values;
-	memset(&pwm_values, 0, sizeof(pwm_values));
-
-	for (int i = 0; i < _params->vtol_motor_count; i++) {
-		pwm_values.values[i] = pwm_value;
-		pwm_values.channel_count = _params->vtol_motor_count;
-	}
-
-	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
-
-	if (ret != OK) {
-		PX4_WARN("failed setting max values");
-	}
-
-	px4_close(fd);
-}

src/modules/vtol_att_control/standard.h

@@ -98,13 +98,13 @@ private:
 		hrt_abstime transition_start;	// at what time did we start a transition (front- or backtransition)
 	} _vtol_schedule;
 
-	bool _flag_enable_mc_motors;
+	// TODO: replace with vtol_type's flag_idle_mc
+	bool _flag_enable_mc_motors{true};
+
 	float _pusher_throttle;
 	float _reverse_output;
 	float _airspeed_trans_blend_margin;
 
-	void set_max_mc(unsigned pwm_value);
-
 	virtual void parameters_update();
 };
 #endif

src/modules/vtol_att_control/tailsitter.cpp

@@ -214,8 +214,7 @@ void Tailsitter::update_transition_state()
 	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
 
 		if (!flag_idle_mc) {
-			set_idle_mc();
+			flag_idle_mc = enable_mc_motors();
-			flag_idle_mc = true;
 		}
 
 		// create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value
@@ -261,8 +260,7 @@ void Tailsitter::update_mc_state()
 
 	// set idle speed for rotary wing mode
 	if (!flag_idle_mc) {
-		set_idle_mc();
+		flag_idle_mc = enable_mc_motors();
-		flag_idle_mc = true;
 	}
 }
 
@@ -271,8 +269,7 @@ void Tailsitter::update_fw_state()
 	VtolType::update_fw_state();
 
 	if (flag_idle_mc) {
-		set_idle_fw();
+		flag_idle_mc = !disable_mc_motors();
-		flag_idle_mc = false;
 	}
 }
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -104,7 +104,7 @@ Tiltrotor::parameters_update()
 	_params_tiltrotor.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
 }
 
-int Tiltrotor::get_motor_off_channels(int channels)
+uint32_t Tiltrotor::get_motor_off_channels(int channels)
 {
 	int channel_bitmap = 0;
 
@@ -251,8 +251,7 @@ void Tiltrotor::update_mc_state()
 
 	// set idle speed for rotary wing mode
 	if (!flag_idle_mc) {
-		set_idle_mc();
+		flag_idle_mc = enable_mc_motors();
-		flag_idle_mc = true;
 	}
 }
 
@@ -270,8 +269,7 @@ void Tiltrotor::update_fw_state()
 
 	// adjust idle for fixed wing flight
 	if (flag_idle_mc) {
-		set_idle_fw();
+		flag_idle_mc = !disable_mc_motors();
-		flag_idle_mc = false;
 	}
 }
 
@@ -347,8 +345,7 @@ void Tiltrotor::update_transition_state()
 		}
 
 		if (!flag_idle_mc) {
-			set_idle_mc();
+			flag_idle_mc = enable_mc_motors();
-			flag_idle_mc = true;
 		}
 
 		// tilt rotors back
@@ -447,8 +444,6 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value)
 		break;
 	}
 
-	int ret;
-	unsigned servo_count;
 	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
 	int fd = px4_open(dev, 0);
 
@@ -456,9 +451,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value)
 		PX4_WARN("can't open %s", dev);
 	}
 
-	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	struct pwm_output_values pwm_max_values = {};
-	struct pwm_output_values pwm_max_values;
-	memset(&pwm_max_values, 0, sizeof(pwm_max_values));
 
 	for (int i = 0; i < _params->vtol_motor_count; i++) {
 		if (is_motor_off_channel(i)) {
@@ -471,10 +464,10 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value)
 		pwm_max_values.channel_count = _params->vtol_motor_count;
 	}
 
-	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values);
+	int ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values);
 
 	if (ret != OK) {
-		PX4_WARN("failed setting max values");
+		PX4_ERR("failed setting max values");
 	}
 
 	px4_close(fd);

src/modules/vtol_att_control/tiltrotor.h

@@ -111,7 +111,7 @@ private:
 	/**
 	 * Return a bitmap of channels that should be turned off in fixed wing mode.
 	 */
-	int get_motor_off_channels(const int channels);
+	uint32_t get_motor_off_channels(const int channels);
 
 	/**
 	 * Return true if the motor channel is off in fixed wing mode.

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -595,9 +595,6 @@ void VtolAttitudeControl::task_main()
 
 	parameters_update();  // initialize parameter cache
 
-	// make sure we start with idle in mc mode
-	_vtol_type->set_idle_mc();
-
 	/* wakeup source*/
 	px4_pollfd_struct_t fds[1] = {};
 	fds[0].fd     = _actuator_inputs_mc;

src/modules/vtol_att_control/vtol_type.cpp

@@ -67,7 +67,9 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_land_detected = _attc->get_land_detected();
 	_params = _attc->get_params();
 
-	flag_idle_mc = true;
+	for (auto &pwm_max : _max_mc_pwm_values.values) {
+		pwm_max = PWM_DEFAULT_MAX;
+	}
 }
 
 VtolType::~VtolType()
@@ -75,71 +77,6 @@ VtolType::~VtolType()
 
 }
 
-/**
-* Adjust idle speed for mc mode.
-*/
-void VtolType::set_idle_mc()
-{
-	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
-	int fd = px4_open(dev, 0);
-
-	if (fd < 0) {
-		PX4_WARN("can't open %s", dev);
-	}
-
-	unsigned servo_count;
-	int ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
-	unsigned pwm_value = _params->idle_pwm_mc;
-	struct pwm_output_values pwm_values;
-	memset(&pwm_values, 0, sizeof(pwm_values));
-
-	for (int i = 0; i < _params->vtol_motor_count; i++) {
-		pwm_values.values[i] = pwm_value;
-		pwm_values.channel_count++;
-	}
-
-	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
-
-	if (ret != OK) {
-		PX4_WARN("failed setting min values");
-	}
-
-	px4_close(fd);
-
-	flag_idle_mc = true;
-}
-
-/**
-* Adjust idle speed for fw mode.
-*/
-void VtolType::set_idle_fw()
-{
-	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
-	int fd = px4_open(dev, 0);
-
-	if (fd < 0) {
-		PX4_WARN("can't open %s", dev);
-	}
-
-	struct pwm_output_values pwm_values;
-
-	memset(&pwm_values, 0, sizeof(pwm_values));
-
-	for (int i = 0; i < _params->vtol_motor_count; i++) {
-
-		pwm_values.values[i] = PWM_MOTOR_OFF;
-		pwm_values.channel_count++;
-	}
-
-	int ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
-
-	if (ret != OK) {
-		PX4_WARN("failed setting min values");
-	}
-
-	px4_close(fd);
-}
-
 void VtolType::update_mc_state()
 {
 	// copy virtual attitude setpoint to real attitude setpoint
@@ -263,5 +200,87 @@ void VtolType::check_quadchute_condition()
 			}
 		}
 	}
-
+}
+
+bool
+VtolType::disable_mc_motors()
+{
+	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
+	int fd = px4_open(dev, 0);
+
+	if (fd < 0) {
+		PX4_ERR("can't open %s", dev);
+		return false;
+	}
+
+	// first save the current max values
+	struct pwm_output_values max_pwm_values = {};
+
+	int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&max_pwm_values);
+
+	if (ret == OK) {
+		_max_mc_pwm_values = max_pwm_values;
+
+	} else {
+		PX4_ERR("failed getting max values");
+		px4_close(fd);
+		return false;
+	}
+
+	// now get the disarmed PWM values
+	pwm_output_values disarmed_pwm_values = {};
+	ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&disarmed_pwm_values);
+
+	if (ret == OK) {
+
+		// finally disable by setting the MC motors max to the disarmed value
+		max_pwm_values.channel_count = _params->vtol_motor_count;
+
+		for (int i = 0; i < _params->vtol_motor_count; i++) {
+			max_pwm_values.values[i] = disarmed_pwm_values.values[i];
+		}
+
+		ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&max_pwm_values);
+
+		if (ret != OK) {
+			PX4_ERR("failed setting max values");
+		}
+
+	} else {
+		PX4_ERR("failed getting max values");
+	}
+
+	px4_close(fd);
+
+	return (ret == PX4_OK);
+}
+
+bool
+VtolType::enable_mc_motors()
+{
+	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
+	int fd = px4_open(dev, 0);
+
+	if (fd < 0) {
+		PX4_ERR("can't open %s", dev);
+		return false;
+	}
+
+	struct pwm_output_values pwm_values = {};
+
+	pwm_values.channel_count = _params->vtol_motor_count;
+
+	for (int i = 0; i < _params->vtol_motor_count; i++) {
+		pwm_values.values[i] = _max_mc_pwm_values.values[i];
+	}
+
+	int ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {
+		PX4_ERR("failed setting max values");
+	}
+
+	px4_close(fd);
+
+	return (ret == PX4_OK);
 }

src/modules/vtol_att_control/vtol_type.h

@@ -45,6 +45,7 @@
 
 #include <lib/mathlib/mathlib.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
 
 struct Params {
 	int32_t idle_pwm_mc;			// pwm value for idle in mc mode
@@ -182,10 +183,17 @@ protected:
 	float _ra_hrate_sp = 0.0f;		// rolling average on height rate setpoint for quadchute condition
 
 	bool _flag_was_in_trans_mode = false;	// true if mode has just switched to transition
+
 	hrt_abstime _trans_finished_ts = 0;
+
 	bool _tecs_running = false;
 	hrt_abstime _tecs_running_ts = 0;
 
+	struct pwm_output_values _max_mc_pwm_values {};
+
+	bool enable_mc_motors();
+	bool disable_mc_motors();
+
 };
 
 #endif