Merge branch 'master' of https://github.com/PX4/Firmware

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -54,8 +54,11 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vision_position_estimate.h>
+#include <uORB/topics/att_pos_mocap.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -117,22 +120,28 @@ private:
 
 	int		_sensors_sub = -1;
 	int		_params_sub = -1;
+	int		_vision_sub = -1;
+	int		_mocap_sub = -1;
 	int		_global_pos_sub = -1;
 	orb_advert_t	_att_pub = nullptr;
+	orb_advert_t	_ctrl_state_pub = nullptr;
 
 	struct {
 		param_t	w_acc;
 		param_t	w_mag;
+		param_t	w_ext_hdg;
 		param_t	w_gyro_bias;
 		param_t	mag_decl;
 		param_t	mag_decl_auto;
 		param_t	acc_comp;
 		param_t	bias_max;
 		param_t vibe_thresh;
+		param_t	ext_hdg_mode;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	float		_w_accel = 0.0f;
 	float		_w_mag = 0.0f;
+	float		_w_ext_hdg = 0.0f;
 	float		_w_gyro_bias = 0.0f;
 	float		_mag_decl = 0.0f;
 	bool		_mag_decl_auto = false;
@@ -140,11 +149,18 @@ private:
 	float		_bias_max = 0.0f;
 	float		_vibration_warning_threshold = 1.0f;
 	hrt_abstime	_vibration_warning_timestamp = 0;
+	int		_ext_hdg_mode = 0;
 
 	Vector<3>	_gyro;
 	Vector<3>	_accel;
 	Vector<3>	_mag;
 
+	vision_position_estimate_s _vision = {};
+	Vector<3>	_vision_hdg;
+
+	att_pos_mocap_s _mocap = {};
+	Vector<3>	_mocap_hdg;
+
 	Quaternion	_q;
 	Vector<3>	_rates;
 	Vector<3>	_gyro_bias;
@@ -168,6 +184,7 @@ private:
 	bool		_data_good = false;
 	bool		_failsafe = false;
 	bool		_vibration_warning = false;
+	bool		_ext_hdg_good = false;
 
 	int		_mavlink_fd = -1;
 
@@ -190,18 +207,20 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_voter_mag(3),
 	_lp_roll_rate(250.0f, 18.0f),
 	_lp_pitch_rate(250.0f, 18.0f),
-	_lp_yaw_rate(250, 10.0f)
+	_lp_yaw_rate(250.0f, 10.0f)
 {
 	_voter_mag.set_timeout(200000);
 
 	_params_handles.w_acc		= param_find("ATT_W_ACC");
 	_params_handles.w_mag		= param_find("ATT_W_MAG");
+	_params_handles.w_ext_hdg	= param_find("ATT_W_EXT_HDG");
 	_params_handles.w_gyro_bias	= param_find("ATT_W_GYRO_BIAS");
 	_params_handles.mag_decl	= param_find("ATT_MAG_DECL");
 	_params_handles.mag_decl_auto	= param_find("ATT_MAG_DECL_A");
 	_params_handles.acc_comp	= param_find("ATT_ACC_COMP");
 	_params_handles.bias_max	= param_find("ATT_BIAS_MAX");
 	_params_handles.vibe_thresh	= param_find("ATT_VIBE_THRESH");
+	_params_handles.ext_hdg_mode	= param_find("ATT_EXT_HDG_M");
 }
 
 /**
@@ -269,6 +288,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
 void AttitudeEstimatorQ::task_main()
 {
 	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
+
+	_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
+	_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
+
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 
@@ -374,6 +397,47 @@ void AttitudeEstimatorQ::task_main()
 			}
 		}
 
+		// Update vision and motion capture heading
+		bool vision_updated = false;
+		orb_check(_vision_sub, &vision_updated);
+
+		bool mocap_updated = false;
+		orb_check(_mocap_sub, &mocap_updated);
+
+		if (vision_updated) {
+			orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
+			math::Quaternion q(_vision.q);
+
+			math::Matrix<3, 3> Rvis = q.to_dcm();
+			math::Vector<3> v(1.0f, 0.0f, 0.4f);
+
+			// Rvis is Rwr (robot respect to world) while v is respect to world.
+			// Hence Rvis must be transposed having (Rwr)' * Vw
+			// Rrw * Vw = vn. This way we have consistency
+			_vision_hdg = Rvis.transposed() * v;
+		}
+
+		if (mocap_updated) {
+			orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
+			math::Quaternion q(_mocap.q);
+			math::Matrix<3, 3> Rmoc = q.to_dcm();
+
+			math::Vector<3> v(1.0f, 0.0f, 0.4f);
+
+			// Rmoc is Rwr (robot respect to world) while v is respect to world.
+			// Hence Rmoc must be transposed having (Rwr)' * Vw
+			// Rrw * Vw = vn. This way we have consistency
+			_mocap_hdg = Rmoc.transposed() * v;
+		}
+
+		// Check for timeouts on data
+		if (_ext_hdg_mode == 1) {
+			_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
+
+		} else if (_ext_hdg_mode == 2) {
+			_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
+		}
+
 		bool gpos_updated;
 		orb_check(_global_pos_sub, &gpos_updated);
 
@@ -431,12 +495,9 @@ void AttitudeEstimatorQ::task_main()
 		att.pitch = euler(1);
 		att.yaw = euler(2);
 
-		/* the complimentary filter should reflect the true system
-		 * state, but we need smoother outputs for the control system
-		 */
-		att.rollspeed = _lp_roll_rate.apply(_rates(0));
-		att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
-		att.yawspeed = _lp_yaw_rate.apply(_rates(2));
+		att.rollspeed = _rates(0);
+		att.pitchspeed = _rates(1);
+		att.yawspeed = _rates(2);
 
 		for (int i = 0; i < 3; i++) {
 			att.g_comp[i] = _accel(i) - _pos_acc(i);
@@ -461,6 +522,34 @@ void AttitudeEstimatorQ::task_main()
 		} else {
 			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
 		}
+
+		struct control_state_s ctrl_state = {};
+
+		ctrl_state.timestamp = sensors.timestamp;
+
+		/* Attitude quaternions for control state */
+		ctrl_state.q[0] = _q(0);
+
+		ctrl_state.q[1] = _q(1);
+
+		ctrl_state.q[2] = _q(2);
+
+		ctrl_state.q[3] = _q(3);
+
+		/* Attitude rates for control state */
+		ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
+
+		ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
+
+		ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
+
+		/* Publish to control state topic */
+		if (_ctrl_state_pub == nullptr) {
+			_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
+
+		} else {
+			orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state);
+		}
 	}
 }
 
@@ -478,6 +567,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 
 		param_get(_params_handles.w_acc, &_w_accel);
 		param_get(_params_handles.w_mag, &_w_mag);
+		param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
 		param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
 		float mag_decl_deg = 0.0f;
 		param_get(_params_handles.mag_decl, &mag_decl_deg);
@@ -490,6 +580,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 		_acc_comp = acc_comp_int != 0;
 		param_get(_params_handles.bias_max, &_bias_max);
 		param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
+		param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
 	}
 }
 
@@ -545,12 +636,34 @@ bool AttitudeEstimatorQ::update(float dt)
 	// Angular rate of correction
 	Vector<3> corr;
 
-	// Magnetometer correction
-	// Project mag field vector to global frame and extract XY component
-	Vector<3> mag_earth = _q.conjugate(_mag);
-	float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
-	// Project magnetometer correction to body frame
-	corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
+	if (_ext_hdg_mode > 0 && _ext_hdg_good) {
+		if (_ext_hdg_mode == 1) {
+			// Vision heading correction
+			// Project heading to global frame and extract XY component
+			Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);
+			float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
+			// Project correction to body frame
+			corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
+		}
+
+		if (_ext_hdg_mode == 2) {
+			// Mocap heading correction
+			// Project heading to global frame and extract XY component
+			Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
+			float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
+			// Project correction to body frame
+			corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
+		}
+	}
+
+	if (_ext_hdg_mode == 0  || !_ext_hdg_good) {
+		// Magnetometer correction
+		// Project mag field vector to global frame and extract XY component
+		Vector<3> mag_earth = _q.conjugate(_mag);
+		float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
+		// Project magnetometer correction to body frame
+		corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
+	}
 
 	// Accelerometer correction
 	// Project 'k' unit vector of earth frame to body frame

src/modules/attitude_estimator_q/attitude_estimator_q_params.c

@@ -61,6 +61,15 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
  */
 PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
 
+/**
+ * Complimentary filter external heading weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f);
+
 /**
  * Complimentary filter gyroscope bias weight
  *
@@ -93,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
  */
 PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
 
+/**
+ * External heading usage mode (from Motion capture/Vision)
+ * Set to 1 to use heading estimate from vision.
+ * Set to 2 to use heading from motion capture.
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 2
+ */
+PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
+
 /**
  * Enable acceleration compensation based on GPS
  * velocity.

src/modules/commander/commander.cpp

@@ -180,6 +180,7 @@ static volatile bool thread_should_exit = false;	/**< daemon exit flag */
 static volatile bool thread_running = false;		/**< daemon status flag */
 static int daemon_task;					/**< Handle of daemon task / thread */
 static bool need_param_autosave = false;		/**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */
+static bool _usb_telemetry_active = false;
 static hrt_abstime commander_boot_timestamp = 0;
 
 static unsigned int leds_counter;
@@ -200,7 +201,7 @@ static struct home_position_s _home;
 static unsigned _last_mission_instance = 0;
 static manual_control_setpoint_s _last_sp_man;
 
-struct vtol_vehicle_status_s vtol_status;
+static struct vtol_vehicle_status_s vtol_status;
 
 /**
  * The daemon app only briefly exists to start
@@ -403,11 +404,11 @@ int commander_main(int argc, char *argv[])
 
 void usage(const char *reason)
 {
-	if (reason) {
-		PX4_INFO("%s\n", reason);
+	if (reason && *reason > 0) {
+		PX4_INFO("%s", reason);
 	}
 
-	PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n");
+	PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n");
 }
 
 void print_status()
@@ -1463,6 +1464,11 @@ int commander_thread_main(int argc, char *argv[])
 					}
 				}
 
+				/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
+				if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
+					_usb_telemetry_active = true;
+				}
+
 				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
 			}
 		}
@@ -1519,18 +1525,19 @@ int commander_thread_main(int argc, char *argv[])
 				/* copy avionics voltage */
 				status.avionics_power_rail_voltage = system_power.voltage5V_v;
 
+				/* if the USB hardware connection went away, reboot */
 				if (status.usb_connected && !system_power.usb_connected) {
 					/*
 					 * apparently the USB cable went away but we are still powered,
 					 * so lets reset to a classic non-usb state.
 					 */
-					usleep(100000);
 					mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.")
 					usleep(400000);
 					px4_systemreset(false);
 				}
 
-				status.usb_connected = system_power.usb_connected;
+				/* finally judge the USB connected state based on software detection */
+				status.usb_connected = _usb_telemetry_active;
 			}
 		}
 
@@ -3053,6 +3060,8 @@ void *commander_low_prio_loop(void *arg)
 
 					if (ret != OK) {
 						mavlink_and_console_log_critical(mavlink_fd, "settings auto save error");
+					} else {
+						warnx("settings saved.");
 					}
 
 					need_param_autosave = false;

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -49,6 +49,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_status.h>
@@ -154,12 +155,14 @@ private:
     int     _armedSub;
 
     orb_advert_t    _att_pub;           /**< vehicle attitude */
+    orb_advert_t    _ctrl_state_pub;        /**< control state */
     orb_advert_t    _global_pos_pub;        /**< global position */
     orb_advert_t    _local_pos_pub;         /**< position in local frame */
     orb_advert_t    _estimator_status_pub;      /**< status of the estimator */
     orb_advert_t    _wind_pub;          /**< wind estimate */
 
     struct vehicle_attitude_s           _att;           /**< vehicle attitude */
+    struct control_state_s              _ctrl_state;    /**< control state */
     struct gyro_report                  _gyro;
     struct accel_report                 _accel;
     struct mag_report                   _mag;
@@ -320,6 +323,12 @@ private:
     **/
     void publishAttitude();
 
+    /**
+    * @brief
+    *   Publish the system state for control modules
+    **/
+    void publishControlState();
+
     /**
     * @brief
     *   Publish local position relative to reference point where filter was initialized

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -134,12 +134,14 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 
 	/* publications */
 	_att_pub(nullptr),
+	_ctrl_state_pub(nullptr),
 	_global_pos_pub(nullptr),
 	_local_pos_pub(nullptr),
 	_estimator_status_pub(nullptr),
 	_wind_pub(nullptr),
 
 	_att{},
+	_ctrl_state{},
 	_gyro{},
 	_accel{},
 	_mag{},
@@ -698,6 +700,9 @@ void AttitudePositionEstimatorEKF::task_main()
 					// Publish attitude estimations
 					publishAttitude();
 
+					// publish control state
+					publishControlState();
+
 					// Publish Local Position estimations
 					publishLocalPosition();
 
@@ -822,9 +827,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.pitch = euler(1);
 	_att.yaw = euler(2);
 
-	_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
-	_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
-	_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
+	_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
+	_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;
+	_att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt;
 
 	// gyro offsets
 	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
@@ -833,7 +838,7 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 
 	/* lazily publish the attitude only once available */
 	if (_att_pub != nullptr) {
-		/* publish the attitude setpoint */
+		/* publish the attitude */
 		orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
 
 	} else {
@@ -842,6 +847,76 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	}
 }
 
+void AttitudePositionEstimatorEKF::publishControlState()
+{
+	/* Accelerations in Body Frame */
+	_ctrl_state.x_acc = _ekf->accel.x;
+	_ctrl_state.y_acc = _ekf->accel.y;
+	_ctrl_state.z_acc = _ekf->accel.z;
+
+	/* Velocity in Body Frame */
+	Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);
+	Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]);
+	Vector3f v_b = _ekf->Tnb * v_n;
+	Vector3f v_b_var = _ekf->Tnb * v_n_var;
+
+	_ctrl_state.x_vel = v_b.x;
+	_ctrl_state.y_vel = v_b.y;
+	_ctrl_state.z_vel = v_b.z;
+
+	_ctrl_state.vel_variance[0] = v_b_var.x;
+	_ctrl_state.vel_variance[1] = v_b_var.y;
+	_ctrl_state.vel_variance[2] = v_b_var.z;
+
+	/* Local Position */
+	_ctrl_state.x_pos = _ekf->states[7];
+	_ctrl_state.y_pos = _ekf->states[8];
+
+	// XXX need to announce change of Z reference somehow elegantly
+	_ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset;
+
+	_ctrl_state.pos_variance[0] = _ekf->P[7][7];
+	_ctrl_state.pos_variance[1] = _ekf->P[8][8];
+	_ctrl_state.pos_variance[2] = _ekf->P[9][9];
+
+	/* Attitude */
+	_ctrl_state.timestamp = _last_sensor_timestamp;
+	_ctrl_state.q[0] = _ekf->states[0];
+	_ctrl_state.q[1] = _ekf->states[1];
+	_ctrl_state.q[2] = _ekf->states[2];
+	_ctrl_state.q[3] = _ekf->states[3];
+
+	/* Airspeed (Groundspeed - Windspeed) */
+	_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] -  _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
+
+	/* Attitude Rates */
+	_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
+	_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
+	_ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
+
+	/* Guard from bad data */
+	if (!PX4_ISFINITE(_ctrl_state.x_vel) ||
+		!PX4_ISFINITE(_ctrl_state.y_vel) ||
+		!PX4_ISFINITE(_ctrl_state.z_vel) ||
+		!PX4_ISFINITE(_ctrl_state.x_pos) ||
+		!PX4_ISFINITE(_ctrl_state.y_pos) ||
+		!PX4_ISFINITE(_ctrl_state.z_pos))
+	{
+		// bad data, abort publication
+		return;
+	}
+
+	/* lazily publish the control state only once available */
+	if (_ctrl_state_pub != nullptr) {
+		/* publish the control state */
+		orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state);
+
+	} else {
+		/* advertise and publish */
+		_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state);
+	}
+}
+
 void AttitudePositionEstimatorEKF::publishLocalPosition()
 {
 	_local_pos.timestamp = _last_sensor_timestamp;
@@ -1672,8 +1747,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 			PX4_INFO(".");
 		}
 
-		PX4_INFO(" ");
-
 		return 0;
 	}
 

src/modules/ekf_att_pos_estimator/estimator_22states.cpp

@@ -48,6 +48,8 @@
 #define M_PI_F static_cast<float>(M_PI)
 #endif
 
+#define MIN_AIRSPEED_MEAS 5.0f
+
 constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
 
 AttPosEKF::AttPosEKF() :
@@ -1686,7 +1688,7 @@ void AttPosEKF::FuseAirspeed()
     // Calculate the predicted airspeed
     VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
     // Perform fusion of True Airspeed measurement
-    if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
+    if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS))
     {
         // Calculate observation jacobians
         SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
@@ -2594,7 +2596,7 @@ void AttPosEKF::CovarianceInit()
     P[13][13] = sq(0.2f*dtIMU);
 
     //Wind velocities
-    P[14][14] = 0.0f;
+    P[14][14] = 0.01f;
     P[15][15]  = P[14][14];
 
     //Earth magnetic field

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -58,7 +58,6 @@
 #include <drivers/drv_accel.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
@@ -67,7 +66,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/fw_virtual_rates_setpoint.h>
 #include <uORB/topics/mc_virtual_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -125,11 +124,10 @@ private:
 	bool		_task_running;			/**< if true, task is running in its mainloop */
 	int		_control_task;			/**< task handle */
 
-	int		_att_sub;			/**< vehicle attitude subscription */
+	int		_ctrl_state_sub;	/**< control state subscription */
 	int		_accel_sub;			/**< accelerometer subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
-	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_vcontrol_mode_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_sub;			/**< notification of manual control updates */
@@ -144,12 +142,11 @@ private:
 	orb_id_t _rates_sp_id;	// pointer to correct rates setpoint uORB metadata structure
 	orb_id_t _actuators_id;	// pointer to correct actuator controls0 uORB metadata structure
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct control_state_s				_ctrl_state;	/**< control state */
 	struct accel_report				_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct vehicle_rates_setpoint_s			_rates_sp;	/* attitude rates setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
 	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
 	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
 	struct actuator_controls_s			_actuators_airframe;	/**< actuator control inputs */
@@ -242,6 +239,11 @@ private:
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
+	// Rotation matrix and euler angles to extract from control state
+	math::Matrix<3, 3> _R;
+	float _roll;
+	float _pitch;
+	float _yaw;
 
 	ECL_RollController				_roll_ctrl;
 	ECL_PitchController				_pitch_ctrl;
@@ -269,12 +271,6 @@ private:
 	 */
 	void		vehicle_manual_poll();
 
-
-	/**
-	 * Check for airspeed updates.
-	 */
-	void		vehicle_airspeed_poll();
-
 	/**
 	 * Check for accel updates.
 	 */
@@ -326,9 +322,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_control_task(-1),
 
 /* subscriptions */
-	_att_sub(-1),
+	_ctrl_state_sub(-1),
 	_accel_sub(-1),
-	_airspeed_sub(-1),
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_sub(-1),
@@ -353,12 +348,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_debug(false)
 {
 	/* safely initialize structs */
-	_att = {};
+	_ctrl_state = {};
 	_accel = {};
 	_att_sp = {};
 	_rates_sp = {};
 	_manual = {};
-	_airspeed = {};
 	_vcontrol_mode = {};
 	_actuators = {};
 	_actuators_airframe = {};
@@ -539,18 +533,6 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_airspeed_poll()
-{
-	/* check if there is a new position */
-	bool airspeed_updated;
-	orb_check(_airspeed_sub, &airspeed_updated);
-
-	if (airspeed_updated) {
-		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
-	}
-}
-
 void
 FixedwingAttitudeControl::vehicle_accel_poll()
 {
@@ -623,9 +605,8 @@ FixedwingAttitudeControl::task_main()
 	 * do subscriptions
 	 */
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
-	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -641,7 +622,6 @@ FixedwingAttitudeControl::task_main()
 	parameters_update();
 
 	/* get an initial update for all sensor and status data */
-	vehicle_airspeed_poll();
 	vehicle_setpoint_poll();
 	vehicle_accel_poll();
 	vehicle_control_mode_poll();
@@ -654,7 +634,7 @@ FixedwingAttitudeControl::task_main()
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
 	fds[0].events = POLLIN;
-	fds[1].fd = _att_sub;
+	fds[1].fd = _ctrl_state_sub;
 	fds[1].events = POLLIN;
 
 	_task_running = true;
@@ -699,9 +679,19 @@ FixedwingAttitudeControl::task_main()
 				deltaT = 0.01f;
 
 			/* load local copies */
-			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+			orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
 
 
+			/* get current rotation matrix and euler angles from control state quaternions */
+			math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
+			_R = q_att.to_dcm();
+
+			math::Vector<3> euler_angles;
+			euler_angles = _R.to_euler();
+			_roll    = euler_angles(0);
+			_pitch   = euler_angles(1);
+			_yaw     = euler_angles(2);
+
 			if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
 				/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
 				 *
@@ -719,50 +709,36 @@ FixedwingAttitudeControl::task_main()
 				 * Rxy	Ryy  Rzy		-Rzy  Ryy  Rxy
 				 * Rxz	Ryz  Rzz		-Rzz  Ryz  Rxz
 				 * */
-				math::Matrix<3, 3> R;				//original rotation matrix
-				math::Matrix<3, 3> R_adapted;		//modified rotation matrix
-				R.set(_att.R);
-				R_adapted.set(_att.R);
+				math::Matrix<3, 3> R_adapted = _R;		//modified rotation matrix
 
 				/* move z to x */
-				R_adapted(0, 0) = R(0, 2);
-				R_adapted(1, 0) = R(1, 2);
-				R_adapted(2, 0) = R(2, 2);
+				R_adapted(0, 0) = _R(0, 2);
+				R_adapted(1, 0) = _R(1, 2);
+				R_adapted(2, 0) = _R(2, 2);
 
 				/* move x to z */
-				R_adapted(0, 2) = R(0, 0);
-				R_adapted(1, 2) = R(1, 0);
-				R_adapted(2, 2) = R(2, 0);
+				R_adapted(0, 2) = _R(0, 0);
+				R_adapted(1, 2) = _R(1, 0);
+				R_adapted(2, 2) = _R(2, 0);
 
 				/* change direction of pitch (convert to right handed system) */
 				R_adapted(0, 0) = -R_adapted(0, 0);
 				R_adapted(1, 0) = -R_adapted(1, 0);
 				R_adapted(2, 0) = -R_adapted(2, 0);
-				math::Vector<3> euler_angles;		//adapted euler angles for fixed wing operation
-				euler_angles = R_adapted.to_euler();
+				euler_angles = R_adapted.to_euler();  //adapted euler angles for fixed wing operation
 
 				/* fill in new attitude data */
-				_att.roll    = euler_angles(0);
-				_att.pitch   = euler_angles(1);
-				_att.yaw     = euler_angles(2);
-				PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
-				PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
-				PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
-				PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
-				PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
-				PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
-				PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
-				PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
-				PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
+				_R = R_adapted;
+				_roll    = euler_angles(0);
+				_pitch   = euler_angles(1);
+				_yaw     = euler_angles(2);
 
 				/* lastly, roll- and yawspeed have to be swaped */
-				float helper = _att.rollspeed;
-				_att.rollspeed = -_att.yawspeed;
-				_att.yawspeed = helper;
+				float helper = _ctrl_state.roll_rate;
+				_ctrl_state.roll_rate = -_ctrl_state.yaw_rate;
+				_ctrl_state.yaw_rate = helper;
 			}
 
-			vehicle_airspeed_poll();
-
 			vehicle_setpoint_poll();
 
 			vehicle_accel_poll();
@@ -813,15 +789,14 @@ FixedwingAttitudeControl::task_main()
 				float airspeed;
 
 				/* if airspeed is not updating, we assume the normal average speed */
-				if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
-				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
+				if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) {
 					airspeed = _parameters.airspeed_trim;
 					if (nonfinite) {
 						perf_count(_nonfinite_input_perf);
 					}
 				} else {
 					/* prevent numerical drama by requiring 0.5 m/s minimal speed */
-					airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
+					airspeed = math::max(0.5f, _ctrl_state.airspeed);
 				}
 
 				/*
@@ -866,7 +841,7 @@ FixedwingAttitudeControl::task_main()
 					/* the pilot does not want to change direction,
 					 * take straight attitude setpoint from position controller
 					 */
-					if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
+					if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
 						roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
 					} else {
 						roll_sp = (_manual.y * _parameters.man_roll_max)
@@ -957,27 +932,18 @@ FixedwingAttitudeControl::task_main()
 				}
 
 				/* Prepare speed_body_u and speed_body_w */
-				float speed_body_u = 0.0f;
-				float speed_body_v = 0.0f;
-				float speed_body_w = 0.0f;
-				if(_att.R_valid) 	{
-					speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
-					speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
-					speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
-				} else	{
-					if (_debug && loop_counter % 10 == 0) {
-						warnx("Did not get a valid R\n");
-					}
-				}
+				float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d;
+				float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d;
+				float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d;
 
 				/* Prepare data for attitude controllers */
 				struct ECL_ControlData control_input = {};
-				control_input.roll = _att.roll;
-				control_input.pitch = _att.pitch;
-				control_input.yaw = _att.yaw;
-				control_input.roll_rate = _att.rollspeed;
-				control_input.pitch_rate = _att.pitchspeed;
-				control_input.yaw_rate = _att.yawspeed;
+				control_input.roll = _roll;
+				control_input.pitch = _pitch;
+				control_input.yaw = _yaw;
+				control_input.roll_rate = _ctrl_state.roll_rate;
+				control_input.pitch_rate = _ctrl_state.pitch_rate;
+				control_input.yaw_rate = _ctrl_state.yaw_rate;
 				control_input.speed_body_u = speed_body_u;
 				control_input.speed_body_v = speed_body_v;
 				control_input.speed_body_w = speed_body_w;
@@ -1100,9 +1066,9 @@ FixedwingAttitudeControl::task_main()
 
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
-			_actuators.timestamp_sample = _att.timestamp;
+			_actuators.timestamp_sample = _ctrl_state.timestamp;
 			_actuators_airframe.timestamp = hrt_absolute_time();
-			_actuators_airframe.timestamp_sample = _att.timestamp;
+			_actuators_airframe.timestamp_sample = _ctrl_state.timestamp;
 
 			/* Only publish if any of the proper modes are enabled */
 			if(_vcontrol_mode.flag_control_rates_enabled ||

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -68,14 +68,13 @@
 #include <drivers/drv_accel.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/sensor_combined.h>
@@ -153,8 +152,7 @@ private:
 
 	int		_global_pos_sub;
 	int		_pos_sp_triplet_sub;
-	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_ctrl_state_sub;			/**< control state subscription */
 	int		_control_mode_sub;		/**< control mode subscription */
 	int		_vehicle_status_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
@@ -165,11 +163,10 @@ private:
 	orb_advert_t	_tecs_status_pub;		/**< TECS status publication */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
-	struct vehicle_attitude_s			_att;				/**< vehicle attitude */
+	struct control_state_s				_ctrl_state;			/**< control state */
 	struct vehicle_attitude_setpoint_s		_att_sp;			/**< vehicle attitude setpoint */
 	struct navigation_capabilities_s		_nav_capabilities;		/**< navigation capabilities */
 	struct manual_control_setpoint_s		_manual;			/**< r/c channel data */
-	struct airspeed_s				_airspeed;			/**< airspeed */
 	struct vehicle_control_mode_s			_control_mode;			/**< control mode */
 	struct vehicle_status_s				_vehicle_status;		/**< vehicle status */
 	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
@@ -222,6 +219,9 @@ private:
 	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
 	bool _global_pos_valid;				///< global position is valid
 	math::Matrix<3, 3> _R_nb;			///< current attitude
+	float _roll;
+	float _pitch;
+	float _yaw;
 
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
@@ -356,14 +356,9 @@ private:
 	bool		vehicle_manual_control_setpoint_poll();
 
 	/**
-	 * Check for airspeed updates.
+	 * Check for changes in control state.
 	 */
-	bool		vehicle_airspeed_poll();
-
-	/**
-	 * Check for position updates.
-	 */
-	void		vehicle_attitude_poll();
+	void		control_state_poll();
 
 	/**
 	 * Check for accel updates.
@@ -481,8 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 /* subscriptions */
 	_global_pos_sub(-1),
 	_pos_sp_triplet_sub(-1),
-	_att_sub(-1),
-	_airspeed_sub(-1),
+	_ctrl_state_sub(-1),
 	_control_mode_sub(-1),
 	_vehicle_status_sub(-1),
 	_params_sub(-1),
@@ -495,11 +489,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_nav_capabilities_pub(nullptr),
 
 /* states */
-	_att(),
+	_ctrl_state(),
 	_att_sp(),
 	_nav_capabilities(),
 	_manual(),
-	_airspeed(),
 	_control_mode(),
 	_vehicle_status(),
 	_global_pos(),
@@ -749,32 +742,6 @@ FixedwingPositionControl::vehicle_status_poll()
 	}
 }
 
-bool
-FixedwingPositionControl::vehicle_airspeed_poll()
-{
-	/* check if there is an airspeed update or if it timed out */
-	bool airspeed_updated;
-	orb_check(_airspeed_sub, &airspeed_updated);
-
-	if (airspeed_updated) {
-		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
-		_airspeed_valid = true;
-		_airspeed_last_valid = hrt_absolute_time();
-
-	} else {
-
-		/* no airspeed updates for one second */
-		if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
-			_airspeed_valid = false;
-		}
-	}
-
-	/* update TECS state */
-	_tecs.enable_airspeed(_airspeed_valid);
-
-	return airspeed_updated;
-}
-
 bool
 FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
 {
@@ -790,21 +757,38 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
 	return manual_updated;
 }
 
-
 void
-FixedwingPositionControl::vehicle_attitude_poll()
+FixedwingPositionControl::control_state_poll()
 {
 	/* check if there is a new position */
-	bool att_updated;
-	orb_check(_att_sub, &att_updated);
+	bool ctrl_state_updated;
+	orb_check(_ctrl_state_sub, &ctrl_state_updated);
 
-	if (att_updated) {
-		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+	if (ctrl_state_updated) {
+		orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
+		_airspeed_valid = true;
+		_airspeed_last_valid = hrt_absolute_time();
 
-		/* set rotation matrix */
-		for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-				_R_nb(i, j) = PX4_R(_att.R, i, j);
+	} else {
+
+		/* no airspeed updates for one second */
+		if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
+			_airspeed_valid = false;
+		}
 	}
+
+	/* set rotation matrix and euler angles */
+	math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
+	_R_nb = q_att.to_dcm();
+
+	math::Vector<3> euler_angles;
+	euler_angles = _R_nb.to_euler();
+	_roll    = euler_angles(0);
+	_pitch   = euler_angles(1);
+	_yaw     = euler_angles(2);
+	
+	/* update TECS state */
+	_tecs.enable_airspeed(_airspeed_valid);
 }
 
 void
@@ -853,7 +837,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 	float airspeed;
 
 	if (_airspeed_valid) {
-		airspeed = _airspeed.true_airspeed_m_s;
+		airspeed = _ctrl_state.airspeed;
 
 	} else {
 		airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
@@ -1084,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	/* update TECS filters */
 	if (!_mTecs.getEnabled()) {
-		_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
+		_tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb,
 			accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
 	}
 
@@ -1117,7 +1101,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
-				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+				_mTecs.resetDerivatives(_ctrl_state.airspeed);
 			} else {
 				_tecs.reset_state();
 			}
@@ -1127,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* reset hold altitude */
 		_hold_alt = _global_pos.alt;
 		/* reset hold yaw */
-		_hdg_hold_yaw = _att.yaw;
+		_hdg_hold_yaw = _yaw;
 
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
@@ -1221,14 +1205,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					if (pos_sp_triplet.previous.valid) {
 						target_bearing = bearing_lastwp_currwp;
 					} else {
-						target_bearing = _att.yaw;
+						target_bearing = _yaw;
 					}
 					mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
 				}
 
-//					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
+//					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
 
-				_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
+				_l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d);
 
 				land_noreturn_horizontal = true;
 
@@ -1469,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
 			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _global_pos.alt;
-			_hdg_hold_yaw = _att.yaw;
+			_hdg_hold_yaw = _yaw;
 			_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
 			_yaw_lock_engaged = false;
 		}
@@ -1478,7 +1462,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
-				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+				_mTecs.resetDerivatives(_ctrl_state.airspeed);
 			} else {
 				_tecs.reset_state();
 			}
@@ -1524,7 +1508,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
 			/* heading / roll is zero, lock onto current heading */
-			if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
+			if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
 				// little yaw movement, lock to current heading
 				_yaw_lock_engaged = true;
 
@@ -1543,7 +1527,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* just switched back from non heading-hold to heading hold */
 				if (!_hdg_hold_enabled) {
 					_hdg_hold_enabled = true;
-					_hdg_hold_yaw = _att.yaw;
+					_hdg_hold_yaw = _yaw;
 
 					get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
 				}
@@ -1591,7 +1575,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
-				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+				_mTecs.resetDerivatives(_ctrl_state.airspeed);
 			} else {
 				_tecs.reset_state();
 			}
@@ -1704,11 +1688,10 @@ FixedwingPositionControl::task_main()
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
@@ -1785,10 +1768,9 @@ FixedwingPositionControl::task_main()
 			// XXX add timestamp check
 			_global_pos_valid = true;
 
-			vehicle_attitude_poll();
+			control_state_poll();
 			vehicle_setpoint_poll();
 			vehicle_sensor_combined_poll();
-			vehicle_airspeed_poll();
 			vehicle_manual_control_setpoint_poll();
 			// vehicle_baro_poll();
 
@@ -1894,7 +1876,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 			/* use pitch max set by MT param */
 			limitOverride.disablePitchMaxOverride();
 		}
-		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
+		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode,
 				limitOverride);
 	} else {
 		if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
@@ -1907,8 +1889,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
 
 		/* Using tecs library */
-		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
-					    _airspeed.indicated_airspeed_m_s, eas2tas,
+		_tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp,
+					    _ctrl_state.airspeed, eas2tas,
 					    climbout_mode, climbout_pitch_min_rad,
 					    throttle_min, throttle_max, throttle_cruise,
 					    pitch_min_rad, pitch_max_rad);

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -811,7 +811,7 @@ void BlockLocalPositionEstimator::predict()
 	Q(X_bz, X_bz) = _pn_b_noise_power.get();
 
 	// continuous time kalman filter prediction
-	Matrix<float, n_x, 1> dx = (A * _x + B * _u) * getDt();
+	Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
 
 	// only predict for components we have
 	// valid measurements for
@@ -904,16 +904,16 @@ void BlockLocalPositionEstimator::correctFlow()
 	_flowY += global_speed[1] * dt;
 
 	// measurement
-	Vector2f y;
+	Vector<float, 2> y;
 	y(0) = _flowX;
 	y(1) = _flowY;
 
 	// residual
-	Vector2f r = y - C * _x;
+	Vector<float, 2> r = y - C * _x;
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_flow, n_y_flow> S_I =
-		(C * _P * C.transpose() + R).inverse();
+		inv<float, n_y_flow>(C * _P * C.transpose() + R);
 
 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -981,17 +981,17 @@ void BlockLocalPositionEstimator::correctSonar()
 	}
 
 	// measurement
-	Matrix<float, n_y_sonar, 1> y;
+	Vector<float, n_y_sonar> y;
 	y(0) = (d - _sonarAltHome) *
 	       cosf(_sub_att.get().roll) *
 	       cosf(_sub_att.get().pitch);
 
 	// residual
-	Matrix<float, n_y_sonar, 1> r = y - C * _x;
+	Vector<float, n_y_sonar> r = y - C * _x;
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_sonar, n_y_sonar> S_I =
-		(C * _P * C.transpose() + R).inverse();
+		inv<float, n_y_sonar>(C * _P * C.transpose() + R);
 
 	// fault detection
 	float beta = sqrtf((r.transpose()  * (S_I * r))(0, 0));
@@ -1032,7 +1032,7 @@ void BlockLocalPositionEstimator::correctSonar()
 void BlockLocalPositionEstimator::correctBaro()
 {
 
-	Matrix<float, n_y_baro, 1> y;
+	Vector<float, n_y_baro> y;
 	y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome;
 
 	// baro measurement matrix
@@ -1046,8 +1046,8 @@ void BlockLocalPositionEstimator::correctBaro()
 
 	// residual
 	Matrix<float, n_y_baro, n_y_baro> S_I =
-		((C * _P * C.transpose()) + R).inverse();
-	Matrix<float, n_y_baro, 1> r = y - (C * _x);
+		inv<float, n_y_baro>((C * _P * C.transpose()) + R);
+	Vector<float, n_y_baro> r = y - (C * _x);
 
 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1060,7 +1060,7 @@ void BlockLocalPositionEstimator::correctBaro()
 		}
 
 		// lower baro trust
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, n_y_baro>((C * _P * C.transpose()) + R * 10);
 
 	} else if (_baroFault) {
 		_baroFault = FAULT_NONE;
@@ -1104,15 +1104,15 @@ void BlockLocalPositionEstimator::correctLidar()
 		R(0, 0) = cov;
 	}
 
-	Matrix<float, n_y_lidar, 1> y;
+	Vector<float, n_y_lidar> y;
 	y.setZero();
 	y(0) = (d - _lidarAltHome) *
 	       cosf(_sub_att.get().roll) *
 	       cosf(_sub_att.get().pitch);
 
 	// residual
-	Matrix<float, n_y_lidar, n_y_lidar> S_I = ((C * _P * C.transpose()) + R).inverse();
-	Matrix<float, n_y_lidar, 1> r = y - C * _x;
+	Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>((C * _P * C.transpose()) + R);
+	Vector<float, n_y_lidar> r = y - C * _x;
 
 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1166,7 +1166,7 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 	//printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt));
 	//printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz));
 
-	Matrix<float, 6, 1> y;
+	Vector<float, 6> y;
 	y.setZero();
 	y(0) = px;
 	y(1) = py;
@@ -1213,8 +1213,8 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 	R(5, 5) = var_vz;
 
 	// residual
-	Matrix<float, 6, 1> r = y - C * _x;
-	Matrix<float, 6, 6> S_I = (C * _P * C.transpose() + R).inverse();
+	Vector<float, 6> r = y - C * _x;
+	Matrix<float, 6, 6> S_I = inv<float, 6>(C * _P * C.transpose() + R);
 
 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1245,7 +1245,7 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 		}
 
 		// trust GPS less
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, 6>((C * _P * C.transpose()) + R * 10);
 
 	} else if (_gpsFault) {
 		_gpsFault = FAULT_NONE;
@@ -1266,7 +1266,7 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 void BlockLocalPositionEstimator::correctVision()
 {
 
-	Matrix<float, 3, 1> y;
+	Vector<float, 3> y;
 	y.setZero();
 	y(0) = _sub_vision_pos.get().x - _visionHome(0);
 	y(1) = _sub_vision_pos.get().y - _visionHome(1);
@@ -1287,7 +1287,7 @@ void BlockLocalPositionEstimator::correctVision()
 	R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
 
 	// residual
-	Matrix<float, n_y_vision, n_y_vision> S_I = ((C * _P * C.transpose()) + R).inverse();
+	Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
 	Matrix<float, n_y_vision, 1> r = y - C * _x;
 
 	// fault detection
@@ -1301,7 +1301,7 @@ void BlockLocalPositionEstimator::correctVision()
 		}
 
 		// trust less
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R * 10);
 
 	} else if (_visionFault) {
 		_visionFault = FAULT_NONE;
@@ -1322,7 +1322,7 @@ void BlockLocalPositionEstimator::correctVision()
 void BlockLocalPositionEstimator::correctmocap()
 {
 
-	Matrix<float, n_y_mocap, 1> y;
+	Vector<float, n_y_mocap> y;
 	y.setZero();
 	y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0);
 	y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1);
@@ -1345,7 +1345,7 @@ void BlockLocalPositionEstimator::correctmocap()
 	R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
 
 	// residual
-	Matrix<float, n_y_mocap, n_y_mocap> S_I = ((C * _P * C.transpose()) + R).inverse();
+	Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R);
 	Matrix<float, n_y_mocap, 1> r = y - C * _x;
 
 	// fault detection
@@ -1359,7 +1359,7 @@ void BlockLocalPositionEstimator::correctmocap()
 		}
 
 		// trust less
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R * 10);
 
 	} else if (_mocapFault) {
 		_mocapFault = FAULT_NONE;

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -6,7 +6,7 @@
 #include <lib/geo/geo.h>
 
 #ifdef USE_MATRIX_LIB
-#include "matrix/src/Matrix.hpp"
+#include "matrix/Matrix.hpp"
 using namespace matrix;
 #else
 #include <Eigen/Eigen>
@@ -304,7 +304,7 @@ private:
 	perf_counter_t _err_perf;
 
 	// state space
-	Matrix<float, n_x, 1>  _x; // state vector
-	Matrix<float, n_u, 1>  _u; // input vector
+	Vector<float, n_x>  _x; // state vector
+	Vector<float, n_u>  _u; // input vector
 	Matrix<float, n_x, n_x>  _P; // state covariance matrix
 };

src/modules/local_position_estimator/CMakeLists.txt

@@ -34,8 +34,6 @@ if(${OS} STREQUAL "nuttx")
 	list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500)
 elseif(${OS} STREQUAL "posix")
 	list(APPEND MODULE_CFLAGS -Wno-error)
-	# add matrix tests
-	add_subdirectory(matrix)
 endif()
 
 # use custom matrix lib instead of Eigen

src/modules/local_position_estimator/matrix/.gitignore

@@ -1 +0,0 @@
-build*/

src/modules/local_position_estimator/matrix/CMakeLists.txt

@@ -1,13 +0,0 @@
-cmake_minimum_required(VERSION 2.8)
-
-project(matrix CXX)
-
-set(CMAKE_BUILD_TYPE "RelWithDebInfo")
-
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++")
-
-enable_testing()
-
-include_directories(src)
-
-add_subdirectory(test)

src/modules/local_position_estimator/matrix/src/Matrix.hpp

@@ -1,464 +0,0 @@
-#pragma once
-
-#include <stdio.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-namespace matrix
-{
-
-template<typename T, size_t  M, size_t N>
-class Matrix
-{
-
-private:
-	T _data[M][N];
-	size_t _rows;
-	size_t _cols;
-
-public:
-
-	Matrix() :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-	}
-
-	Matrix(const T *data) :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-		memcpy(_data, data, sizeof(_data));
-	}
-
-	Matrix(const Matrix &other) :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-		memcpy(_data, other._data, sizeof(_data));
-	}
-
-	Matrix(T x, T y, T z) :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-		// TODO, limit to only 3x1 matrices
-		_data[0][0] = x;
-		_data[1][0] = y;
-		_data[2][0] = z;
-	}
-
-	/**
-	 * Accessors/ Assignment etc.
-	 */
-
-	T *data()
-	{
-		return _data[0];
-	}
-
-	inline size_t rows() const
-	{
-		return _rows;
-	}
-
-	inline size_t cols() const
-	{
-		return _rows;
-	}
-
-	inline T operator()(size_t i) const
-	{
-		return _data[i][0];
-	}
-
-	inline T operator()(size_t i, size_t j) const
-	{
-		return _data[i][j];
-	}
-
-	inline T &operator()(size_t i)
-	{
-		return _data[i][0];
-	}
-
-	inline T &operator()(size_t i, size_t j)
-	{
-		return _data[i][j];
-	}
-
-	/**
-	 * Matrix Operations
-	 */
-
-	// this might use a lot of programming memory
-	// since it instantiates a class for every
-	// required mult pair, but it provides
-	// compile time size_t checking
-	template<size_t P>
-	Matrix<T, M, P> operator*(const Matrix<T, N, P> &other) const
-	{
-		const Matrix<T, M, N> &self = *this;
-		Matrix<T, M, P> res;
-		res.setZero();
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t k = 0; k < P; k++) {
-				for (size_t j = 0; j < N; j++) {
-					res(i, k) += self(i, j) * other(j, k);
-				}
-			}
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, N> operator+(const Matrix<T, M, N> &other) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) + other(i, j);
-			}
-		}
-
-		return res;
-	}
-
-	bool operator==(const Matrix<T, M, N> &other) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				if (self(i , j) != other(i, j)) {
-					return false;
-				}
-			}
-		}
-
-		return true;
-	}
-
-	Matrix<T, M, N> operator-(const Matrix<T, M, N> &other) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) - other(i, j);
-			}
-		}
-
-		return res;
-	}
-
-	void operator+=(const Matrix<T, M, N> &other)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self + other;
-	}
-
-	void operator-=(const Matrix<T, M, N> &other)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self - other;
-	}
-
-	void operator*=(const Matrix<T, M, N> &other)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self * other;
-	}
-
-	/**
-	 * Scalar Operations
-	 */
-
-	Matrix<T, M, N> operator*(T scalar) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) * scalar;
-			}
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, N> operator+(T scalar) const
-	{
-		Matrix<T, M, N> res;
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) + scalar;
-			}
-		}
-
-		return res;
-	}
-
-	void operator*=(T scalar)
-	{
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				self(i, j) = self(i, j) * scalar;
-			}
-		}
-	}
-
-	void operator/=(T scalar)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self * (1.0f / scalar);
-	}
-
-	/**
-	 * Misc. Functions
-	 */
-
-	void print()
-	{
-		Matrix<T, M, N> &self = *this;
-		printf("\n");
-
-		for (size_t i = 0; i < M; i++) {
-			printf("[");
-
-			for (size_t j = 0; j < N; j++) {
-				printf("%10g\t", double(self(i, j)));
-			}
-
-			printf("]\n");
-		}
-	}
-
-	Matrix<T, N, M> transpose() const
-	{
-		Matrix<T, N, M> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(j, i) = self(i, j);
-			}
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, M> expm(float dt, size_t n) const
-	{
-		Matrix<float, M, M> res;
-		res.setIdentity();
-		Matrix<float, M, M> A_pow = *this;
-		float k_fact = 1;
-		size_t k = 1;
-
-		while (k < n) {
-			res += A_pow * (float(pow(dt, k)) / k_fact);
-
-			if (k == n) { break; }
-
-			A_pow *= A_pow;
-			k_fact *= k;
-			k++;
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, 1> diagonal() const
-	{
-		Matrix<T, M, 1> res;
-		// force square for now
-		const Matrix<T, M, M> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			res(i) = self(i, i);
-		}
-
-		return res;
-	}
-
-	void setZero()
-	{
-		memset(_data, 0, sizeof(_data));
-	}
-
-	void setIdentity()
-	{
-		setZero();
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M and i < N; i++) {
-			self(i, i) = 1;
-		}
-	}
-
-	inline void swapRows(size_t a, size_t b)
-	{
-		if (a == b) { return; }
-
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t j = 0; j < cols(); j++) {
-			T tmp = self(a, j);
-			self(a, j) = self(b, j);
-			self(b, j) = tmp;
-		}
-	}
-
-	inline void swapCols(size_t a, size_t b)
-	{
-		if (a == b) { return; }
-
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < rows(); i++) {
-			T tmp = self(i, a);
-			self(i, a) = self(i, b);
-			self(i, b) = tmp;
-		}
-	}
-
-	/**
-	 * inverse based on LU factorization with partial pivotting
-	 */
-	Matrix <T, M, M> inverse() const
-	{
-		Matrix<T, M, M> L;
-		L.setIdentity();
-		const Matrix<T, M, M> &A = (*this);
-		Matrix<T, M, M> U = A;
-		Matrix<T, M, M> P;
-		P.setIdentity();
-
-		//printf("A:\n"); A.print();
-
-		// for all diagonal elements
-		for (size_t n = 0; n < N; n++) {
-
-			// if diagonal is zero, swap with row below
-			if (fabsf(U(n, n)) < 1e-8f) {
-				//printf("trying pivot for row %d\n",n);
-				for (size_t i = 0; i < N; i++) {
-					if (i == n) { continue; }
-
-					//printf("\ttrying row %d\n",i);
-					if (fabsf(U(i, n)) > 1e-8f) {
-						//printf("swapped %d\n",i);
-						U.swapRows(i, n);
-						P.swapRows(i, n);
-					}
-				}
-			}
-
-#ifdef MATRIX_ASSERT
-			//printf("A:\n"); A.print();
-			//printf("U:\n"); U.print();
-			//printf("P:\n"); P.print();
-			//fflush(stdout);
-			ASSERT(fabsf(U(n, n)) > 1e-8f);
-#endif
-
-			// failsafe, return zero matrix
-			if (fabsf(U(n, n)) < 1e-8f) {
-				Matrix<T, M, M> zero;
-				zero.setZero();
-				return zero;
-			}
-
-			// for all rows below diagonal
-			for (size_t i = (n + 1); i < N; i++) {
-				L(i, n) = U(i, n) / U(n, n);
-
-				// add i-th row and n-th row
-				// multiplied by: -a(i,n)/a(n,n)
-				for (size_t k = n; k < N; k++) {
-					U(i, k) -= L(i, n) * U(n, k);
-				}
-			}
-		}
-
-		//printf("L:\n"); L.print();
-		//printf("U:\n"); U.print();
-
-		// solve LY=P*I for Y by forward subst
-		Matrix<T, M, M> Y = P;
-
-		// for all columns of Y
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of L
-			for (size_t i = 0; i < N; i++) {
-				// for all columns of L
-				for (size_t j = 0; j < i; j++) {
-					// for all existing y
-					// subtract the component they
-					// contribute to the solution
-					Y(i, c) -= L(i, j) * Y(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				// Y(i,c) /= L(i,i);
-				// but L(i,i) = 1.0
-			}
-		}
-
-		//printf("Y:\n"); Y.print();
-
-		// solve Ux=y for x by back subst
-		Matrix<T, M, M> X = Y;
-
-		// for all columns of X
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of U
-			for (size_t k = 0; k < N; k++) {
-				// have to go in reverse order
-				size_t i = N - 1 - k;
-
-				// for all columns of U
-				for (size_t j = i + 1; j < N; j++) {
-					// for all existing x
-					// subtract the component they
-					// contribute to the solution
-					X(i, c) -= U(i, j) * X(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				X(i, c) /= U(i, i);
-			}
-		}
-
-		//printf("X:\n"); X.print();
-		return X;
-	}
-
-};
-
-typedef Matrix<float, 2, 1> Vector2f;
-typedef Matrix<float, 3, 1> Vector3f;
-typedef Matrix<float, 3, 3> Matrix3f;
-
-}; // namespace matrix

src/modules/local_position_estimator/matrix/test/CMakeLists.txt

@@ -1,15 +0,0 @@
-set(tests
-	setIdentity
-	inverse
-	matrixMult
-	vectorAssignment
-	matrixAssignment
-	matrixScalarMult
-	transpose
-	)
-
-foreach(test ${tests})
-	add_executable(${test}
-		${test}.cpp)
-	add_test(${test} ${test})
-endforeach()

src/modules/local_position_estimator/matrix/test/inverse.cpp

@@ -1,30 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
-	Matrix3f A(data);
-	Matrix3f A_I = A.inverse();
-	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
-	Matrix3f A_I_check(data_check);
-	(void)A_I;
-	assert(A_I == A_I_check);
-
-	// stess test
-	static const size_t n = 100;
-	Matrix<float, n, n> A_large;
-	A_large.setIdentity();
-	Matrix<float, n, n> A_large_I;
-	A_large_I.setZero();
-
-	for (size_t i = 0; i < 100; i++) {
-		A_large_I = A_large.inverse();
-		assert(A_large == A_large_I);
-	}
-
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp

@@ -1,29 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-
-using namespace matrix;
-
-int main()
-{
-	Matrix3f m;
-	m.setZero();
-	m(0, 0) = 1;
-	m(0, 1) = 2;
-	m(0, 2) = 3;
-	m(1, 0) = 4;
-	m(1, 1) = 5;
-	m(1, 2) = 6;
-	m(2, 0) = 7;
-	m(2, 1) = 8;
-	m(2, 2) = 9;
-
-	m.print();
-
-	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
-	Matrix3f m2(data);
-	m2.print();
-
-	assert(m == m2);
-
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/matrixMult.cpp

@@ -1,26 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
-	Matrix3f A(data);
-	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
-	Matrix3f A_I(data_check);
-	Matrix3f I;
-	I.setIdentity();
-	A.print();
-	A_I.print();
-	Matrix3f R = A * A_I;
-	R.print();
-	assert(R == I);
-
-	Matrix3f R2 = A;
-	R2 *= A_I;
-	R2.print();
-	assert(R2 == I);
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp

@@ -1,18 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
-	Matrix3f A(data);
-	A = A * 2;
-	float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
-	Matrix3f A_check(data_check);
-	A.print();
-	A_check.print();
-	assert(A == A_check);
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/setIdentity.cpp

@@ -1,25 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-
-using namespace matrix;
-
-int main()
-{
-	Matrix3f A;
-	A.setIdentity();
-	assert(A.rows() == 3);
-	assert(A.cols() == 3);
-
-	for (int i = 0; i < 3; i++) {
-		for (int j = 0; j < 3; j++) {
-			if (i == j) {
-				assert(A(i, j) == 1);
-
-			} else {
-				assert(A(i, j) == 0);
-			}
-		}
-	}
-
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/transpose.cpp

@@ -1,18 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 2, 3, 4, 5, 6};
-	Matrix<float, 2, 3> A(data);
-	Matrix<float, 3, 2> A_T = A.transpose();
-	float data_check[9] = {1, 4, 2, 5, 3, 6};
-	Matrix<float, 3, 2> A_T_check(data_check);
-	A_T.print();
-	A_T_check.print();
-	assert(A_T == A_T_check);
-	return 0;
-}

src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp

@@ -1,28 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-
-using namespace matrix;
-
-int main()
-{
-	Vector3f v;
-	v(0) = 1;
-	v(1) = 2;
-	v(2) = 3;
-
-	v.print();
-
-	assert(v(0) == 1);
-	assert(v(1) == 2);
-	assert(v(2) == 3);
-
-	Vector3f v2(4, 5, 6);
-
-	v2.print();
-
-	assert(v2(0) == 4);
-	assert(v2(1) == 5);
-	assert(v2(2) == 6);
-
-	return 0;
-}

src/modules/mavlink/CMakeLists.txt

@@ -56,7 +56,6 @@ px4_add_module(
 		mavlink_rate_limiter.cpp
 		mavlink_receiver.cpp
 		mavlink_ftp.cpp
-		mavlink_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/mavlink/mavlink_main.cpp

@@ -698,6 +698,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 
 	} else {
 		_is_usb_uart = true;
+		/* USB has no baudrate, but use a magic number for 'fast' */
+		_baudrate = 2000000;
+		_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB;
 	}
 
 #if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
@@ -2094,8 +2097,12 @@ Mavlink::display_status()
 			printf("3DR RADIO\n");
 			break;
 
+		case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB:
+			printf("USB CDC\n");
+			break;
+
 		default:
-			printf("UNKNOWN RADIO\n");
+			printf("GENERIC LINK OR RADIO\n");
 			break;
 		}
 

src/modules/mavlink/mavlink_main.h

@@ -288,6 +288,8 @@ public:
 
 	float			get_rate_mult();
 
+	float			get_baudrate() { return _baudrate; }
+
 	/* Functions for waiting to start transmission until message received. */
 	void			set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
 	bool			get_has_received_messages() { return _received_messages; }

src/modules/mavlink/mavlink_params.c

@@ -31,8 +31,6 @@
  *
  ****************************************************************************/
 
-#include <systemlib/param/param.h>
-
 /**
  * MAVLink system ID
  * @group MAVLink

src/modules/mavlink/mavlink_receiver.cpp

@@ -1757,7 +1757,7 @@ MavlinkReceiver::receive_thread(void *arg)
 	uint8_t buf[1600];
 #else
 	/* the serial port buffers internally as well, we just need to fit a small chunk */
-	uint8_t buf[32];
+	uint8_t buf[64];
 #endif
 	mavlink_message_t msg;
 
@@ -1797,10 +1797,17 @@ MavlinkReceiver::receive_thread(void *arg)
 	while (!_mavlink->_task_should_exit) {
 		if (poll(&fds[0], 1, timeout) > 0) {
 			if (_mavlink->get_protocol() == SERIAL) {
+
+				/*
+				 * to avoid reading very small chunks wait for data before reading
+				 * this is designed to target one message, so >20 bytes at a time
+				 */
+				const unsigned character_count = 20;
+
 				/* non-blocking read. read may return negative values */
-				if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
-					/* to avoid reading very small chunks wait for data before reading */
-					usleep(1000);
+				if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)character_count) {
+					unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000;
+					usleep(sleeptime);
 				}
 			}
 #ifdef __PX4_POSIX

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -74,7 +74,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/fw_virtual_rates_setpoint.h>
 #include <uORB/topics/mc_virtual_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
@@ -126,7 +126,7 @@ private:
 	bool	_task_should_exit;		/**< if true, task_main() should exit */
 	int		_control_task;			/**< task handle */
 
-	int		_v_att_sub;				/**< vehicle attitude subscription */
+	int		_ctrl_state_sub;		/**< control state subscription */
 	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
 	int		_v_rates_sp_sub;		/**< vehicle rates setpoint subscription */
 	int		_v_control_mode_sub;	/**< vehicle control mode subscription */
@@ -145,7 +145,7 @@ private:
 
 	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
 
-	struct vehicle_attitude_s			_v_att;				/**< vehicle attitude */
+	struct control_state_s				_ctrl_state;		/**< control state */
 	struct vehicle_attitude_setpoint_s	_v_att_sp;			/**< vehicle attitude setpoint */
 	struct vehicle_rates_setpoint_s		_v_rates_sp;		/**< vehicle rates setpoint */
 	struct manual_control_setpoint_s	_manual_control_sp;	/**< manual control setpoint */
@@ -297,7 +297,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_control_task(-1),
 
 /* subscriptions */
-	_v_att_sub(-1),
+	_ctrl_state_sub(-1),
 	_v_att_sp_sub(-1),
 	_v_control_mode_sub(-1),
 	_params_sub(-1),
@@ -319,7 +319,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
 
 {
-	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_ctrl_state, 0, sizeof(_ctrl_state));
 	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
 	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
 	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
@@ -599,9 +599,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	math::Matrix<3, 3> R_sp;
 	R_sp.set(_v_att_sp.R_body);
 
-	/* rotation matrix for current state */
-	math::Matrix<3, 3> R;
-	R.set(_v_att.R);
+	/* get current rotation matrix from control state quaternions */
+	math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
+	math::Matrix<3, 3> R = q_att.to_dcm();
 
 	/* all input data is ready, run controller itself */
 
@@ -693,9 +693,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 
 	/* current body angular rates */
 	math::Vector<3> rates;
-	rates(0) = _v_att.rollspeed;
-	rates(1) = _v_att.pitchspeed;
-	rates(2) = _v_att.yawspeed;
+	rates(0) = _ctrl_state.roll_rate;
+	rates(1) = _ctrl_state.pitch_rate;
+	rates(2) = _ctrl_state.yaw_rate;
 
 	/* angular rates error */
 	math::Vector<3> rates_err = _rates_sp - rates;
@@ -733,7 +733,7 @@ MulticopterAttitudeControl::task_main()
 	 */
 	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -747,7 +747,7 @@ MulticopterAttitudeControl::task_main()
 	/* wakeup source: vehicle attitude */
 	px4_pollfd_struct_t fds[1];
 
-	fds[0].fd = _v_att_sub;
+	fds[0].fd = _ctrl_state_sub;
 	fds[0].events = POLLIN;
 
 	while (!_task_should_exit) {
@@ -783,8 +783,8 @@ MulticopterAttitudeControl::task_main()
 				dt = 0.02f;
 			}
 
-			/* copy attitude topic */
-			orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+			/* copy attitude and control state topics */
+			orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
 
 			/* check for updates in other topics */
 			parameter_update_poll();
@@ -862,7 +862,7 @@ MulticopterAttitudeControl::task_main()
 				_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
 				_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
 				_actuators.timestamp = hrt_absolute_time();
-				_actuators.timestamp_sample = _v_att.timestamp;
+				_actuators.timestamp_sample = _ctrl_state.timestamp;
 
 				_controller_status.roll_rate_integ = _rates_int(0);
 				_controller_status.pitch_rate_integ = _rates_int(1);

src/modules/mc_att_control_multiplatform/CMakeLists.txt

@@ -38,7 +38,6 @@ px4_add_module(
 		mc_att_control_start_nuttx.cpp
 		mc_att_control.cpp
 		mc_att_control_base.cpp
-		mc_att_control_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/mc_att_control_multiplatform/mc_att_control_params.c

@@ -36,14 +36,10 @@
  * Parameters for multicopter attitude controller.
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <px4_defines.h>
-#include "mc_att_control_params.h"
-#include <systemlib/param/param.h>
-
 /**
  * Roll P gain
  *

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -68,7 +68,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
@@ -119,14 +119,14 @@ public:
 	int		start();
 
 private:
-	const float alt_ctl_dz = 0.2f;
+	const float alt_ctl_dz = 0.1f;
 
 	bool		_task_should_exit;		/**< if true, task should exit */
 	int		_control_task;			/**< task handle for task */
 	int		_mavlink_fd;			/**< mavlink fd */
 
 	int		_vehicle_status_sub;		/**< vehicle status subscription */
-	int		_att_sub;			/**< vehicle attitude subscription */
+	int		_ctrl_state_sub;			/**< control state subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_params_sub;			/**< notification of parameter updates */
@@ -142,7 +142,7 @@ private:
 	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
 	struct vehicle_status_s 			_vehicle_status; 	/**< vehicle status */
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct control_state_s				_ctrl_state;			/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
 	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
@@ -230,6 +230,9 @@ private:
 	math::Vector<3> _vel_prev;			/**< velocity on previous step */
 	math::Vector<3> _vel_ff;
 
+	math::Matrix<3, 3> _R;			/**< rotation matrix from attitude quaternions */
+	float _yaw;				/**< yaw angle (euler) */
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -319,7 +322,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_mavlink_fd(-1),
 
 /* subscriptions */
-	_att_sub(-1),
+	_ctrl_state_sub(-1),
 	_att_sp_sub(-1),
 	_control_mode_sub(-1),
 	_params_sub(-1),
@@ -347,10 +350,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_pos_hold_engaged(false),
 	_alt_hold_engaged(false),
 	_run_pos_control(true),
-	_run_alt_control(true)
+	_run_alt_control(true),
+	_yaw(0.0f)
 {
 	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
-	memset(&_att, 0, sizeof(_att));
+	memset(&_ctrl_state, 0, sizeof(_ctrl_state));
 	memset(&_att_sp, 0, sizeof(_att_sp));
 	memset(&_manual, 0, sizeof(_manual));
 	memset(&_control_mode, 0, sizeof(_control_mode));
@@ -377,6 +381,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_vel_prev.zero();
 	_vel_ff.zero();
 
+	_R.identity();
+
 	_params_handles.thr_min		= param_find("MPC_THR_MIN");
 	_params_handles.thr_max		= param_find("MPC_THR_MAX");
 	_params_handles.z_p			= param_find("MPC_Z_P");
@@ -529,10 +535,10 @@ MulticopterPositionControl::poll_subscriptions()
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 	}
 
-	orb_check(_att_sub, &updated);
+	orb_check(_ctrl_state_sub, &updated);
 
 	if (updated) {
-		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+		orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
 	}
 
 	orb_check(_att_sp_sub, &updated);
@@ -855,18 +861,45 @@ void MulticopterPositionControl::control_auto(float dt)
 		}
 	}
 
+	bool current_setpoint_valid = false;
+	bool previous_setpoint_valid = false;
+
+	math::Vector<3> prev_sp;
+	math::Vector<3> curr_sp;
+
 	if (_pos_sp_triplet.current.valid) {
-		/* in case of interrupted mission don't go to waypoint but stay at current position */
-		_reset_pos_sp = true;
-		_reset_alt_sp = true;
 
 		/* project setpoint to local frame */
-		math::Vector<3> curr_sp;
 		map_projection_project(&_ref_pos,
 				       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
 				       &curr_sp.data[0], &curr_sp.data[1]);
 		curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
 
+		if (PX4_ISFINITE(curr_sp(0)) &&
+			PX4_ISFINITE(curr_sp(1)) &&
+			PX4_ISFINITE(curr_sp(2))) {
+			current_setpoint_valid = true;
+		}
+	}
+
+	if (_pos_sp_triplet.previous.valid) {
+		map_projection_project(&_ref_pos,
+					   _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
+					   &prev_sp.data[0], &prev_sp.data[1]);
+		prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
+
+		if (PX4_ISFINITE(prev_sp(0)) &&
+			PX4_ISFINITE(prev_sp(1)) &&
+			PX4_ISFINITE(prev_sp(2))) {
+			previous_setpoint_valid = true;
+		}
+	}
+
+	if (current_setpoint_valid) {
+		/* in case of interrupted mission don't go to waypoint but stay at current position */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+
 		/* scaled space: 1 == position error resulting max allowed speed */
 		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
 
@@ -876,13 +909,8 @@ void MulticopterPositionControl::control_auto(float dt)
 		/* by default use current setpoint as is */
 		math::Vector<3> pos_sp_s = curr_sp_s;
 
-		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
+		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) {
 			/* follow "previous - current" line */
-			math::Vector<3> prev_sp;
-			map_projection_project(&_ref_pos,
-						   _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
-						   &prev_sp.data[0], &prev_sp.data[1]);
-			prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
 
 			if ((curr_sp - prev_sp).length() > MIN_DIST) {
 
@@ -987,7 +1015,7 @@ MulticopterPositionControl::task_main()
 	 * do subscriptions
 	 */
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -1042,6 +1070,14 @@ MulticopterPositionControl::task_main()
 		}
 
 		poll_subscriptions();
+
+		/* get current rotation matrix and euler angles from control state quaternions */
+		math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
+		_R = q_att.to_dcm();
+		math::Vector<3> euler_angles;
+		euler_angles = _R.to_euler();
+		_yaw     = euler_angles(2);
+
 		parameters_update(false);
 
 		hrt_abstime t = hrt_absolute_time();
@@ -1117,7 +1153,7 @@ MulticopterPositionControl::task_main()
 
 				_att_sp.roll_body = 0.0f;
 				_att_sp.pitch_body = 0.0f;
-				_att_sp.yaw_body = _att.yaw;
+				_att_sp.yaw_body = _yaw;
 				_att_sp.thrust = 0.0f;
 
 				_att_sp.timestamp = hrt_absolute_time();
@@ -1302,11 +1338,11 @@ MulticopterPositionControl::task_main()
 						/* thrust compensation for altitude only control mode */
 						float att_comp;
 
-						if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
-							att_comp = 1.0f / PX4_R(_att.R, 2, 2);
+						if (_R(2, 2) > TILT_COS_MAX) {
+							att_comp = 1.0f / _R(2, 2);
 
-						} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
-							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
+						} else if (_R(2, 2) > 0.0f) {
+							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
 							saturation_z = true;
 
 						} else {
@@ -1493,7 +1529,7 @@ MulticopterPositionControl::task_main()
 			/* reset yaw setpoint to current position if needed */
 			if (reset_yaw_sp) {
 				reset_yaw_sp = false;
-				_att_sp.yaw_body = _att.yaw;
+				_att_sp.yaw_body = _yaw;
 			}
 
 			/* do not move yaw while arming */
@@ -1503,7 +1539,7 @@ MulticopterPositionControl::task_main()
 
 				_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
 				float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
-				float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
+				float yaw_offs = _wrap_pi(yaw_target - _yaw);
 
 				// If the yaw offset became too big for the system to track stop
 				// shifting it

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -116,11 +116,11 @@ static inline int max(int val1, int val2)
  */
 static void usage(const char *reason)
 {
-	if (reason) {
-		PX4_INFO("%s\n", reason);
+	if (reason && *reason) {
+		PX4_INFO("%s", reason);
 	}
 
-	PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+	PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n");
 	return;
 }
 

src/modules/px4iofirmware/mixer.cpp

@@ -88,9 +88,6 @@ static int	mixer_callback(uintptr_t handle,
 
 static MixerGroup mixer_group(mixer_callback, 0);
 
-/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
-static void mixer_set_failsafe();
-
 void
 mixer_tick(void)
 {
@@ -479,15 +476,6 @@ mixer_handle_text(const void *buffer, size_t length)
 		/* if anything was parsed */
 		if (resid != mixer_text_length) {
 
-			/* only set mixer ok if no residual is left over */
-			if (resid == 0) {
-				r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
-
-			} else {
-				/* not yet reached the end of the mixer, set as not ok */
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
-			}
-
 			isr_debug(2, "used %u", mixer_text_length - resid);
 
 			/* copy any leftover text to the base of the buffer for re-use */
@@ -496,9 +484,6 @@ mixer_handle_text(const void *buffer, size_t length)
 			}
 
 			mixer_text_length = resid;
-
-			/* update failsafe values */
-			mixer_set_failsafe();
 		}
 
 		break;
@@ -507,7 +492,7 @@ mixer_handle_text(const void *buffer, size_t length)
 	return 0;
 }
 
-static void
+void
 mixer_set_failsafe()
 {
 	/*

src/modules/px4iofirmware/px4io.h

@@ -190,6 +190,8 @@ extern pwm_limit_t pwm_limit;
  */
 extern void	mixer_tick(void);
 extern int	mixer_handle_text(const void *buffer, size_t length);
+/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
+extern void	mixer_set_failsafe(void);
 
 /**
  * Safety switch/LED.

src/modules/px4iofirmware/registers.c

@@ -469,8 +469,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * Allow FMU override of arming state (to allow in-air restores),
 			 * but only if the arming state is not in sync on the IO side.
 			 */
-			if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+
+			if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
+
+			} else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 				r_status_flags = value;
+
+			}
+
+			if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) {
+
+				/* update failsafe values, now that the mixer is set to ok */
+				mixer_set_failsafe();
 			}
 
 			break;

src/modules/sdlog2/sdlog2.c

@@ -73,6 +73,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -1089,6 +1090,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vtol_vehicle_status_s vtol_status;
 		struct time_offset_s time_offset;
 		struct mc_att_ctrl_status_s mc_att_ctrl_status;
+		struct control_state_s ctrl_state;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1137,6 +1139,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ENCD_s log_ENCD;
 			struct log_TSYN_s log_TSYN;
 			struct log_MACS_s log_MACS;
+			struct log_CTS_s log_CTS;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1179,6 +1182,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int encoders_sub;
 		int tsync_sub;
 		int mc_att_ctrl_status_sub;
+		int ctrl_state_sub;
 	} subs;
 
 	subs.cmd_sub = -1;
@@ -1212,6 +1216,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.wind_sub = -1;
 	subs.tsync_sub = -1;
 	subs.mc_att_ctrl_status_sub = -1;
+	subs.ctrl_state_sub = -1;
 	subs.encoders_sub = -1;
 
 	/* add new topics HERE */
@@ -1856,6 +1861,19 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(MACS);
 		}
 
+		/* --- CONTROL STATE --- */
+		if (copy_if_updated(ORB_ID(control_state), &subs.ctrl_state_sub, &buf.ctrl_state)) {
+			log_msg.msg_type = LOG_CTS_MSG;
+			log_msg.body.log_CTS.vx_body = buf.ctrl_state.x_vel;
+			log_msg.body.log_CTS.vy_body = buf.ctrl_state.y_vel;
+			log_msg.body.log_CTS.vz_body = buf.ctrl_state.z_vel;
+			log_msg.body.log_CTS.airspeed = buf.ctrl_state.airspeed;
+			log_msg.body.log_CTS.roll_rate = buf.ctrl_state.roll_rate;
+			log_msg.body.log_CTS.pitch_rate = buf.ctrl_state.pitch_rate;
+			log_msg.body.log_CTS.yaw_rate = buf.ctrl_state.yaw_rate;
+			LOGBUFFER_WRITE_AND_COUNT(CTS);
+		}
+
 		/* signal the other thread new data, but not yet unlock */
 		if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 			/* only request write if several packets can be written at once */
@@ -1898,7 +1916,7 @@ void sdlog2_status()
 }
 
 /**
- * @return 0 if file exists
+ * @return true if file exists
  */
 bool file_exist(const char *filename)
 {

src/modules/sdlog2/sdlog2_messages.h

@@ -477,6 +477,18 @@ struct log_MACS_s {
 	float yaw_rate_integ;
 };
 
+/* --- CONTROL STATE --- */
+#define LOG_CTS_MSG 47
+struct log_CTS_s {
+	float vx_body;
+	float vy_body;
+	float vz_body;
+	float airspeed;
+	float roll_rate;
+	float pitch_rate;
+	float yaw_rate;
+};
+
 /* WARNING: ID 46 is already in use for ATTC1 */
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
@@ -519,6 +531,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBfBBf",		"MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
 	LOG_FORMAT(VTOL, "f",		"Arsp"),
+	LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
 	LOG_FORMAT(RC, "ffffffffBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
 	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),

src/modules/sensors/sensors.cpp

@@ -850,7 +850,7 @@ Sensors::parameters_update()
 		_parameters.battery_voltage_scaling = 0.0082f;
 #elif CONFIG_ARCH_BOARD_AEROCORE
 		_parameters.battery_voltage_scaling = 0.0063f;
-#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+#elif CONFIG_ARCH_BOARD_PX4FMU_V1
 		_parameters.battery_voltage_scaling = 0.00459340659f;
 #else
 		/* ensure a missing default trips a low voltage lockdown */
@@ -1250,7 +1250,7 @@ Sensors::vehicle_control_mode_poll()
 void
 Sensors::parameter_update_poll(bool forced)
 {
-	bool param_updated;
+	bool param_updated = false;
 
 	/* Check if any parameter has changed */
 	orb_check(_params_sub, &param_updated);
@@ -2146,7 +2146,7 @@ Sensors::task_main()
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
 	/* wakeup source(s) */
-	px4_pollfd_struct_t fds[1];
+	px4_pollfd_struct_t fds[1] = {};
 
 	/* use the gyro to pace output */
 	fds[0].fd = _gyro_sub[0];

src/modules/simulator/simulator_mavlink.cpp

@@ -463,10 +463,12 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	char serial_buf[1024];
 
 	struct pollfd fds[2];
+	memset(fds, 0, sizeof(fds));
 	unsigned fd_count = 1;
 	fds[0].fd = _fd;
 	fds[0].events = POLLIN;
 
+
 	if (serial_fd >= 0) {
 		fds[1].fd = serial_fd;
 		fds[1].events = POLLIN;

src/modules/systemlib/mixer/mixer.h

@@ -481,7 +481,7 @@ public:
 	 *				compared to thrust.
 	 * @param yaw_wcale		Scaling factor applied to yaw inputs compared
 	 *				to thrust.
-	 * @param deadband		Minumum rotor control output value; usually
+	 * @param idle_speed		Minumum rotor control output value; usually
 	 *				tuned to ensure that rotors never stall at the
 	 * 				low end of their control range.
 	 */
@@ -491,7 +491,7 @@ public:
 			float roll_scale,
 			float pitch_scale,
 			float yaw_scale,
-			float deadband);
+			float idle_speed);
 	~MultirotorMixer();
 
 	/**

src/modules/systemlib/mixer/multi_tables.mk

@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 Anton Matosov <anton.matosov@gmail.com>. 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.
-#
-############################################################################
-
-
-SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
-MULTI_TABLES := $(SELF_DIR)multi_tables.py
-
-# Add explicit dependency, as implicit one doesn't work often.
-$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h
-
-$(SELF_DIR)mixer_multirotor.generated.h : $(MULTI_TABLES)
-	$(Q) $(PYTHON) $(MULTI_TABLES) > $(SELF_DIR)mixer_multirotor.generated.h

src/modules/systemlib/system_params.c

@@ -40,7 +40,7 @@
 /**
  * Auto-start script index.
  *
- * Defines the auto-start script used to bootstrap the system.
+ * CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system.
  *
  * @group System
  */
@@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
 /**
 * Companion computer interface
 *
-* Configures the baud rate of the companion computer interface.
+* CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the companion computer interface.
 * Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!)
 * 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1.
 * 157600: enables OSD mode at 57600 baud, 8N1.

src/modules/uORB/objects_common.cpp

@@ -69,6 +69,9 @@ ORB_DEFINE(pwm_input, struct pwm_input_s);
 #include "topics/vehicle_attitude.h"
 ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s);
 
+#include "topics/control_state.h"
+ORB_DEFINE(control_state, struct control_state_s);
+
 #include "topics/sensor_combined.h"
 ORB_DEFINE(sensor_combined, struct sensor_combined_s);
 

src/modules/vtol_att_control/CMakeLists.txt

@@ -38,12 +38,9 @@ px4_add_module(
 		
 	SRCS
 		vtol_att_control_main.cpp
-		vtol_att_control_params.c
-		tiltrotor_params.c
 		tiltrotor.cpp
 		vtol_type.cpp
 		tailsitter.cpp
-		standard_params.c
 		standard.cpp
 	DEPENDS
 		platforms__common

src/modules/vtol_att_control/standard.cpp

@@ -60,7 +60,7 @@ Standard::Standard(VtolAttitudeControl *attc) :
 	_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()
 {
@@ -98,12 +98,12 @@ Standard::parameters_update()
 
 void Standard::update_vtol_state()
 {
- 	parameters_update();
+	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. 
+	 * 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 (!_attc->is_fixed_wing_requested()) {
 		// the transition to fw mode switch is off
@@ -130,7 +130,7 @@ void Standard::update_vtol_state()
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
 			// transition to MC mode if transition time has passed
 			if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
-					(_params_standard.back_trans_dur * 1000000.0f)) {
+			    (_params_standard.back_trans_dur * 1000000.0f)) {
 				_vtol_schedule.flight_mode = MC_MODE;
 			}
 		}
@@ -168,17 +168,19 @@ void Standard::update_vtol_state()
 	}
 
 	// 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;
+	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;
 	}
 }
 
@@ -188,18 +190,21 @@ void Standard::update_transition_state()
 		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);
+			_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) {
-			float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
+			float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) /
+				       _airspeed_trans_blend_margin;
 			_mc_roll_weight = weight;
 			_mc_pitch_weight = weight;
 			_mc_yaw_weight = weight;
+
 		} else {
 			// at low speeds give full weight to mc
 			_mc_roll_weight = 1.0f;
@@ -210,17 +215,19 @@ void Standard::update_transition_state()
 	} 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) {
-			float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
+			float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur *
+					1000000.0f);
 			_mc_roll_weight = weight;
 			_mc_pitch_weight = weight;
 			_mc_yaw_weight = weight;
+
 		} else {
 			_mc_roll_weight = 1.0f;
 			_mc_pitch_weight = 1.0f;
 			_mc_yaw_weight = 1.0f;
 		}
 
-		// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again	
+		// 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();
@@ -238,15 +245,15 @@ void Standard::update_mc_state()
 	// do nothing
 }
 
- void Standard::update_fw_state()
+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	
+	// 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()
 {
@@ -259,22 +266,31 @@ void Standard::update_external_state()
 void Standard::fill_actuator_outputs()
 {
 	/* multirotor controls */
-	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;	// roll
-	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;	// pitch
-	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;	// yaw
-	_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];	// throttle
+	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
+			* _mc_roll_weight;	// roll
+	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;	// pitch
+	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
+			_mc_yaw_weight;	// yaw
+	_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];	// throttle
 
 	/* fixed wing controls */
-	_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight);	//roll
-	_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);	//pitch
-	_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight);	// yaw
+	_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
+			* (1 - _mc_roll_weight);	//roll
+	_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
+		(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);	//pitch
+	_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
+			* (1 - _mc_yaw_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[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
+		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
+
 	} else {
-		// otherwise we may be ramping up the throttle during the transition to fw mode 
+		// otherwise we may be ramping up the throttle during the transition to fw mode
 		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
 	}
 }
@@ -288,11 +304,11 @@ Standard::set_max_mc(unsigned pwm_value)
 	int ret;
 	unsigned servo_count;
 	char *dev = PWM_OUTPUT0_DEVICE_PATH;
-	int fd = open(dev, 0);
+	int fd = px4_open(dev, 0);
 
-	if (fd < 0) {err(1, "can't open %s", dev);}
+	if (fd < 0) {PX4_WARN("can't open %s", dev);}
 
-	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	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));
 
@@ -301,9 +317,9 @@ Standard::set_max_mc(unsigned pwm_value)
 		pwm_values.channel_count = _params->vtol_motor_count;
 	}
 
-	ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
+	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
 
-	if (ret != OK) {errx(ret, "failed setting max values");}
+	if (ret != OK) {PX4_WARN("failed setting max values");}
 
-	close(fd);
+	px4_close(fd);
 }

src/modules/vtol_att_control/standard.h

@@ -31,15 +31,15 @@
  *
  ****************************************************************************/
 
- /**
- * @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>
- *
- */
+/**
+* @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
@@ -52,7 +52,7 @@ class Standard : public VtolType
 
 public:
 
-	Standard(VtolAttitudeControl * _att_controller);
+	Standard(VtolAttitudeControl *_att_controller);
 	~Standard();
 
 	void update_vtol_state();
@@ -89,7 +89,7 @@ private:
 	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;
+	} _vtol_schedule;
 
 	bool _flag_enable_mc_motors;
 	float _pusher_throttle;

src/modules/vtol_att_control/standard_params.c

@@ -36,11 +36,9 @@
  * Parameters for the standard VTOL controller.
  *
  * @author Simon Wilks	<simon@uaventure.com>
- * @author Roman Bapst	<bapstroman@gmail.com>
+ * @author Roman Bapst	<roman@px4.io>
  */
 
-#include <systemlib/param/param.h>
-
 /**
  * Target throttle value for pusher/puller motor during the transition to fw mode
  *

src/modules/vtol_att_control/tailsitter.cpp

@@ -31,21 +31,21 @@
  *
  ****************************************************************************/
 
- /**
- * @file tailsitter.cpp
- *
- * @author Roman Bapst 		<bapstroman@gmail.com>
- *
- */
+/**
+* @file tailsitter.cpp
+*
+* @author Roman Bapst 		<bapstroman@gmail.com>
+*
+*/
 
- #include "tailsitter.h"
- #include "vtol_att_control_main.h"
+#include "tailsitter.h"
+#include "vtol_att_control_main.h"
 
-Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) :
-VtolType(att_controller),
-_airspeed_tot(0),
-_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
-_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
+Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) :
+	VtolType(att_controller),
+	_airspeed_tot(0),
+	_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
 {
 
 }
@@ -60,6 +60,7 @@ void Tailsitter::update_vtol_state()
 	// simply switch between the two modes
 	if (!_attc->is_fixed_wing_requested()) {
 		_vtol_mode = ROTARY_WING;
+
 	} else {
 		_vtol_mode = FIXED_WING;
 	}
@@ -67,7 +68,7 @@ void Tailsitter::update_vtol_state()
 
 void Tailsitter::update_mc_state()
 {
- 	if (!flag_idle_mc) {
+	if (!flag_idle_mc) {
 		set_idle_mc();
 		flag_idle_mc = true;
 	}
@@ -91,18 +92,18 @@ void Tailsitter::update_external_state()
 
 }
 
- void Tailsitter::calc_tot_airspeed()
- {
+void Tailsitter::calc_tot_airspeed()
+{
 	float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s);	// prevent numerical drama
 	// calculate momentary power of one engine
 	float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count;
-	P = math::constrain(P,1.0f,_params->power_max);
+	P = math::constrain(P, 1.0f, _params->power_max);
 	// calculate prop efficiency
-	float power_factor = 1.0f - P*_params->prop_eff/_params->power_max;
-	float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f;
-	eta = math::constrain(eta,0.001f,1.0f);	// live on the safe side
+	float power_factor = 1.0f - P * _params->prop_eff / _params->power_max;
+	float eta = (1.0f / (1 + expf(-0.4f * power_factor * airspeed)) - 0.5f) * 2.0f;
+	eta = math::constrain(eta, 0.001f, 1.0f);	// live on the safe side
 	// calculate induced airspeed by propeller
-	float v_ind = (airspeed/eta - airspeed)*2.0f;
+	float v_ind = (airspeed / eta - airspeed) * 2.0f;
 	// calculate total airspeed
 	float airspeed_raw = airspeed + v_ind;
 	// apply low-pass filter
@@ -115,16 +116,19 @@ Tailsitter::scale_mc_output()
 	// scale around tuning airspeed
 	float airspeed;
 	calc_tot_airspeed();	// estimate air velocity seen by elevons
+
 	// if airspeed is not updating, we assume the normal average speed
-	if (bool nonfinite = !isfinite(_airspeed->true_airspeed_m_s) ||
-	    hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
+	if (bool nonfinite = !PX4_ISFINITE(_airspeed->true_airspeed_m_s) ||
+			     hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
 		airspeed = _params->mc_airspeed_trim;
+
 		if (nonfinite) {
 			perf_count(_nonfinite_input_perf);
 		}
+
 	} else {
 		airspeed = _airspeed_tot;
-		airspeed = math::constrain(airspeed,_params->mc_airspeed_min, _params->mc_airspeed_max);
+		airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max);
 	}
 
 	_vtol_vehicle_status->airspeed_tot = airspeed;	// save value for logging
@@ -135,8 +139,10 @@ Tailsitter::scale_mc_output()
 	 *
 	 * Forcing the scaling to this value allows reasonable handheld tests.
 	 */
-	float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : airspeed);
-	_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
+	float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min :
+				 airspeed);
+	_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling,
+				       -1.0f, 1.0f);
 }
 
 /**
@@ -144,37 +150,50 @@ Tailsitter::scale_mc_output()
 */
 void Tailsitter::fill_actuator_outputs()
 {
-	switch(_vtol_mode) {
-		case ROTARY_WING:
-			_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
-			_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
-			_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
-			_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
+	switch (_vtol_mode) {
+	case ROTARY_WING:
+		_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
+		_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
+			_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
+		_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
+		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
 
-			if (_params->elevons_mc_lock == 1) {
-				_actuators_out_1->control[0] = 0;
-				_actuators_out_1->control[1] = 0;
-			} else {
-				// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
-				_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];	//roll elevon
-				_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];	//pitch elevon
-			}
-			break;
-		case FIXED_WING:
-			// in fixed wing mode we use engines only for providing thrust, no moments are generated
-			_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
-			_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
-			_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
-			_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
+		if (_params->elevons_mc_lock == 1) {
+			_actuators_out_1->control[0] = 0;
+			_actuators_out_1->control[1] = 0;
 
-			_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];	// roll elevon
-			_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim;	// pitch elevon
-			_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW];	// yaw
-			_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];	// throttle
-			break;
-		case TRANSITION:
-		case EXTERNAL:
-			// not yet implemented, we are switching brute force at the moment
-			break;
+		} else {
+			// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
+			_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
+				_actuators_mc_in->control[actuator_controls_s::INDEX_YAW];	//roll elevon
+			_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
+				_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];	//pitch elevon
+		}
+
+		break;
+
+	case FIXED_WING:
+		// in fixed wing mode we use engines only for providing thrust, no moments are generated
+		_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
+		_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
+		_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
+		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
+
+		_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
+			-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];	// roll elevon
+		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim;	// pitch elevon
+		_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_YAW];	// yaw
+		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];	// throttle
+		break;
+
+	case TRANSITION:
+	case EXTERNAL:
+		// not yet implemented, we are switching brute force at the moment
+		break;
 	}
 }

src/modules/vtol_att_control/tailsitter.h

@@ -31,12 +31,12 @@
  *
  ****************************************************************************/
 
- /**
- * @file tiltrotor.h
- *
- * @author Roman Bapst 		<bapstroman@gmail.com>
- *
- */
+/**
+* @file tiltrotor.h
+*
+* @author Roman Bapst 		<bapstroman@gmail.com>
+*
+*/
 
 #ifndef TAILSITTER_H
 #define TAILSITTER_H
@@ -48,7 +48,7 @@ class Tailsitter : public VtolType
 {
 
 public:
-	Tailsitter(VtolAttitudeControl * _att_controller);
+	Tailsitter(VtolAttitudeControl *_att_controller);
 	~Tailsitter();
 
 	void update_vtol_state();

src/modules/vtol_att_control/tiltrotor.cpp

@@ -44,12 +44,10 @@
 #define ARSP_YAW_CTRL_DISABLE 7.0f	// airspeed at which we stop controlling yaw during a front transition
 
 Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
-VtolType(attc),
-_rear_motors(ENABLED),
-_tilt_control(0.0f),
-_roll_weight_mc(1.0f),
-_yaw_weight_mc(1.0f),
-_min_front_trans_dur(0.5f)
+	VtolType(attc),
+	_rear_motors(ENABLED),
+	_tilt_control(0.0f),
+	_min_front_trans_dur(0.5f)
 {
 	_vtol_schedule.flight_mode = MC_MODE;
 	_vtol_schedule.transition_start = 0;
@@ -88,11 +86,11 @@ Tiltrotor::parameters_update()
 
 	/* vtol duration of a front transition */
 	param_get(_params_handles_tiltrotor.front_trans_dur, &v);
-	_params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f);
+	_params_tiltrotor.front_trans_dur = math::constrain(v, 1.0f, 5.0f);
 
 	/* vtol duration of a back transition */
 	param_get(_params_handles_tiltrotor.back_trans_dur, &v);
-	_params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f);
+	_params_tiltrotor.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
 
 	/* vtol tilt mechanism position in mc mode */
 	param_get(_params_handles_tiltrotor.tilt_mc, &v);
@@ -125,22 +123,26 @@ Tiltrotor::parameters_update()
 	/* avoid parameters which will lead to zero division in the transition code */
 	_params_tiltrotor.front_trans_dur = math::max(_params_tiltrotor.front_trans_dur, _min_front_trans_dur);
 
-	if ( _params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f ) {
+	if (_params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f) {
 		_params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f;
 	}
 
 	return OK;
 }
 
-int Tiltrotor::get_motor_off_channels(int channels) {
+int Tiltrotor::get_motor_off_channels(int channels)
+{
 	int channel_bitmap = 0;
-	
+
 	int channel;
+
 	for (int i = 0; i < _params->vtol_motor_count; ++i) {
 		channel = channels % 10;
+
 		if (channel == 0) {
 			break;
 		}
+
 		channel_bitmap |= 1 << channel;
 		channels = channels / 10;
 	}
@@ -150,82 +152,98 @@ int Tiltrotor::get_motor_off_channels(int channels) {
 
 void Tiltrotor::update_vtol_state()
 {
- 	parameters_update();
+	parameters_update();
 
- 	/* simple logic using a two way switch to perform transitions.
+	/* simple logic using a two way switch to perform transitions.
 	 * after flipping the switch the vehicle will start tilting rotors, picking up
 	 * forward speed. After the vehicle has picked up enough speed the rotors are tilted
 	 * forward completely. For the backtransition the motors simply rotate back.
- 	*/
+	*/
 
 	if (!_attc->is_fixed_wing_requested()) {
 
 		// plane is in multicopter mode
-		switch(_vtol_schedule.flight_mode) {
-			case MC_MODE:
-				break;
-			case FW_MODE:
-				_vtol_schedule.flight_mode 	= TRANSITION_BACK;
-				_vtol_schedule.transition_start = hrt_absolute_time();
-				break;
-			case TRANSITION_FRONT_P1:
-				// failsafe into multicopter mode
+		switch (_vtol_schedule.flight_mode) {
+		case MC_MODE:
+			break;
+
+		case FW_MODE:
+			_vtol_schedule.flight_mode 	= TRANSITION_BACK;
+			_vtol_schedule.transition_start = hrt_absolute_time();
+			break;
+
+		case TRANSITION_FRONT_P1:
+			// failsafe into multicopter mode
+			_vtol_schedule.flight_mode = MC_MODE;
+			break;
+
+		case TRANSITION_FRONT_P2:
+			// failsafe into multicopter mode
+			_vtol_schedule.flight_mode = MC_MODE;
+			break;
+
+		case TRANSITION_BACK:
+			if (_tilt_control <= _params_tiltrotor.tilt_mc) {
 				_vtol_schedule.flight_mode = MC_MODE;
-				break;
-			case TRANSITION_FRONT_P2:
-				// failsafe into multicopter mode
-				_vtol_schedule.flight_mode = MC_MODE;
-				break;
-			case TRANSITION_BACK:
-				if (_tilt_control <= _params_tiltrotor.tilt_mc) {
-					_vtol_schedule.flight_mode = MC_MODE;
-				}
-				break;
+			}
+
+			break;
 		}
+
 	} else {
 
-		switch(_vtol_schedule.flight_mode) {
-			case MC_MODE:
-				// initialise a front transition
-				_vtol_schedule.flight_mode 	= TRANSITION_FRONT_P1;
+		switch (_vtol_schedule.flight_mode) {
+		case MC_MODE:
+			// initialise a front transition
+			_vtol_schedule.flight_mode 	= TRANSITION_FRONT_P1;
+			_vtol_schedule.transition_start = hrt_absolute_time();
+			break;
+
+		case FW_MODE:
+			break;
+
+		case TRANSITION_FRONT_P1:
+
+			// check if we have reached airspeed to switch to fw mode
+			if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
+				_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
 				_vtol_schedule.transition_start = hrt_absolute_time();
-				break;
-			case FW_MODE:
-				break;
-			case TRANSITION_FRONT_P1:
-				// check if we have reached airspeed to switch to fw mode
-				if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
-					_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
-					_vtol_schedule.transition_start = hrt_absolute_time();
-				}
-				break;
-			case TRANSITION_FRONT_P2:
-				// if the rotors have been tilted completely we switch to fw mode
-				if (_tilt_control >= _params_tiltrotor.tilt_fw) {
-					_vtol_schedule.flight_mode = FW_MODE;
-					_tilt_control = _params_tiltrotor.tilt_fw;
-				}
-				break;
-			case TRANSITION_BACK:
-				// failsafe into fixed wing mode
+			}
+
+			break;
+
+		case TRANSITION_FRONT_P2:
+
+			// if the rotors have been tilted completely we switch to fw mode
+			if (_tilt_control >= _params_tiltrotor.tilt_fw) {
 				_vtol_schedule.flight_mode = FW_MODE;
-				break;
+				_tilt_control = _params_tiltrotor.tilt_fw;
+			}
+
+			break;
+
+		case TRANSITION_BACK:
+			// failsafe into fixed wing mode
+			_vtol_schedule.flight_mode = FW_MODE;
+			break;
 		}
 	}
 
 	// map tiltrotor 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_FRONT_P1:
-		case TRANSITION_FRONT_P2:
-		case TRANSITION_BACK:
-			_vtol_mode = TRANSITION;
-			break;
+	switch (_vtol_schedule.flight_mode) {
+	case MC_MODE:
+		_vtol_mode = ROTARY_WING;
+		break;
+
+	case FW_MODE:
+		_vtol_mode = FIXED_WING;
+		break;
+
+	case TRANSITION_FRONT_P1:
+	case TRANSITION_FRONT_P2:
+	case TRANSITION_BACK:
+		_vtol_mode = TRANSITION;
+		break;
 	}
 }
 
@@ -250,7 +268,7 @@ void Tiltrotor::update_mc_state()
 	_mc_yaw_weight = 1.0f;
 }
 
- void Tiltrotor::update_fw_state()
+void Tiltrotor::update_fw_state()
 {
 	// make sure motors are tilted forward
 	_tilt_control = _params_tiltrotor.tilt_fw;
@@ -278,15 +296,18 @@ void Tiltrotor::update_transition_state()
 		if (_rear_motors != ENABLED) {
 			set_rear_motor_state(ENABLED);
 		}
+
 		// tilt rotors forward up to certain angle
-		if (_tilt_control <= _params_tiltrotor.tilt_transition ) {
+		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);
+					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 >= _params_tiltrotor.airspeed_blend_start) {
 			_mc_roll_weight = 0.0f;
+
 		} else {
 			// at low speeds give full weight to mc
 			_mc_roll_weight = 1.0f;
@@ -294,6 +315,7 @@ void Tiltrotor::update_transition_state()
 
 		// disable mc yaw control once the plane has picked up speed
 		_mc_yaw_weight = 1.0f;
+
 		if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
 			_mc_yaw_weight = 0.0f;
 		}
@@ -301,8 +323,10 @@ void Tiltrotor::update_transition_state()
 	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
 		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
 		_tilt_control = _params_tiltrotor.tilt_transition +
-			fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
+				fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(
+					&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);
 		_mc_roll_weight = 0.0f;
+
 	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
 		if (_rear_motors != IDLE) {
 			set_rear_motor_state(IDLE);
@@ -312,10 +336,12 @@ void Tiltrotor::update_transition_state()
 			set_idle_mc();
 			flag_idle_mc = true;
 		}
+
 		// tilt rotors back
 		if (_tilt_control > _params_tiltrotor.tilt_mc) {
 			_tilt_control = _params_tiltrotor.tilt_fw -
-				fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur * 1000000.0f);
+					fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(
+						&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);
 		}
 
 		// set zero throttle for backtransition otherwise unwanted moments will be created
@@ -339,19 +365,28 @@ void Tiltrotor::update_external_state()
 */
 void Tiltrotor::fill_actuator_outputs()
 {
-	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
-	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
-	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
+			* _mc_roll_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
+	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
+			_mc_yaw_weight;
 
 	if (_vtol_schedule.flight_mode == FW_MODE) {
-		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
+		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
+
 	} else {
-		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
+		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];;
 	}
 
-	_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight);
-	_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
-	_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight);	// yaw
+	_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
+			* (1 - _mc_roll_weight);
+	_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
+		(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);
+	_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
+			* (1 - _mc_yaw_weight);	// yaw
 	_actuators_out_1->control[4] = _tilt_control;
 }
 
@@ -360,52 +395,58 @@ void Tiltrotor::fill_actuator_outputs()
 * Set state of rear motors.
 */
 
-void Tiltrotor::set_rear_motor_state(rear_motor_state state) {
+void Tiltrotor::set_rear_motor_state(rear_motor_state state)
+{
 	int pwm_value = PWM_DEFAULT_MAX;
 
 	// map desired rear rotor state to max allowed pwm signal
 	switch (state) {
-		case ENABLED:
-			pwm_value = PWM_DEFAULT_MAX;
-			_rear_motors = ENABLED;
-			break;
-		case DISABLED:
-			pwm_value = PWM_LOWEST_MAX;
-			_rear_motors = DISABLED;
-			break;
-		case IDLE:
-			pwm_value = _params->idle_pwm_mc;
-			_rear_motors = IDLE;
-			break;
+	case ENABLED:
+		pwm_value = PWM_DEFAULT_MAX;
+		_rear_motors = ENABLED;
+		break;
+
+	case DISABLED:
+		pwm_value = PWM_LOWEST_MAX;
+		_rear_motors = DISABLED;
+		break;
+
+	case IDLE:
+		pwm_value = _params->idle_pwm_mc;
+		_rear_motors = IDLE;
+		break;
 	}
 
 	int ret;
 	unsigned servo_count;
 	char *dev = PWM_OUTPUT0_DEVICE_PATH;
-	int fd = open(dev, 0);
+	int fd = px4_open(dev, 0);
 
-	if (fd < 0) {err(1, "can't open %s", dev);}
+	if (fd < 0) {PX4_WARN("can't open %s", dev);}
 
-	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	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++) {
 		if (is_motor_off_channel(i)) {
 			pwm_values.values[i] = pwm_value;
+
 		} else {
 			pwm_values.values[i] = PWM_DEFAULT_MAX;
 		}
+
 		pwm_values.channel_count = _params->vtol_motor_count;
 	}
 
-	ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
+	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
 
-	if (ret != OK) {errx(ret, "failed setting max values");}
+	if (ret != OK) {PX4_WARN("failed setting max values");}
 
-	close(fd);
+	px4_close(fd);
 }
 
-bool Tiltrotor::is_motor_off_channel(const int channel) {
+bool Tiltrotor::is_motor_off_channel(const int channel)
+{
 	return (_params_tiltrotor.fw_motors_off >> channel) & 1;
 }

src/modules/vtol_att_control/tiltrotor.h

@@ -31,12 +31,12 @@
  *
  ****************************************************************************/
 
- /**
- * @file tiltrotor.h
- *
- * @author Roman Bapst 		<bapstroman@gmail.com>
- *
- */
+/**
+* @file tiltrotor.h
+*
+* @author Roman Bapst 		<bapstroman@gmail.com>
+*
+*/
 
 #ifndef TILTROTOR_H
 #define TILTROTOR_H
@@ -49,7 +49,7 @@ class Tiltrotor : public VtolType
 
 public:
 
-	Tiltrotor(VtolAttitudeControl * _att_controller);
+	Tiltrotor(VtolAttitudeControl *_att_controller);
 	~Tiltrotor();
 
 	/**
@@ -127,11 +127,9 @@ private:
 	struct {
 		vtol_mode flight_mode;			/**< vtol flight mode, defined by enum vtol_mode */
 		hrt_abstime transition_start;	/**< absoulte time at which front transition started */
-	}_vtol_schedule;
+	} _vtol_schedule;
 
 	float _tilt_control;		/**< actuator value for the tilt servo */
-	float _roll_weight_mc;		/**< multicopter desired roll moment weight */
-	float _yaw_weight_mc;		/**< multicopter desired yaw moment weight */
 
 	const float _min_front_trans_dur;	/**< min possible time in which rotors are rotated into the first position */
 

src/modules/vtol_att_control/tiltrotor_params.c

@@ -35,7 +35,7 @@
  * @file tiltrotor_params.c
  * Parameters for vtol attitude controller.
  *
- * @author Roman Bapst <bapstroman@gmail.com>
+ * @author Roman Bapst <roman@px4.io>
  */
 
 #include <systemlib/param/param.h>

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -93,10 +93,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
 	memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
 	memset(&_armed, 0, sizeof(_armed));
-	memset(&_local_pos,0,sizeof(_local_pos));
-	memset(&_airspeed,0,sizeof(_airspeed));
-	memset(&_batt_status,0,sizeof(_batt_status));
-	memset(&_vehicle_cmd,0, sizeof(_vehicle_cmd));
+	memset(&_local_pos, 0, sizeof(_local_pos));
+	memset(&_airspeed, 0, sizeof(_airspeed));
+	memset(&_batt_status, 0, sizeof(_batt_status));
+	memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd));
 
 	_params.idle_pwm_mc = PWM_LOWEST_MIN;
 	_params.vtol_motor_count = 0;
@@ -121,12 +121,15 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	if (_params.vtol_type == 0) {
 		_tailsitter = new Tailsitter(this);
 		_vtol_type = _tailsitter;
+
 	} 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;
 	}
@@ -150,7 +153,7 @@ VtolAttitudeControl::~VtolAttitudeControl()
 
 			/* if we have given up, kill it */
 			if (++i > 50) {
-				task_delete(_control_task);
+				px4_task_delete(_control_task);
 				break;
 			}
 		} while (_control_task != -1);
@@ -258,7 +261,8 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll()
 * Check for airspeed updates.
 */
 void
-VtolAttitudeControl::vehicle_airspeed_poll() {
+VtolAttitudeControl::vehicle_airspeed_poll()
+{
 	bool updated;
 	orb_check(_airspeed_sub, &updated);
 
@@ -271,7 +275,8 @@ VtolAttitudeControl::vehicle_airspeed_poll() {
 * Check for battery updates.
 */
 void
-VtolAttitudeControl::vehicle_battery_poll() {
+VtolAttitudeControl::vehicle_battery_poll()
+{
 	bool updated;
 	orb_check(_battery_status_sub, &updated);
 
@@ -442,7 +447,7 @@ VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
 
 void VtolAttitudeControl::task_main()
 {
-	warnx("started");
+	PX4_WARN("started");
 	fflush(stdout);
 
 	/* do subscriptions */
@@ -471,7 +476,7 @@ void VtolAttitudeControl::task_main()
 	_vtol_type->set_idle_mc();
 
 	/* wakeup source*/
-	struct pollfd fds[3];	/*input_mc, input_fw, parameters*/
+	px4_pollfd_struct_t fds[3];	/*input_mc, input_fw, parameters*/
 
 	fds[0].fd     = _actuator_inputs_mc;
 	fds[0].events = POLLIN;
@@ -491,7 +496,7 @@ void VtolAttitudeControl::task_main()
 		}
 
 		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 
 		/* timed out - periodic check for _task_should_exit */
@@ -633,7 +638,7 @@ void VtolAttitudeControl::task_main()
 
 	warnx("exit");
 	_control_task = -1;
-	_exit(0);
+	return;
 }
 
 int
@@ -643,14 +648,14 @@ VtolAttitudeControl::start()
 
 	/* start the task */
 	_control_task = px4_task_spawn_cmd("vtol_att_control",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 10,
-				       2048,
-				       (main_t)&VtolAttitudeControl::task_main_trampoline,
-				       nullptr);
+					   SCHED_DEFAULT,
+					   SCHED_PRIORITY_MAX - 10,
+					   2048,
+					   (px4_main_t)&VtolAttitudeControl::task_main_trampoline,
+					   nullptr);
 
 	if (_control_task < 0) {
-		warn("task start failed");
+		PX4_WARN("task start failed");
 		return -errno;
 	}
 
@@ -661,49 +666,54 @@ VtolAttitudeControl::start()
 int vtol_att_control_main(int argc, char *argv[])
 {
 	if (argc < 2) {
-		errx(1, "usage: vtol_att_control {start|stop|status}");
+		PX4_WARN("usage: vtol_att_control {start|stop|status}");
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
 		if (VTOL_att_control::g_control != nullptr) {
-			errx(1, "already running");
+			PX4_WARN("already running");
+			return 0;
 		}
 
 		VTOL_att_control::g_control = new VtolAttitudeControl;
 
 		if (VTOL_att_control::g_control == nullptr) {
-			errx(1, "alloc failed");
+			PX4_WARN("alloc failed");
+			return 1;
 		}
 
 		if (OK != VTOL_att_control::g_control->start()) {
 			delete VTOL_att_control::g_control;
 			VTOL_att_control::g_control = nullptr;
-			err(1, "start failed");
+			PX4_WARN("start failed");
+			return 1;
 		}
 
-		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 		if (VTOL_att_control::g_control == nullptr) {
-			errx(1, "not running");
+			PX4_WARN("not running");
+			return 0;
 		}
 
 		delete VTOL_att_control::g_control;
 		VTOL_att_control::g_control = nullptr;
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (VTOL_att_control::g_control) {
-			errx(0, "running");
+			PX4_WARN("running");
 
 		} else {
-			errx(1, "not running");
+			PX4_WARN("not running");
 		}
+
+		return 0;
 	}
 
-	warnx("unrecognized command");
+	PX4_WARN("unrecognized command");
 	return 1;
 }

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -46,6 +46,10 @@
 #ifndef VTOL_ATT_CONTROL_MAIN_H
 #define VTOL_ATT_CONTROL_MAIN_H
 
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -80,7 +84,6 @@
 #include <systemlib/circuit_breaker.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
-#include <nuttx/fs/ioctl.h>
 #include <fcntl.h>
 
 #include "tiltrotor.h"
@@ -101,24 +104,24 @@ public:
 	int start();	/* start the task and return OK on success */
 	bool is_fixed_wing_requested();
 
-	struct vehicle_attitude_s* 			get_att () {return &_v_att;}
-	struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;}
-	struct vehicle_rates_setpoint_s* 	get_rates_sp () {return &_v_rates_sp;}
-	struct vehicle_rates_setpoint_s* 	get_mc_virtual_rates_sp () {return &_mc_virtual_v_rates_sp;}
-	struct vehicle_rates_setpoint_s* 	get_fw_virtual_rates_sp () {return &_fw_virtual_v_rates_sp;}
-	struct manual_control_setpoint_s* 	get_manual_control_sp () {return &_manual_control_sp;}
-	struct vehicle_control_mode_s* 		get_control_mode () {return &_v_control_mode;}
-	struct vtol_vehicle_status_s*		get_vehicle_status () {return &_vtol_vehicle_status;}
-	struct actuator_controls_s* 		get_actuators_out0 () {return &_actuators_out_0;}
-	struct actuator_controls_s* 		get_actuators_out1 () {return &_actuators_out_1;}
-	struct actuator_controls_s* 		get_actuators_mc_in () {return &_actuators_mc_in;}
-	struct actuator_controls_s* 		get_actuators_fw_in () {return &_actuators_fw_in;}
-	struct actuator_armed_s* 			get_armed () {return &_armed;}
-	struct vehicle_local_position_s* 	get_local_pos () {return &_local_pos;}
-	struct airspeed_s* 					get_airspeed () {return &_airspeed;}
-	struct battery_status_s* 			get_batt_status () {return &_batt_status;}
+	struct vehicle_attitude_s 			*get_att() {return &_v_att;}
+	struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
+	struct vehicle_rates_setpoint_s 	*get_rates_sp() {return &_v_rates_sp;}
+	struct vehicle_rates_setpoint_s 	*get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
+	struct vehicle_rates_setpoint_s 	*get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
+	struct manual_control_setpoint_s 	*get_manual_control_sp() {return &_manual_control_sp;}
+	struct vehicle_control_mode_s 		*get_control_mode() {return &_v_control_mode;}
+	struct vtol_vehicle_status_s		*get_vehicle_status() {return &_vtol_vehicle_status;}
+	struct actuator_controls_s 		*get_actuators_out0() {return &_actuators_out_0;}
+	struct actuator_controls_s 		*get_actuators_out1() {return &_actuators_out_1;}
+	struct actuator_controls_s 		*get_actuators_mc_in() {return &_actuators_mc_in;}
+	struct actuator_controls_s 		*get_actuators_fw_in() {return &_actuators_fw_in;}
+	struct actuator_armed_s 			*get_armed() {return &_armed;}
+	struct vehicle_local_position_s 	*get_local_pos() {return &_local_pos;}
+	struct airspeed_s 					*get_airspeed() {return &_airspeed;}
+	struct battery_status_s 			*get_batt_status() {return &_batt_status;}
 
-	struct Params* 						get_params () {return &_params;}
+	struct Params 						*get_params() {return &_params;}
 
 
 private:
@@ -184,17 +187,12 @@ private:
 		param_t elevons_mc_lock;
 	} _params_handles;
 
-	/* for multicopters it is usual to have a non-zero idle speed of the engines
-	 * for fixed wings we want to have an idle speed of zero since we do not want
-	 * to waste energy when gliding. */
-	unsigned _motor_count;	// number of motors
-	float _airspeed_tot;
 	int _transition_command;
 
-	VtolType * _vtol_type;	// base class for different vtol types
-	Tiltrotor * _tiltrotor;	// tailsitter vtol type
-	Tailsitter * _tailsitter;	// tiltrotor vtol type
-	Standard * _standard;	// standard vtol type
+	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

@@ -35,11 +35,9 @@
  * @file vtol_att_control_params.c
  * Parameters for vtol attitude controller.
  *
- * @author Roman Bapst <bapstr@ethz.ch>
+ * @author Roman Bapst <roman@px4.io>
  */
 
-#include <systemlib/param/param.h>
-
 /**
  * VTOL number of engines
  *

src/modules/vtol_att_control/vtol_type.cpp

@@ -31,21 +31,21 @@
  *
  ****************************************************************************/
 
- /**
- * @file airframe.cpp
- *
- * @author Roman Bapst 		<bapstroman@gmail.com>
- *
- */
+/**
+* @file airframe.cpp
+*
+* @author Roman Bapst 		<bapstroman@gmail.com>
+*
+*/
 
 #include "vtol_type.h"
 #include "drivers/drv_pwm_output.h"
-#include <nuttx/fs/ioctl.h>
+#include <px4_defines.h>
 #include "vtol_att_control_main.h"
 
 VtolType::VtolType(VtolAttitudeControl *att_controller) :
-_attc(att_controller),
-_vtol_mode(ROTARY_WING)
+	_attc(att_controller),
+	_vtol_mode(ROTARY_WING)
 {
 	_v_att = _attc->get_att();
 	_v_att_sp = _attc->get_att_sp();
@@ -70,7 +70,7 @@ _vtol_mode(ROTARY_WING)
 
 VtolType::~VtolType()
 {
-	
+
 }
 
 /**
@@ -81,11 +81,11 @@ void VtolType::set_idle_mc()
 	int ret;
 	unsigned servo_count;
 	char *dev = PWM_OUTPUT0_DEVICE_PATH;
-	int fd = open(dev, 0);
+	int fd = px4_open(dev, 0);
 
-	if (fd < 0) {err(1, "can't open %s", dev);}
+	if (fd < 0) {PX4_WARN("can't open %s", dev);}
 
-	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	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));
@@ -95,11 +95,11 @@ void VtolType::set_idle_mc()
 		pwm_values.channel_count++;
 	}
 
-	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
 
-	if (ret != OK) {errx(ret, "failed setting min values");}
+	if (ret != OK) {PX4_WARN("failed setting min values");}
 
-	close(fd);
+	px4_close(fd);
 
 	flag_idle_mc = true;
 }
@@ -111,9 +111,9 @@ void VtolType::set_idle_fw()
 {
 	int ret;
 	char *dev = PWM_OUTPUT0_DEVICE_PATH;
-	int fd = open(dev, 0);
+	int fd = px4_open(dev, 0);
 
-	if (fd < 0) {err(1, "can't open %s", dev);}
+	if (fd < 0) {PX4_WARN("can't open %s", dev);}
 
 	unsigned pwm_value = PWM_LOWEST_MIN;
 	struct pwm_output_values pwm_values;
@@ -125,9 +125,9 @@ void VtolType::set_idle_fw()
 		pwm_values.channel_count++;
 	}
 
-	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
 
-	if (ret != OK) {errx(ret, "failed setting min values");}
+	if (ret != OK) {PX4_WARN("failed setting min values");}
 
-	close(fd);
+	px4_close(fd);
 }

src/modules/vtol_att_control/vtol_type.h

@@ -31,12 +31,12 @@
  *
  ****************************************************************************/
 
- /**
- * @file airframe.h
- *
- * @author Roman Bapst 		<bapstroman@gmail.com>
- *
- */
+/**
+* @file airframe.h
+*
+* @author Roman Bapst 		<bapstroman@gmail.com>
+*
+*/
 
 #ifndef VTOL_TYPE_H
 #define VTOL_TYPE_H
@@ -85,7 +85,7 @@ public:
 	void set_idle_mc();
 	void set_idle_fw();
 
-	mode get_mode () {return _vtol_mode;};
+	mode get_mode() {return _vtol_mode;};
 
 protected:
 	VtolAttitudeControl *_attc;