fw_pos_control_l1: pass time through from run

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -536,16 +536,12 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim
 }
 
 bool
-FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vector2f &ground_speed,
+FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2f &curr_pos,
+		const Vector2f &ground_speed,
 		const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
 {
-	float dt = 0.01f;
-
-	if (_control_position_last_called > 0) {
-		dt = hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
-	}
-
-	_control_position_last_called = hrt_absolute_time();
+	const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
+	_control_position_last_called = now;
 
 	_l1_control.set_dt(dt);
 
@@ -582,7 +578,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 	/* save time when airplane is in air */
 	if (!_was_in_air && !_vehicle_land_detected.landed) {
 		_was_in_air = true;
-		_time_went_in_air = hrt_absolute_time();
+		_time_went_in_air = now;
 		_takeoff_ground_alt = _current_altitude;
 	}
 
@@ -688,7 +684,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			tecs_update_pitch_throttle(pos_sp_curr.alt,
+			tecs_update_pitch_throttle(now, pos_sp_curr.alt,
 						   calculate_target_airspeed(mission_airspeed, ground_speed),
 						   radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),
 						   radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
@@ -745,7 +741,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 				_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
 			}
 
-			tecs_update_pitch_throttle(alt_sp,
+			tecs_update_pitch_throttle(now, alt_sp,
 						   calculate_target_airspeed(mission_airspeed, ground_speed),
 						   radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),
 						   radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
@@ -756,10 +752,10 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 						   radians(_param_fw_p_lim_min.get()));
 
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
-			control_landing(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
+			control_landing(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
 
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
-			control_takeoff(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
+			control_takeoff(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
 		}
 
 		/* reset landing state */
@@ -815,7 +811,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 			throttle_max = 0.0f;
 		}
 
-		tecs_update_pitch_throttle(_hold_alt,
+		tecs_update_pitch_throttle(now, _hold_alt,
 					   altctrl_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(_param_fw_p_lim_max.get()),
@@ -917,7 +913,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 			throttle_max = 0.0f;
 		}
 
-		tecs_update_pitch_throttle(_hold_alt,
+		tecs_update_pitch_throttle(now, _hold_alt,
 					   altctrl_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(_param_fw_p_lim_max.get()),
@@ -962,7 +958,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 		   pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 		   _runway_takeoff.runwayTakeoffEnabled()) {
 
-		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max));
+		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, min(get_tecs_thrust(), throttle_max));
 
 	} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
 		   pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
@@ -1013,8 +1009,8 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
 }
 
 void
-FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed,
-		const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f &curr_pos,
+		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
 	/* current waypoint (the one currently heading for) */
 	Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
@@ -1047,7 +1043,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
 	if (_runway_takeoff.runwayTakeoffEnabled()) {
 		if (!_runway_takeoff.isInitialized()) {
 			Eulerf euler(Quatf(_att.q));
-			_runway_takeoff.init(euler.psi(), _current_latitude, _current_longitude);
+			_runway_takeoff.init(now, euler.psi(), _current_latitude, _current_longitude);
 
 			/* need this already before takeoff is detected
 			 * doesn't matter if it gets reset when takeoff is detected eventually */
@@ -1059,7 +1055,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
 		float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt);
 
 		// update runway takeoff helper
-		_runway_takeoff.update(_airspeed, _current_altitude - terrain_alt,
+		_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
 				       _current_latitude, _current_longitude, &_mavlink_log_pub);
 
 		/*
@@ -1071,7 +1067,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
 		// update tecs
 		const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
 
-		tecs_update_pitch_throttle(pos_sp_curr.alt,
+		tecs_update_pitch_throttle(now, pos_sp_curr.alt,
 					   calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed),
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(takeoff_pitch_max_deg),
@@ -1101,13 +1097,13 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
 				/* Perform launch detection */
 
 				/* Inform user that launchdetection is running every 4s */
-				if (hrt_elapsed_time(&_launch_detection_notify) > 4e6) {
+				if ((now - _launch_detection_notify) > 4_s) {
 					mavlink_log_critical(&_mavlink_log_pub, "Launch detection running");
-					_launch_detection_notify = hrt_absolute_time();
+					_launch_detection_notify = now;
 				}
 
 				/* Detect launch using body X (forward) acceleration */
-				_launchDetector.update(_vehicle_acceleration_sub.get().xyz[0]);
+				_launchDetector.update(now, _vehicle_acceleration_sub.get().xyz[0]);
 
 				/* update our copy of the launch detection state */
 				_launch_detection_state = _launchDetector.getLaunchDetected();
@@ -1142,7 +1138,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
 			/* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */
 			if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) {
 				/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-				tecs_update_pitch_throttle(pos_sp_curr.alt,
+				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
 							   _param_fw_airspd_trim.get(),
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(takeoff_pitch_max_deg),
@@ -1157,7 +1153,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
 				_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
 
 			} else {
-				tecs_update_pitch_throttle(pos_sp_curr.alt,
+				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
 							   calculate_target_airspeed(_param_fw_airspd_trim.get(), ground_speed),
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(_param_fw_p_lim_max.get()),
@@ -1183,8 +1179,8 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
 }
 
 void
-FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed,
-		const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
+FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f &curr_pos,
+		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
 	/* current waypoint (the one currently heading for) */
 	Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
@@ -1213,7 +1209,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 	// save time at which we started landing and reset abort_landing
 	if (_time_started_landing == 0) {
 		reset_landing_state();
-		_time_started_landing = hrt_absolute_time();
+		_time_started_landing = now;
 	}
 
 	const float bearing_airplane_currwp = get_bearing_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1),
@@ -1299,13 +1295,13 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 			float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
 			terrain_alt = (_local_pos.ref_alt - terrain_vpos);
 			_t_alt_prev_valid = terrain_alt;
-			_time_last_t_alt = hrt_absolute_time();
+			_time_last_t_alt = now;
 
 		} else if (_time_last_t_alt == 0) {
 			// we have started landing phase but don't have valid terrain
 			// wait for some time, maybe we will soon get a valid estimate
 			// until then just use the altitude of the landing waypoint
-			if (hrt_elapsed_time(&_time_started_landing) < 10_s) {
+			if ((now - _time_started_landing) < 10_s) {
 				terrain_alt = pos_sp_curr.alt;
 
 			} else {
@@ -1314,7 +1310,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 				abort_landing(true);
 			}
 
-		} else if ((!_local_pos.dist_bottom_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT)
+		} else if ((!_local_pos.dist_bottom_valid && (now - _time_last_t_alt) < T_ALT_TIMEOUT)
 			   || _land_noreturn_vertical) {
 			// use previous terrain estimate for some time and hope to recover
 			// if we are already flaring (land_noreturn_vertical) then just
@@ -1372,7 +1368,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 		const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
 		const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
 
-		tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
+		tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel,
 					   calculate_target_airspeed(airspeed_land, ground_speed),
 					   radians(_param_fw_lnd_fl_pmin.get()),
 					   radians(_param_fw_lnd_fl_pmax.get()),
@@ -1440,7 +1436,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 
 		const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
 
-		tecs_update_pitch_throttle(altitude_desired,
+		tecs_update_pitch_throttle(now, altitude_desired,
 					   calculate_target_airspeed(airspeed_approach, ground_speed),
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(_param_fw_p_lim_max.get()),
@@ -1573,8 +1569,9 @@ FixedwingPositionControl::Run()
 		 * Attempt to control position, on success (= sensors present and not in manual mode),
 		 * publish setpoint.
 		 */
-		if (control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current, _pos_sp_triplet.next)) {
-			_att_sp.timestamp = hrt_absolute_time();
+		if (control_position(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
+				     _pos_sp_triplet.next)) {
+
 
 			// add attitude setpoint offsets
 			_att_sp.roll_body += radians(_param_fw_rsp_off.get());
@@ -1587,15 +1584,16 @@ FixedwingPositionControl::Run()
 							       radians(_param_fw_man_p_max.get()));
 			}
 
-			Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
-			q.copyTo(_att_sp.q_d);
-
 			if (_control_mode.flag_control_offboard_enabled ||
 			    _control_mode.flag_control_position_enabled ||
 			    _control_mode.flag_control_velocity_enabled ||
 			    _control_mode.flag_control_acceleration_enabled ||
 			    _control_mode.flag_control_altitude_enabled) {
 
+				const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
+				q.copyTo(_att_sp.q_d);
+
+				_att_sp.timestamp = hrt_absolute_time();
 				_attitude_sp_pub.publish(_att_sp);
 
 				// only publish status in full FW mode
@@ -1650,19 +1648,14 @@ FixedwingPositionControl::reset_landing_state()
 }
 
 void
-FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspeed_sp,
+FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp,
 		float pitch_min_rad, float pitch_max_rad,
 		float throttle_min, float throttle_max, float throttle_cruise,
 		bool climbout_mode, float climbout_pitch_min_rad,
 		uint8_t mode)
 {
-	float dt = 0.01f; // prevent division with 0
-
-	if (_last_tecs_update > 0) {
-		dt = hrt_elapsed_time(&_last_tecs_update) * 1e-6;
-	}
-
-	_last_tecs_update = hrt_absolute_time();
+	const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, 0.01f, 0.05f);
+	_last_tecs_update = now;
 
 	// do not run TECS if we are not in air
 	bool run_tecs = !_vehicle_land_detected.landed;

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -325,11 +325,14 @@ private:
 	 */
 	bool		update_desired_altitude(float dt);
 
-	bool		control_position(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
+	bool		control_position(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
+					 const position_setpoint_s &pos_sp_prev,
 					 const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
-	void		control_takeoff(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
+	void		control_takeoff(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
+					const position_setpoint_s &pos_sp_prev,
 					const position_setpoint_s &pos_sp_curr);
-	void		control_landing(const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
+	void		control_landing(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed,
+					const position_setpoint_s &pos_sp_prev,
 					const position_setpoint_s &pos_sp_curr);
 
 	float		get_tecs_pitch();
@@ -349,7 +352,7 @@ private:
 	/*
 	 * Call TECS : a wrapper function to call the TECS implementation
 	 */
-	void tecs_update_pitch_throttle(float alt_sp, float airspeed_sp,
+	void tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp,
 					float pitch_min_rad, float pitch_max_rad,
 					float throttle_min, float throttle_max, float throttle_cruise,
 					bool climbout_mode, float climbout_pitch_min_rad,

src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.cpp

@@ -49,14 +49,10 @@ namespace launchdetection
 CatapultLaunchMethod::CatapultLaunchMethod(ModuleParams *parent) :
 	ModuleParams(parent)
 {
-	_last_timestamp = hrt_absolute_time();
 }
 
-void CatapultLaunchMethod::update(float accel_x)
+void CatapultLaunchMethod::update(const float dt, float accel_x)
 {
-	float dt = hrt_elapsed_time(&_last_timestamp) * 1e-6f;
-	_last_timestamp = hrt_absolute_time();
-
 	switch (state) {
 	case LAUNCHDETECTION_RES_NONE:
 

src/modules/fw_pos_control_l1/launchdetection/CatapultLaunchMethod.h

@@ -55,13 +55,12 @@ public:
 	CatapultLaunchMethod(ModuleParams *parent);
 	~CatapultLaunchMethod() override = default;
 
-	void update(float accel_x) override;
+	void update(const float dt, float accel_x) override;
 	LaunchDetectionResult getLaunchDetected() const override;
 	void reset() override;
 	float getPitchMax(float pitchMaxDefault) override;
 
 private:
-	hrt_abstime _last_timestamp{0};
 	float _integrator{0.0f};
 	float _motorDelayCounter{0.0f};
 

src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp

@@ -68,11 +68,11 @@ void LaunchDetector::reset()
 	_activeLaunchDetectionMethodIndex = -1;
 }
 
-void LaunchDetector::update(float accel_x)
+void LaunchDetector::update(const float dt, float accel_x)
 {
 	if (launchDetectionEnabled()) {
 		for (const auto launchMethod : _launchMethods) {
-			launchMethod->update(accel_x);
+			launchMethod->update(dt, accel_x);
 		}
 	}
 }

src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h

@@ -58,7 +58,7 @@ public:
 
 	void reset();
 
-	void update(float accel_x);
+	void update(const float dt, float accel_x);
 	LaunchDetectionResult getLaunchDetected();
 	bool launchDetectionEnabled() { return _param_laun_all_on.get(); }
 

src/modules/fw_pos_control_l1/launchdetection/LaunchMethod.h

@@ -59,7 +59,7 @@ class LaunchMethod
 public:
 	virtual ~LaunchMethod() = default;
 
-	virtual void update(float accel_x) = 0;
+	virtual void update(const float dt, float accel_x) = 0;
 	virtual LaunchDetectionResult getLaunchDetected() const = 0;
 	virtual void reset() = 0;
 

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp

@@ -47,6 +47,7 @@
 #include <mathlib/mathlib.h>
 
 using matrix::Vector2f;
+using namespace time_literals;
 
 namespace runwaytakeoff
 {
@@ -61,25 +62,25 @@ RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) :
 {
 }
 
-void RunwayTakeoff::init(float yaw, double current_lat, double current_lon)
+void RunwayTakeoff::init(const hrt_abstime &now, float yaw, double current_lat, double current_lon)
 {
 	_init_yaw = yaw;
 	_initialized = true;
 	_state = RunwayTakeoffState::THROTTLE_RAMP;
-	_initialized_time = hrt_absolute_time();
+	_initialized_time = now;
 	_climbout = true; // this is true until climbout is finished
 	_start_wp(0) = (float)current_lat;
 	_start_wp(1) = (float)current_lon;
 }
 
-void RunwayTakeoff::update(float airspeed, float alt_agl,
+void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl,
 			   double current_lat, double current_lon, orb_advert_t *mavlink_log_pub)
 {
-
 	switch (_state) {
 	case RunwayTakeoffState::THROTTLE_RAMP:
-		if (hrt_elapsed_time(&_initialized_time) > _param_rwto_ramp_time.get() * 1e6f
-		    || airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get() * 0.9f) {
+		if (((now - _initialized_time) > (_param_rwto_ramp_time.get() * 1_s))
+		    || (airspeed > (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get() * 0.9f))) {
+
 			_state = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
 		}
 
@@ -191,15 +192,12 @@ float RunwayTakeoff::getYaw(float navigatorYaw)
  * Ramps up in the beginning, until it lifts off the runway it is set to
  * parameter value, then it returns the TECS throttle.
  */
-float RunwayTakeoff::getThrottle(float tecsThrottle)
+float RunwayTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle)
 {
 	switch (_state) {
 	case RunwayTakeoffState::THROTTLE_RAMP: {
-			float throttle = (hrt_elapsed_time(&_initialized_time) / (float)_param_rwto_ramp_time.get() * 1e6f) *
-					 _param_rwto_max_thr.get();
-			return throttle < _param_rwto_max_thr.get() ?
-			       throttle :
-			       _param_rwto_max_thr.get();
+			float throttle = ((now - _initialized_time) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get();
+			return math::min(throttle, _param_rwto_max_thr.get());
 		}
 
 	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:

src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h

@@ -68,8 +68,9 @@ public:
 	RunwayTakeoff(ModuleParams *parent);
 	~RunwayTakeoff() = default;
 
-	void init(float yaw, double current_lat, double current_lon);
-	void update(float airspeed, float alt_agl, double current_lat, double current_lon, orb_advert_t *mavlink_log_pub);
+	void init(const hrt_abstime &now, float yaw, double current_lat, double current_lon);
+	void update(const hrt_abstime &now, float airspeed, float alt_agl, double current_lat, double current_lon,
+		    orb_advert_t *mavlink_log_pub);
 
 	RunwayTakeoffState getState() { return _state; }
 	bool isInitialized() { return _initialized; }
@@ -83,7 +84,7 @@ public:
 	float getPitch(float tecsPitch);
 	float getRoll(float navigatorRoll);
 	float getYaw(float navigatorYaw);
-	float getThrottle(float tecsThrottle);
+	float getThrottle(const hrt_abstime &now, float tecsThrottle);
 	bool resetIntegrators();
 	float getMinPitch(float sp_min, float climbout_min, float min);
 	float getMaxPitch(float max);
@@ -93,11 +94,11 @@ public:
 
 private:
 	/** state variables **/
-	RunwayTakeoffState _state;
-	bool _initialized;
-	hrt_abstime _initialized_time;
-	float _init_yaw;
-	bool _climbout;
+	RunwayTakeoffState _state{THROTTLE_RAMP};
+	bool _initialized{false};
+	hrt_abstime _initialized_time{0};
+	float _init_yaw{0.f};
+	bool _climbout{false};
 	matrix::Vector2f _start_wp;
 
 	DEFINE_PARAMETERS(