Merge remote-tracking branch 'remotes/px4/master' into px4fix/masterMultiMavlinkFix

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -47,7 +47,6 @@
 #include <poll.h>
 #include <fcntl.h>
 #include <float.h>
-#include <termios.h>
 #include <errno.h>
 #include <limits.h>
 #include <math.h>

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -695,7 +695,6 @@ FixedwingAttitudeControl::task_main()
 		/* only run controller if attitude changed */
 		if (fds[1].revents & POLLIN) {
 
-
 			static uint64_t last_run = 0;
 			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 			last_run = hrt_absolute_time();
@@ -800,7 +799,7 @@ FixedwingAttitudeControl::task_main()
 			}
 
 			/* if we are in rotary wing mode, do nothing */
-			if (_vehicle_status.is_rotary_wing) {
+			if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) {
 				continue;
 			}
 
@@ -813,11 +812,8 @@ FixedwingAttitudeControl::task_main()
 			}
 
 			/* decide if in stabilized or full manual control */
-
 			if (_vcontrol_mode.flag_control_attitude_enabled) {
-
 				/* scale around tuning airspeed */
-
 				float airspeed;
 
 				/* if airspeed is not updating, we assume the normal average speed */

src/modules/mavlink/mavlink_parameters.cpp

@@ -215,6 +215,10 @@ MavlinkParametersManager::send(const hrt_abstime t)
 		if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
 			_send_all_index = -1;
 		}
+	} else if (_send_all_index == 0 && hrt_absolute_time() > 20 * 1000 * 1000) {
+		/* the boot did not seem to ever complete, warn user and set boot complete */
+		_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");
+		_mavlink->set_boot_complete();
 	}
 }
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -137,7 +137,6 @@ private:
 	int		_vehicle_status_sub;	/**< vehicle status subscription */
 	int 	_motor_limits_sub;		/**< motor limits subscription */
 
-	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
 	orb_advert_t	_controller_status_pub;	/**< controller status publication */
@@ -170,8 +169,6 @@ private:
 
 	math::Matrix<3, 3>  _I;				/**< identity matrix */
 
-	bool	_reset_yaw_sp;			/**< reset yaw setpoint flag */
-
 	struct {
 		param_t roll_p;
 		param_t roll_rate_p;
@@ -315,7 +312,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_vehicle_status_sub(-1),
 
 /* publications */
-	_att_sp_pub(nullptr),
 	_v_rates_sp_pub(nullptr),
 	_actuators_0_pub(nullptr),
 	_controller_status_pub(nullptr),

src/modules/muorb/adsp/uORBFastRpcChannel.cpp

@@ -457,7 +457,7 @@ int16_t uORB::FastRpcChannel::get_bulk_data
 	}
 
 	if (topic_count_to_return != *topic_count) {
-		PX4_WARN("Not sending all topics: topics_to_return:[%d] topics_returning:[%d]", topic_count_to_return, *topic_count);
+		PX4_WARN("Not sending all topics: topics_to_return:[%ld] topics_returning:[%ld]", topic_count_to_return, *topic_count);
 	}
 
 	_QueueMutex.unlock();
@@ -539,7 +539,7 @@ int32_t uORB::FastRpcChannel::copy_data_to_buffer(int32_t src_index, uint8_t *ds
 
 	} else {
 		PX4_WARN("Error coping the DataMsg to dst buffer, insuffienct space. ");
-		PX4_WARN("... offset[%d] len[%d] data_msg_len[%d]",
+		PX4_WARN("... offset[%ld] len[%ld] data_msg_len[%ld]",
 			 offset, dst_buffer_len, (field_data_offset - offset) + _DataMsgQueue[ src_index ]._Length);
 	}
 

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -46,8 +46,6 @@
 #include <fcntl.h>
 #include <string.h>
 #include <px4_config.h>
-#include <sys/prctl.h>
-#include <termios.h>
 #include <math.h>
 #include <float.h>
 #include <uORB/uORB.h>
@@ -187,11 +185,11 @@ int position_estimator_inav_main(int argc, char *argv[])
 	return 1;
 }
 
+#ifdef INAV_DEBUG
 static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2],
 	float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p,
 	float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v)
 {
-	return;
 	FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a");
 
 	if (f) {
@@ -216,6 +214,9 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
 	fsync(fileno(f));
 	fclose(f);
 }
+#else
+#define write_debug_log(...) 
+#endif
 
 /****************************************************************************
  * main

src/modules/simulator/simulator.h

@@ -210,8 +210,9 @@ private:
 	_baro_pub(nullptr),
 	_gyro_pub(nullptr),
 	_mag_pub(nullptr),
-	_initialized(false),
+	_initialized(false)
 #ifndef __PX4_QURT
+	,
 	_rc_channels_pub(nullptr),
 	_actuator_outputs_sub(-1),
 	_vehicle_attitude_sub(-1),

src/modules/vtol_att_control/module.mk

@@ -42,6 +42,8 @@ SRCS		= vtol_att_control_main.cpp \
 			  tiltrotor_params.c \
 			  tiltrotor.cpp \
 			  vtol_type.cpp \
-			  tailsitter.cpp
+			  tailsitter.cpp \
+			  standard_params.c \
+			  standard.cpp
 
 EXTRACXXFLAGS = -Wno-write-strings

src/modules/vtol_att_control/standard.cpp

@@ -0,0 +1,296 @@
+/****************************************************************************
+ *
+ *   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 standard.cpp
+ *
+ * @author Simon Wilks		<simon@uaventure.com>
+ * @author Roman Bapst 		<bapstroman@gmail.com>
+ *
+*/
+
+#include "standard.h"
+#include "vtol_att_control_main.h"
+
+Standard::Standard(VtolAttitudeControl *attc) :
+	VtolType(attc),
+	_flag_enable_mc_motors(true),
+	_pusher_throttle(0.0f),
+	_mc_att_ctl_weight(1.0f),
+	_airspeed_trans_blend_margin(0.0f)
+{
+	_vtol_schedule.flight_mode = MC_MODE;
+	_vtol_schedule.transition_start = 0;
+
+	_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
+	_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
+	_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
+	_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
+	_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
+ }
+
+Standard::~Standard()
+{
+}
+
+int
+Standard::parameters_update()
+{
+	float v;
+
+	/* duration of a forwards transition to fw mode */
+	param_get(_params_handles_standard.front_trans_dur, &v);
+	_params_standard.front_trans_dur = math::constrain(v, 0.0f, 5.0f);
+
+	/* duration of a back transition to mc mode */
+	param_get(_params_handles_standard.back_trans_dur, &v);
+	_params_standard.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
+
+	/* target throttle value for pusher motor during the transition to fw mode */
+	param_get(_params_handles_standard.pusher_trans, &v);
+	_params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f);
+
+	/* airspeed at which it we should switch to fw mode */
+	param_get(_params_handles_standard.airspeed_trans, &v);
+	_params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f);
+
+	/* airspeed at which we start blending mc/fw controls */
+	param_get(_params_handles_standard.airspeed_blend, &v);
+	_params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f);
+
+	_airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend;
+
+	return OK;
+}
+
+void Standard::update_vtol_state()
+{
+ 	parameters_update();
+
+	/* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up
+	 * forward speed. After the vehicle has picked up enough speed the rotors shutdown. 
+	 * For the back transition the pusher motor is immediately stopped and rotors reactivated.
+ 	 */
+
+	if (_manual_control_sp->aux1 < 0.0f) {
+		// the transition to fw mode switch is off
+		if (_vtol_schedule.flight_mode == MC_MODE) {
+			// in mc mode
+			_vtol_schedule.flight_mode = MC_MODE;
+			_mc_att_ctl_weight = 1.0f;
+
+		} else if (_vtol_schedule.flight_mode == FW_MODE) {
+			// transition to mc mode
+			_vtol_schedule.flight_mode = TRANSITION_TO_MC;
+			_flag_enable_mc_motors = true;
+			_vtol_schedule.transition_start = hrt_absolute_time();
+
+		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
+			// failsafe back to mc mode
+			_vtol_schedule.flight_mode = MC_MODE;
+			_mc_att_ctl_weight = 1.0f;
+
+		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
+			// keep transitioning to mc mode
+			_vtol_schedule.flight_mode = MC_MODE;
+		}
+
+		// the pusher motor should never be powered when in or transitioning to mc mode
+		_pusher_throttle = 0.0f;
+
+	} else {
+		// the transition to fw mode switch is on
+		if (_vtol_schedule.flight_mode == MC_MODE) {
+			// start transition to fw mode
+			_vtol_schedule.flight_mode = TRANSITION_TO_FW;
+			_vtol_schedule.transition_start = hrt_absolute_time();
+
+		} else if (_vtol_schedule.flight_mode == FW_MODE) {
+			// in fw mode
+			_vtol_schedule.flight_mode = FW_MODE;
+			_mc_att_ctl_weight = 0.0f;
+
+		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
+			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
+			if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) {
+				_vtol_schedule.flight_mode = FW_MODE;
+				// we can turn off the multirotor motors now
+				_flag_enable_mc_motors = false;
+				// don't set pusher throttle here as it's being ramped up elsewhere
+			}
+
+		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
+			// transitioning to mc mode & transition switch on - failsafe back into fw mode
+			_vtol_schedule.flight_mode = FW_MODE;
+		}
+	}
+
+	// map specific control phases to simple control modes
+	switch(_vtol_schedule.flight_mode) {
+		case MC_MODE:
+			_vtol_mode = ROTARY_WING;
+			break;
+		case FW_MODE:
+			_vtol_mode = FIXED_WING;
+			break;
+		case TRANSITION_TO_FW:
+		case TRANSITION_TO_MC:
+			_vtol_mode = TRANSITION;
+			break;
+	}
+}
+
+void Standard::update_transition_state()
+{
+	if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
+		if (_params_standard.front_trans_dur <= 0.0f) {
+			// just set the final target throttle value
+			_pusher_throttle = _params_standard.pusher_trans;
+		} else if (_pusher_throttle <= _params_standard.pusher_trans) {
+			// ramp up throttle to the target throttle value
+			_pusher_throttle = _params_standard.pusher_trans * 
+						(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
+		}
+
+		// do blending of mc and fw controls if a blending airspeed has been provided
+		if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) {
+			_mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
+		} else {
+			// at low speeds give full weight to mc
+			_mc_att_ctl_weight = 1.0f;
+		}
+
+	} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
+		// continually increase mc attitude control as we transition back to mc mode
+		if (_params_standard.back_trans_dur > 0.0f) {
+			_mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
+		} else {
+			_mc_att_ctl_weight = 1.0f;	
+		}
+
+		// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again	
+		if (_flag_enable_mc_motors) {
+			set_max_mc(2000);
+			set_idle_mc();
+			_flag_enable_mc_motors = false;
+		}
+	}
+
+	_mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f);
+}
+
+void Standard::update_mc_state()
+{
+	// do nothing
+}
+
+void Standard::process_mc_data()
+{
+	fill_att_control_output();
+}
+
+void Standard::process_fw_data()
+{
+	fill_att_control_output();
+}
+
+ void Standard::update_fw_state()
+{
+	// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again	
+	if (!_flag_enable_mc_motors) {
+		set_max_mc(950);
+		set_idle_fw();  // force them to stop, not just idle
+		_flag_enable_mc_motors = true;
+	}
+ }
+
+void Standard::update_external_state()
+{
+}
+
+/**
+ * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine
+ * what proportion of control should be applied to each of the control groups (mc and fw).
+ */
+void Standard::fill_att_control_output()
+{
+	/* multirotor controls */
+	_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight;	// roll
+	_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight;	// pitch
+	_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight;	// yaw
+	_actuators_out_0->control[3] = _actuators_mc_in->control[3];	// throttle
+
+	/* fixed wing controls */
+	const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight;
+	_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight;	//roll
+	_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight;	//pitch
+	_actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight;	// yaw
+
+	// set the fixed wing throttle control
+	if (_vtol_schedule.flight_mode == FW_MODE) {
+		// take the throttle value commanded by the fw controller
+		_actuators_out_1->control[3] = _actuators_fw_in->control[3];
+	} else {
+		// otherwise we may be ramping up the throttle during the transition to fw mode 
+		_actuators_out_1->control[3] = _pusher_throttle;
+	}
+}
+
+/**
+* Disable all multirotor motors when in fw mode.
+*/
+void
+Standard::set_max_mc(unsigned pwm_value)
+{
+	int ret;
+	unsigned servo_count;
+	char *dev = PWM_OUTPUT0_DEVICE_PATH;
+	int fd = open(dev, 0);
+
+	if (fd < 0) {err(1, "can't open %s", dev);}
+
+	ret = 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 = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {errx(ret, "failed setting max values");}
+
+	close(fd);
+}

src/modules/vtol_att_control/standard.h

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   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 standard.h
+ * VTOL with fixed multirotor motor configurations (such as quad) and a pusher
+ * (or puller aka tractor) motor for forward flight.
+ *
+ * @author Simon Wilks 		<simon@uaventure.com>
+ * @author Roman Bapst 		<bapstroman@gmail.com>
+ *
+ */
+
+#ifndef STANDARD_H
+#define STANDARD_H
+#include "vtol_type.h"
+#include <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
+
+class Standard : public VtolType
+{
+
+public:
+
+	Standard(VtolAttitudeControl * _att_controller);
+	~Standard();
+
+	void update_vtol_state();
+	void update_mc_state();
+	void process_mc_data();
+	void update_fw_state();
+	void process_fw_data();
+	void update_transition_state();
+	void update_external_state();
+
+private:
+
+	struct {
+		float front_trans_dur;
+		float back_trans_dur;
+		float pusher_trans;
+		float airspeed_blend;
+		float airspeed_trans;
+	} _params_standard;
+
+	struct {
+		param_t front_trans_dur;
+		param_t back_trans_dur;
+		param_t pusher_trans;
+		param_t airspeed_blend;
+		param_t airspeed_trans;
+	} _params_handles_standard;
+
+	enum vtol_mode {
+		MC_MODE = 0,
+		TRANSITION_TO_FW,
+		TRANSITION_TO_MC,
+		FW_MODE
+	};
+
+	struct {
+		vtol_mode flight_mode;			// indicates in which mode the vehicle is in
+		hrt_abstime transition_start;	// at what time did we start a transition (front- or backtransition)
+	}_vtol_schedule;
+
+	bool _flag_enable_mc_motors;
+	float _pusher_throttle;
+	float _mc_att_ctl_weight;	// the amount of multicopter attitude control that should be applied in fixed wing mode while transitioning 
+	float _airspeed_trans_blend_margin;
+
+	void fill_att_control_output();
+	void set_max_mc(unsigned pwm_value);
+
+	int parameters_update();
+
+};
+#endif

src/modules/vtol_att_control/standard_params.c

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   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 standard_params.c
+ * Parameters for the standard VTOL controller.
+ *
+ * @author Simon Wilks	<simon@uaventure.com>
+ * @author Roman Bapst	<bapstroman@gmail.com>
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * Target throttle value for pusher/puller motor during the transition to fw mode
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f);

src/modules/vtol_att_control/tiltrotor.cpp

@@ -194,7 +194,7 @@ void Tiltrotor::update_mc_state()
 
 void Tiltrotor::process_mc_data()
 {
-	fill_mc_att_control_output();
+	fill_att_control_output();
 }
 
  void Tiltrotor::update_fw_state()
@@ -222,19 +222,22 @@ void Tiltrotor::process_mc_data()
 
 void Tiltrotor::process_fw_data()
 {
-	fill_fw_att_control_output();
+	fill_att_control_output();
 }
 
 void Tiltrotor::update_transition_state()
 {
 	if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
 		// tilt rotors forward up to certain angle
-		if (_tilt_control <= _params_tiltrotor.tilt_transition) {
-			_tilt_control = _params_tiltrotor.tilt_mc +  fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f);
+		if (_params_tiltrotor.front_trans_dur <= 0.0f) {
+			_tilt_control = _params_tiltrotor.tilt_transition;
+		} else if (_tilt_control <= _params_tiltrotor.tilt_transition) {
+			_tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * 
+						(float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f);
 		}
 
 		// do blending of mc and fw controls
-		if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) {
+		if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START && _params_tiltrotor.airspeed_trans - ARSP_BLEND_START > 0.0f) {
 			_roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START);
 		} else {
 			// at low speeds give full weight to mc
@@ -244,13 +247,14 @@ void Tiltrotor::update_transition_state()
 		_roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f);
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
-		_tilt_control = _params_tiltrotor.tilt_transition +  fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f);
+		_tilt_control = _params_tiltrotor.tilt_transition +  fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) *
+					(float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (0.5f * 1000000.0f);
 		_roll_weight_mc = 0.0f;
 	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
 		// tilt rotors forward up to certain angle
-		float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f);
+		float progress = (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);
 		if (_tilt_control > _params_tiltrotor.tilt_mc) {
-			_tilt_control = _params_tiltrotor.tilt_fw -  fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress;
+			_tilt_control = _params_tiltrotor.tilt_fw -  fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * progress;
 		}
 
 		_roll_weight_mc = progress;
@@ -263,38 +267,26 @@ void Tiltrotor::update_external_state()
 }
 
  /**
-* Prepare message to acutators with data from mc attitude controller.
+* Prepare message to acutators with data from the attitude controllers.
 */
-void Tiltrotor::fill_mc_att_control_output()
+void Tiltrotor::fill_att_control_output()
 {
-	_actuators_out_0->control[0] = _actuators_mc_in->control[0];
-	_actuators_out_0->control[1] = _actuators_mc_in->control[1];
-	_actuators_out_0->control[2] = _actuators_mc_in->control[2];
-	_actuators_out_0->control[3] = _actuators_mc_in->control[3];
+	_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc;	// roll
+	_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc;	// pitch
+	_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc;	// yaw
+
+	if (_vtol_schedule.flight_mode == FW_MODE) {
+		_actuators_out_1->control[3] = _actuators_fw_in->control[3];	// fw throttle
+	} else {
+		_actuators_out_0->control[3] = _actuators_mc_in->control[3];	// mc throttle
+	}
 
 	_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc);	//roll elevon
-	_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc);	//pitch elevon
-
+	_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc);	//pitch elevon	
 	_actuators_out_1->control[4] = _tilt_control;	// for tilt-rotor control
-}
 
-/**
-* Prepare message to acutators with data from fw attitude controller.
-*/
-void Tiltrotor::fill_fw_att_control_output()
-{
-	/*For the first test in fw mode, only use engines for thrust!!!*/
-	_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc;
-	_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc;
-	_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc;
-	_actuators_out_0->control[3] = _actuators_fw_in->control[3];
-	/*controls for the elevons */
-	_actuators_out_1->control[0] = -_actuators_fw_in->control[0];	// roll elevon
-	_actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim;	// pitch elevon
 	// unused now but still logged
-	_actuators_out_1->control[2] = _actuators_fw_in->control[2];	// yaw
-	_actuators_out_1->control[3] = _actuators_fw_in->control[3];	// throttle
-	_actuators_out_1->control[4] = _tilt_control;
+	_actuators_out_1->control[2] = _actuators_fw_in->control[2];	// fw yaw
 }
 
 /**

src/modules/vtol_att_control/tiltrotor.h

@@ -99,8 +99,7 @@ private:
 	float _tilt_control;
 	float _roll_weight_mc;
 
-	void fill_mc_att_control_output();
-	void fill_fw_att_control_output();
+	void fill_att_control_output();
 	void set_max_mc();
 	void set_max_fw(unsigned pwm_value);
 

src/modules/vtol_att_control/tiltrotor_params.c

@@ -39,28 +39,6 @@
  */
 
 #include <systemlib/param/param.h>
- 
-/**
- * Duration of a front transition
- *
- * Time in seconds used for a transition
- *
- * @min 0.0
- * @max 5
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f);
-
-/**
- * Duration of a back transition
- *
- * Time in seconds used for a back transition
- *
- * @min 0.0
- * @max 5
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f);
 
 /**
  * Position of tilt servo in mc mode
@@ -71,7 +49,7 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f);
  * @max 1
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f);
+PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f);
 
 /**
  * Position of tilt servo in transition mode
@@ -82,7 +60,7 @@ PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f);
  * @max 1
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f);
+PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f);
 
 /**
  * Position of tilt servo in fw mode
@@ -93,15 +71,4 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f);
  * @max 1
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f);
-
-/**
- * Transition airspeed
- *
- * Airspeed at which we can switch to fw mode
- *
- * @min 0.0
- * @max 20
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f);
+PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f);

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -123,6 +123,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	} else if (_params.vtol_type == 1) {
 		_tiltrotor = new Tiltrotor(this);
 		_vtol_type = _tiltrotor;
+	} else if (_params.vtol_type == 2) {
+		_standard = new Standard(this);
+		_vtol_type = _standard;
 	} else {
 		_task_should_exit = true;
 	}
@@ -531,6 +534,9 @@ void VtolAttitudeControl::task_main()
 			// update transition state if got any new data
 			if (got_new_data) {
 				_vtol_type->update_transition_state();
+
+				_vtol_type->process_mc_data();
+				fill_mc_att_rates_sp();
 			}
 
 		} else if (_vtol_type->get_mode() == EXTERNAL) {

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -84,6 +84,7 @@
 
 #include "tiltrotor.h"
 #include "tailsitter.h"
+#include "standard.h"
 
 
 extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
@@ -188,6 +189,7 @@ private:
 	VtolType * _vtol_type;	// base class for different vtol types
 	Tiltrotor * _tiltrotor;	// tailsitter vtol type
 	Tailsitter * _tailsitter;	// tiltrotor vtol type
+	Standard * _standard;	// standard vtol type
 
 //*****************Member functions***********************************************************************
 

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -143,10 +143,10 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f);
 PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f);
 
 /**
- * VTOL Type (Tailsitter=0, Tiltrotor=1)
+ * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2)
  *
  * @min 0
- * @max 1
+ * @max 2
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_TYPE, 0);
@@ -161,3 +161,47 @@ PARAM_DEFINE_INT32(VT_TYPE, 0);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0);
+
+/**
+ * Duration of a front transition
+ *
+ * Time in seconds used for a transition
+ *
+ * @min 0.0
+ * @max 5
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f);
+
+/**
+ * Duration of a back transition
+ *
+ * Time in seconds used for a back transition
+ *
+ * @min 0.0
+ * @max 5
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 2.0f);
+
+/**
+ * Transition blending airspeed
+ *
+ * Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable.
+ *
+ * @min 0.0
+ * @max 20.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f);
+
+/**
+ * Transition airspeed
+ *
+ * Airspeed at which we can switch to fw mode
+ *
+ * @min 1.0
+ * @max 20
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);

src/modules/vtol_att_control/vtol_type.h

@@ -38,8 +38,8 @@
  *
  */
 
-#ifndef VTOL_YYPE_H
-#define VTOL_YYPE_H
+#ifndef VTOL_TYPE_H
+#define VTOL_TYPE_H
 
 struct Params {
 	int idle_pwm_mc;			// pwm value for idle in mc mode