Adapted for shared library use with ROS

1 files changed, 214 insertions, 205 deletions

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index c60d8d348..5cff390e2 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -83,8 +83,7 @@
  */
 extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
 
-class FixedwingAttitudeControl
-{
+class FixedwingAttitudeControl {
 public:
 	/**
 	 * Constructor
@@ -101,54 +100,56 @@ public:
 	 *
 	 * @return	OK on success.
 	 */
-	int		start();
+	int start();
 
 	/**
 	 * Task status
 	 *
 	 * @return	true if the mainloop is running
 	 */
-	bool		task_running() { return _task_running; }
+	bool task_running() {
+		return _task_running;
+	}
 
 private:
 
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
-	bool		_task_running;			/**< if true, task is running in its mainloop */
-	int		_control_task;			/**< task handle for sensor task */
-
-	int		_att_sub;			/**< vehicle attitude 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 */
-	int		_global_pos_sub;		/**< global position subscription */
-	int		_vehicle_status_sub;		/**< vehicle status subscription */
-
-	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
-	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
-	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
-	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
-
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct accel_report				_accel;			/**< body frame accelerations */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude 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 */
-	struct vehicle_global_position_s		_global_pos;		/**< global position */
-	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
-	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
-
-	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
-	bool		_debug;				/**< if set to true, print debug output */
+	bool _task_should_exit; /**< if true, sensor task should exit */
+	bool _task_running; /**< if true, task is running in its mainloop */
+	int _control_task; /**< task handle for sensor task */
+
+	int _att_sub; /**< vehicle attitude 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 */
+	int _global_pos_sub; /**< global position subscription */
+	int _vehicle_status_sub; /**< vehicle status subscription */
+
+	orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
+	orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
+	orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */
+	orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */
+
+	struct vehicle_attitude_s _att; /**< vehicle attitude */
+	struct accel_report _accel; /**< body frame accelerations */
+	struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude 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 */
+	struct vehicle_global_position_s _global_pos; /**< global position */
+	struct vehicle_status_s _vehicle_status; /**< vehicle status */
+
+	perf_counter_t _loop_perf; /**< loop performance counter */
+	perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
+	perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
+
+	bool _setpoint_valid; /**< flag if the position control setpoint is valid */
+	bool _debug; /**< if set to true, print debug output */
 
 	struct {
 		float tconst;
@@ -182,14 +183,14 @@ private:
 		float trim_roll;
 		float trim_pitch;
 		float trim_yaw;
-		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
-		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
-		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
-		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
-		float man_roll_max;						/**< Max Roll in rad */
-		float man_pitch_max;					/**< Max Pitch in rad */
+		float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */
+		float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
+		float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
+		float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */
+		float man_roll_max; /**< Max Roll in rad */
+		float man_pitch_max; /**< Max Pitch in rad */
 
-	}		_parameters;			/**< local copies of interesting parameters */
+	} _parameters; /**< local copies of interesting parameters */
 
 	struct {
 
@@ -228,75 +229,71 @@ private:
 		param_t pitchsp_offset_deg;
 		param_t man_roll_max;
 		param_t man_pitch_max;
-	}		_parameter_handles;		/**< handles for interesting parameters */
-
-
-	ECL_RollController				_roll_ctrl;
-	ECL_PitchController				_pitch_ctrl;
-	ECL_YawController				_yaw_ctrl;
+	} _parameter_handles; /**< handles for interesting parameters */
 
+	ECL_RollController _roll_ctrl;
+	ECL_PitchController _pitch_ctrl;
+	ECL_YawController _yaw_ctrl;
 
 	/**
 	 * Update our local parameter cache.
 	 */
-	int		parameters_update();
+	int parameters_update();
 
 	/**
 	 * Update control outputs
 	 *
 	 */
-	void		control_update();
+	void control_update();
 
 	/**
 	 * Check for changes in vehicle control mode.
 	 */
-	void		vehicle_control_mode_poll();
+	void vehicle_control_mode_poll();
 
 	/**
 	 * Check for changes in manual inputs.
 	 */
-	void		vehicle_manual_poll();
-
+	void vehicle_manual_poll();
 
 	/**
 	 * Check for airspeed updates.
 	 */
-	void		vehicle_airspeed_poll();
+	void vehicle_airspeed_poll();
 
 	/**
 	 * Check for accel updates.
 	 */
-	void		vehicle_accel_poll();
+	void vehicle_accel_poll();
 
 	/**
 	 * Check for set triplet updates.
 	 */
-	void		vehicle_setpoint_poll();
+	void vehicle_setpoint_poll();
 
 	/**
 	 * Check for global position updates.
 	 */
-	void		global_pos_poll();
+	void global_pos_poll();
 
 	/**
 	 * Check for vehicle status updates.
 	 */
-	void		vehicle_status_poll();
+	void vehicle_status_poll();
 
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
-	static void	task_main_trampoline(int argc, char *argv[]);
+	static void task_main_trampoline(int argc, char *argv[]);
 
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main();
+	void task_main();
 
 };
 
-namespace att_control
-{
+namespace att_control {
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -304,39 +301,28 @@ namespace att_control
 #endif
 static const int ERROR = -1;
 
-FixedwingAttitudeControl	*g_control = nullptr;
+FixedwingAttitudeControl *g_control = nullptr;
 }
 
 FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
-	_task_should_exit(false),
-	_task_running(false),
-	_control_task(-1),
-
-/* subscriptions */
-	_att_sub(-1),
-	_accel_sub(-1),
-	_airspeed_sub(-1),
-	_vcontrol_mode_sub(-1),
-	_params_sub(-1),
-	_manual_sub(-1),
-	_global_pos_sub(-1),
-	_vehicle_status_sub(-1),
-
-/* publications */
-	_rate_sp_pub(-1),
-	_attitude_sp_pub(-1),
-	_actuators_0_pub(-1),
-	_actuators_1_pub(-1),
-
-/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
-	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
-	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
-/* states */
-	_setpoint_valid(false),
-	_debug(false)
-{
+		_task_should_exit(false), _task_running(false), _control_task(-1),
+
+		/* subscriptions */
+		_att_sub(-1), _accel_sub(-1), _airspeed_sub(-1), _vcontrol_mode_sub(-1), _params_sub(
+				-1), _manual_sub(-1), _global_pos_sub(-1), _vehicle_status_sub(
+				-1),
+
+		/* publications */
+		_rate_sp_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), _actuators_1_pub(
+				-1),
+
+		/* performance counters */
+		_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(
+				perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(
+				perf_alloc(PC_COUNT, "fw att control nonfinite output")),
+		/* states */
+		_setpoint_valid(false), _debug(false) {
 	/* safely initialize structs */
 	_att = {};
 	_accel = {};
@@ -349,7 +335,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_global_pos = {};
 	_vehicle_status = {};
 
-
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
 	_parameter_handles.p_p = param_find("FW_PR_P");
 	_parameter_handles.p_i = param_find("FW_PR_I");
@@ -390,8 +375,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	parameters_update();
 }
 
-FixedwingAttitudeControl::~FixedwingAttitudeControl()
-{
+FixedwingAttitudeControl::~FixedwingAttitudeControl() {
 	if (_control_task != -1) {
 
 		/* task wakes up every 100ms or so at the longest */
@@ -419,9 +403,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl()
 	att_control::g_control = nullptr;
 }
 
-int
-FixedwingAttitudeControl::parameters_update()
-{
+int FixedwingAttitudeControl::parameters_update() {
 
 	param_get(_parameter_handles.tconst, &(_parameters.tconst));
 	param_get(_parameter_handles.p_p, &(_parameters.p_p));
@@ -429,21 +411,26 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.p_ff, &(_parameters.p_ff));
 	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
 	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
-	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
-	param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
+	param_get(_parameter_handles.p_integrator_max,
+			&(_parameters.p_integrator_max));
+	param_get(_parameter_handles.p_roll_feedforward,
+			&(_parameters.p_roll_feedforward));
 
 	param_get(_parameter_handles.r_p, &(_parameters.r_p));
 	param_get(_parameter_handles.r_i, &(_parameters.r_i));
 	param_get(_parameter_handles.r_ff, &(_parameters.r_ff));
 
-	param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
+	param_get(_parameter_handles.r_integrator_max,
+			&(_parameters.r_integrator_max));
 	param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
 
 	param_get(_parameter_handles.y_p, &(_parameters.y_p));
 	param_get(_parameter_handles.y_i, &(_parameters.y_i));
 	param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
-	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
-	param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
+	param_get(_parameter_handles.y_integrator_max,
+			&(_parameters.y_integrator_max));
+	param_get(_parameter_handles.y_coordinated_min_speed,
+			&(_parameters.y_coordinated_min_speed));
 	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
@@ -453,16 +440,19 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
 	param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
 	param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
-	param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
-	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
-	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
-	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
+	param_get(_parameter_handles.rollsp_offset_deg,
+			&(_parameters.rollsp_offset_deg));
+	param_get(_parameter_handles.pitchsp_offset_deg,
+			&(_parameters.pitchsp_offset_deg));
+	_parameters.rollsp_offset_rad = math::radians(
+			_parameters.rollsp_offset_deg);
+	_parameters.pitchsp_offset_rad = math::radians(
+			_parameters.pitchsp_offset_deg);
 	param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
 	param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
 	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
 	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
 
-
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
 	_pitch_ctrl.set_k_p(_parameters.p_p);
@@ -492,9 +482,7 @@ FixedwingAttitudeControl::parameters_update()
 	return OK;
 }
 
-void
-FixedwingAttitudeControl::vehicle_control_mode_poll()
-{
+void FixedwingAttitudeControl::vehicle_control_mode_poll() {
 	bool vcontrol_mode_updated;
 
 	/* Check HIL state if vehicle status has changed */
@@ -502,13 +490,12 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 
 	if (vcontrol_mode_updated) {
 
-		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
+		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub,
+				&_vcontrol_mode);
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_manual_poll()
-{
+void FixedwingAttitudeControl::vehicle_manual_poll() {
 	bool manual_updated;
 
 	/* get pilots inputs */
@@ -520,9 +507,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_airspeed_poll()
-{
+void FixedwingAttitudeControl::vehicle_airspeed_poll() {
 	/* check if there is a new position */
 	bool airspeed_updated;
 	orb_check(_airspeed_sub, &airspeed_updated);
@@ -533,9 +518,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_accel_poll()
-{
+void FixedwingAttitudeControl::vehicle_accel_poll() {
 	/* check if there is a new position */
 	bool accel_updated;
 	orb_check(_accel_sub, &accel_updated);
@@ -545,9 +528,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_setpoint_poll()
-{
+void FixedwingAttitudeControl::vehicle_setpoint_poll() {
 	/* check if there is a new setpoint */
 	bool att_sp_updated;
 	orb_check(_att_sp_sub, &att_sp_updated);
@@ -558,21 +539,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::global_pos_poll()
-{
+void FixedwingAttitudeControl::global_pos_poll() {
 	/* check if there is a new global position */
 	bool global_pos_updated;
 	orb_check(_global_pos_sub, &global_pos_updated);
 
 	if (global_pos_updated) {
-		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub,
+				&_global_pos);
 	}
 }
 
-void
-FixedwingAttitudeControl::vehicle_status_poll()
-{
+void FixedwingAttitudeControl::vehicle_status_poll() {
 	/* check if there is new status information */
 	bool vehicle_status_updated;
 	orb_check(_vehicle_status_sub, &vehicle_status_updated);
@@ -582,15 +560,11 @@ FixedwingAttitudeControl::vehicle_status_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
-{
+void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) {
 	att_control::g_control->task_main();
 }
 
-void
-FixedwingAttitudeControl::task_main()
-{
+void FixedwingAttitudeControl::task_main() {
 
 	/* inform about start */
 	warnx("Initializing..");
@@ -667,7 +641,6 @@ FixedwingAttitudeControl::task_main()
 		/* only run controller if attitude changed */
 		if (fds[1].revents & POLLIN) {
 
-
 			static uint64_t last_run = 0;
 			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 			last_run = hrt_absolute_time();
@@ -721,8 +694,8 @@ FixedwingAttitudeControl::task_main()
 				float airspeed;
 
 				/* 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 = !isfinite(_airspeed.true_airspeed_m_s)
+						|| hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
 					airspeed = _parameters.airspeed_trim;
 					if (nonfinite) {
 						perf_count(_nonfinite_input_perf);
@@ -740,16 +713,20 @@ FixedwingAttitudeControl::task_main()
 				 * Forcing the scaling to this value allows reasonable handheld tests.
 				 */
 
-				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
+				float airspeed_scaling = _parameters.airspeed_trim
+						/ ((airspeed < _parameters.airspeed_min) ?
+								_parameters.airspeed_min : airspeed);
 
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
 				float throttle_sp = 0.0f;
 
-				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
+				if (_vcontrol_mode.flag_control_velocity_enabled
+						|| _vcontrol_mode.flag_control_position_enabled) {
 					/* read in attitude setpoint from attitude setpoint uorb topic */
 					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
-					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+					pitch_sp = _att_sp.pitch_body
+							+ _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
@@ -775,10 +752,12 @@ FixedwingAttitudeControl::task_main()
 					 * the intended attitude setpoint. Later, after the rate control step the
 					 * trim is added again to the control signal.
 					 */
-					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
-						+ _parameters.rollsp_offset_rad;
-					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
-						+ _parameters.pitchsp_offset_rad;
+					roll_sp = (_manual.y * _parameters.man_roll_max
+							- _parameters.trim_roll)
+							+ _parameters.rollsp_offset_rad;
+					pitch_sp = -(_manual.x * _parameters.man_pitch_max
+							- _parameters.trim_pitch)
+							+ _parameters.pitchsp_offset_rad;
 					throttle_sp = _manual.z;
 					_actuators.control[4] = _manual.flaps;
 
@@ -797,11 +776,13 @@ FixedwingAttitudeControl::task_main()
 					/* lazily publish the setpoint only once available */
 					if (_attitude_sp_pub > 0) {
 						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
+						orb_publish(ORB_ID(vehicle_attitude_setpoint),
+								_attitude_sp_pub, &att_sp);
 
 					} else {
 						/* advertise and publish */
-						_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+						_attitude_sp_pub = orb_advertise(
+								ORB_ID(vehicle_attitude_setpoint), &att_sp);
 					}
 				}
 
@@ -816,11 +797,17 @@ FixedwingAttitudeControl::task_main()
 				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 = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
-					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
-					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
-				} else	{
+				if (_att.R_valid) {
+					speed_body_u = _att.R[0][0] * _global_pos.vel_n
+							+ _att.R[1][0] * _global_pos.vel_e
+							+ _att.R[2][0] * _global_pos.vel_d;
+					speed_body_v = _att.R[0][1] * _global_pos.vel_n
+							+ _att.R[1][1] * _global_pos.vel_e
+							+ _att.R[2][1] * _global_pos.vel_d;
+					speed_body_w = _att.R[0][2] * _global_pos.vel_n
+							+ _att.R[1][2] * _global_pos.vel_e
+							+ _att.R[2][2] * _global_pos.vel_d;
+				} else {
 					if (_debug && loop_counter % 10 == 0) {
 						warnx("Did not get a valid R\n");
 					}
@@ -829,74 +816,94 @@ FixedwingAttitudeControl::task_main()
 				/* Run attitude controllers */
 				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
 					_roll_ctrl.control_attitude(roll_sp, _att.roll);
-					_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
+					_pitch_ctrl.control_attitude(pitch_sp, _att.roll,
+							_att.pitch, airspeed);
 					_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
 							speed_body_u, speed_body_v, speed_body_w,
-							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+							_roll_ctrl.get_desired_rate(),
+							_pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
 
 					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
 					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
 							_att.rollspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
+							_parameters.airspeed_min, _parameters.airspeed_max,
+							airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[0] =
+							(isfinite(roll_u)) ?
+									roll_u + _parameters.trim_roll :
+									_parameters.trim_roll;
 					if (!isfinite(roll_u)) {
 						_roll_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("roll_u %.4f", (double)roll_u);
+							warnx("roll_u %.4f", (double) roll_u);
 						}
 					}
 
-					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
-							_att.pitchspeed, _att.yawspeed,
+					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll,
+							_att.pitch, _att.pitchspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
+							_parameters.airspeed_min, _parameters.airspeed_max,
+							airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[1] =
+							(isfinite(pitch_u)) ?
+									pitch_u + _parameters.trim_pitch :
+									_parameters.trim_pitch;
 					if (!isfinite(pitch_u)) {
 						_pitch_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
-								" airspeed %.4f, airspeed_scaling %.4f,"
-								" roll_sp %.4f, pitch_sp %.4f,"
-								" _roll_ctrl.get_desired_rate() %.4f,"
-								" _pitch_ctrl.get_desired_rate() %.4f"
-								" att_sp.roll_body %.4f",
-								(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
-								(double)airspeed, (double)airspeed_scaling,
-								(double)roll_sp, (double)pitch_sp,
-								(double)_roll_ctrl.get_desired_rate(),
-								(double)_pitch_ctrl.get_desired_rate(),
-								(double)_att_sp.roll_body);
+							warnx(
+									"pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
+											" airspeed %.4f, airspeed_scaling %.4f,"
+											" roll_sp %.4f, pitch_sp %.4f,"
+											" _roll_ctrl.get_desired_rate() %.4f,"
+											" _pitch_ctrl.get_desired_rate() %.4f"
+											" att_sp.roll_body %.4f",
+									(double) pitch_u,
+									(double) _yaw_ctrl.get_desired_rate(),
+									(double) airspeed,
+									(double) airspeed_scaling, (double) roll_sp,
+									(double) pitch_sp,
+									(double) _roll_ctrl.get_desired_rate(),
+									(double) _pitch_ctrl.get_desired_rate(),
+									(double) _att_sp.roll_body);
 						}
 					}
 
-					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
-							_att.pitchspeed, _att.yawspeed,
+					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll,
+							_att.pitch, _att.pitchspeed, _att.yawspeed,
 							_pitch_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+							_parameters.airspeed_min, _parameters.airspeed_max,
+							airspeed, airspeed_scaling, lock_integrator);
+					_actuators.control[2] =
+							(isfinite(yaw_u)) ?
+									yaw_u + _parameters.trim_yaw :
+									_parameters.trim_yaw;
 					if (!isfinite(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("yaw_u %.4f", (double)yaw_u);
+							warnx("yaw_u %.4f", (double) yaw_u);
 						}
 					}
 
 					/* throttle passed through */
-					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+					_actuators.control[3] =
+							(isfinite(throttle_sp)) ? throttle_sp : 0.0f;
 					if (!isfinite(throttle_sp)) {
 						if (_debug && loop_counter % 10 == 0) {
-							warnx("throttle_sp %.4f", (double)throttle_sp);
+							warnx("throttle_sp %.4f", (double) throttle_sp);
 						}
 					}
 				} else {
 					perf_count(_nonfinite_input_perf);
 					if (_debug && loop_counter % 10 == 0) {
-						warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
+						warnx(
+								"Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f",
+								(double) roll_sp, (double) pitch_sp);
 					}
 				}
 
@@ -913,11 +920,13 @@ FixedwingAttitudeControl::task_main()
 
 				if (_rate_sp_pub > 0) {
 					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub,
+							&rates_sp);
 
 				} else {
 					/* advertise and publish */
-					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),
+							&rates_sp);
 				}
 
 			} else {
@@ -939,16 +948,19 @@ FixedwingAttitudeControl::task_main()
 
 			if (_actuators_0_pub > 0) {
 				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub,
+						&_actuators);
 
 			} else {
 				/* advertise and publish */
-				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0),
+						&_actuators);
 			}
 
 			if (_actuators_1_pub > 0) {
 				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
+				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub,
+						&_actuators_airframe);
 //				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
 //						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
 //						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
@@ -956,7 +968,8 @@ FixedwingAttitudeControl::task_main()
 
 			} else {
 				/* advertise and publish */
-				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
+				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1),
+						&_actuators_airframe);
 			}
 
 		}
@@ -972,18 +985,14 @@ FixedwingAttitudeControl::task_main()
 	_exit(0);
 }
 
-int
-FixedwingAttitudeControl::start()
-{
+int FixedwingAttitudeControl::start() {
 	ASSERT(_control_task == -1);
 
 	/* start the task */
 	_control_task = task_spawn_cmd("fw_att_control",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2048,
-				       (main_t)&FixedwingAttitudeControl::task_main_trampoline,
-				       nullptr);
+	SCHED_DEFAULT,
+	SCHED_PRIORITY_MAX - 5, 2048,
+			(main_t) &FixedwingAttitudeControl::task_main_trampoline, nullptr);
 
 	if (_control_task < 0) {
 		warn("task start failed");
@@ -993,8 +1002,7 @@ FixedwingAttitudeControl::start()
 	return OK;
 }
 
-int fw_att_control_main(int argc, char *argv[])
-{
+int fw_att_control_main(int argc, char *argv[]) {
 	if (argc < 1)
 		errx(1, "usage: fw_att_control {start|stop|status}");
 
@@ -1015,7 +1023,8 @@ int fw_att_control_main(int argc, char *argv[])
 		}
 
 		/* avoid memory fragmentation by not exiting start handler until the task has fully started */
-		while (att_control::g_control == nullptr || !att_control::g_control->task_running()) {
+		while (att_control::g_control == nullptr
+				|| !att_control::g_control->task_running()) {
 			usleep(50000);
 			printf(".");
 			fflush(stdout);