From 7fa858ad84a4fa12deecdd342e5343a53012a69c Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 16 Feb 2018 20:23:47 -0500
Subject: [PATCH] fw_att_control only store battery_status scale

---
 .../FixedwingAttitudeControl.cpp              | 32 +++++++++----------
 .../FixedwingAttitudeControl.hpp              |  8 ++---
 2 files changed, 18 insertions(+), 22 deletions(-)

diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index 73d1db85e0..026ee19a4d 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -325,18 +325,6 @@ FixedwingAttitudeControl::vehicle_land_detected_poll()
 	}
 }
 
-void
-FixedwingAttitudeControl::battery_status_poll()
-{
-	/* check if there is a new message */
-	bool updated;
-	orb_check(_battery_status_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(battery_status), _battery_status_sub, &_battery_status);
-	}
-}
-
 void FixedwingAttitudeControl::run()
 {
 	/*
@@ -360,7 +348,6 @@ void FixedwingAttitudeControl::run()
 	vehicle_manual_poll();
 	vehicle_status_poll();
 	vehicle_land_detected_poll();
-	battery_status_poll();
 
 	/* wakeup source */
 	px4_pollfd_struct_t fds[1];
@@ -478,7 +465,6 @@ void FixedwingAttitudeControl::run()
 			global_pos_poll();
 			vehicle_status_poll();
 			vehicle_land_detected_poll();
-			battery_status_poll();
 
 			// the position controller will not emit attitude setpoints in some modes
 			// we need to make sure that this flag is reset
@@ -715,9 +701,23 @@ void FixedwingAttitudeControl::run()
 								&& !_vehicle_status.engine_failure) ? throttle_sp : 0.0f;
 
 						/* scale effort by battery status */
-						if (_parameters.bat_scale_en && _battery_status.scale > 0.0f &&
+						if (_parameters.bat_scale_en &&
 						    _actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
-							_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_status.scale;
+
+							bool updated = false;
+							orb_check(_battery_status_sub, &updated);
+
+							if (updated) {
+								battery_status_s battery_status = {};
+
+								if (orb_copy(ORB_ID(battery_status), _battery_status_sub, &battery_status) == PX4_OK) {
+									if (battery_status.scale > 0.0f) {
+										_battery_scale = battery_status.scale;
+									}
+								}
+							}
+
+							_actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
 						}
 
 					} else {
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index 4966ec60f2..39fac9d7da 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -116,7 +116,6 @@ private:
 
 	actuator_controls_s			_actuators {};		/**< actuator control inputs */
 	actuator_controls_s			_actuators_airframe {};	/**< actuator control inputs */
-	battery_status_s			_battery_status {};	/**< battery status */
 	manual_control_setpoint_s		_manual {};		/**< r/c channel data */
 	vehicle_attitude_s			_att {};		/**< vehicle attitude setpoint */
 	vehicle_attitude_setpoint_s		_att_sp {};		/**< vehicle attitude setpoint */
@@ -136,6 +135,8 @@ private:
 
 	bool _landed{true};
 
+	float _battery_scale{1.0f};
+
 	struct {
 		float p_tc;
 		float p_p;
@@ -301,9 +302,4 @@ private:
 	 */
 	void		vehicle_land_detected_poll();
 
-	/**
-	 * Check for battery status updates.
-	 */
-	void		battery_status_poll();
-
 };