estimataor reset counter: set Flighttaskcounter equal reset-counter during activation (#10035)

- fixes #10033

src/lib/FlightTasks/tasks/FlightTask.cpp

@@ -12,6 +12,10 @@ bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
 		return false;
 	}
 
+	if (!subscription_array.get(ORB_ID(vehicle_attitude), _sub_attitude)) {
+		return false;
+	}
+
 	return true;
 }
 
@@ -20,6 +24,7 @@ bool FlightTask::activate()
 	_resetSetpoints();
 	_setDefaultConstraints();
 	_time_stamp_activate = hrt_absolute_time();
+	_heading_reset_counter = _sub_attitude->get().quat_reset_counter;
 	return true;
 }
 

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -49,6 +49,7 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_constraints.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include "../SubscriptionArray.hpp"
 
 class FlightTask : public ModuleParams
@@ -129,6 +130,8 @@ public:
 protected:
 
 	uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
+	uORB::Subscription<vehicle_attitude_s> *_sub_attitude{nullptr};
+	uint8_t _heading_reset_counter{0}; /**< estimator heading reset */
 
 	/**
 	 * Reset all setpoints to NAN

src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp

@@ -40,20 +40,6 @@
 #include <float.h>
 
 using namespace matrix;
-uint8_t FlightTaskManualStabilized::_heading_reset_counter = 0;
-
-bool FlightTaskManualStabilized::initializeSubscriptions(SubscriptionArray &subscription_array)
-{
-	if (!FlightTaskManual::initializeSubscriptions(subscription_array)) {
-		return false;
-	}
-
-	if (!subscription_array.get(ORB_ID(vehicle_attitude), _sub_attitude)) {
-		return false;
-	}
-
-	return true;
-}
 
 bool FlightTaskManualStabilized::activate()
 {

src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp

@@ -41,7 +41,6 @@
 #pragma once
 
 #include "FlightTaskManual.hpp"
-#include <uORB/topics/vehicle_attitude.h>
 
 class FlightTaskManualStabilized : public FlightTaskManual
 {
@@ -52,8 +51,6 @@ public:
 	bool activate() override;
 	bool updateInitialize() override;
 	bool update() override;
-	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
-
 
 protected:
 	virtual void _updateSetpoints(); /**< updates all setpoints*/
@@ -61,11 +58,6 @@ protected:
 	void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /**< rotates vector into local frame */
 
 private:
-	static uint8_t _heading_reset_counter; /**< estimator heading reset */
-
-	uORB::Subscription<vehicle_attitude_s> *_sub_attitude{nullptr};
-
-
 	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
 	void _updateThrustSetpoints(); /**< sets thrust setpoint */
 	float _throttleCurve(); /**< piecewise linear mapping from stick to throttle */