fw_att_control: pass time through from run

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -284,7 +284,10 @@ void FixedwingAttitudeControl::Run()
 
 	perf_begin(_loop_perf);
 
-	if (_att_sub.update(&_att)) {
+	// only run controller if attitude changed
+	vehicle_attitude_s att;
+
+	if (_att_sub.update(&att)) {
 
 		// only update parameters if they changed
 		bool params_updated = _parameter_update_sub.updated();
@@ -300,13 +303,11 @@ void FixedwingAttitudeControl::Run()
 			parameters_update();
 		}
 
-		/* only run controller if attitude changed */
+		const float dt = math::constrain((att.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f);
-		static uint64_t last_run = 0;
+		_last_run = att.timestamp;
-		float deltaT = constrain((hrt_elapsed_time(&last_run) / 1e6f), 0.01f, 0.1f);
-		last_run = hrt_absolute_time();
 
 		/* get current rotation matrix and euler angles from control state quaternions */
-		matrix::Dcmf R = matrix::Quatf(_att.q);
+		matrix::Dcmf R = matrix::Quatf(att.q);
 
 		vehicle_angular_velocity_s angular_velocity{};
 		_vehicle_rates_sub.copy(&angular_velocity);
@@ -379,9 +380,10 @@ void FixedwingAttitudeControl::Run()
 			wheel_control = true;
 		}
 
-		/* lock integrator until control is started */
+		/* lock integrator until control is started or for long intervals (> 20 ms) */
 		bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled
-				       || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && ! _vehicle_status.in_transition_mode);
+				       || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && ! _vehicle_status.in_transition_mode)
+				       || (dt > 0.02f);
 
 		/* if we are in rotary wing mode, do nothing */
 		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
@@ -389,7 +391,7 @@ void FixedwingAttitudeControl::Run()
 			return;
 		}
 
-		control_flaps(deltaT);
+		control_flaps(dt);
 
 		/* decide if in stabilized or full manual control */
 		if (_vcontrol_mode.flag_control_rates_enabled) {
@@ -424,7 +426,7 @@ void FixedwingAttitudeControl::Run()
 			}
 
 			/* Prepare data for attitude controllers */
-			struct ECL_ControlData control_input = {};
+			ECL_ControlData control_input{};
 			control_input.roll = euler_angles.phi();
 			control_input.pitch = euler_angles.theta();
 			control_input.yaw = euler_angles.psi();
@@ -502,16 +504,16 @@ void FixedwingAttitudeControl::Run()
 			/* Run attitude controllers */
 			if (_vcontrol_mode.flag_control_attitude_enabled) {
 				if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
-					_roll_ctrl.control_attitude(control_input);
+					_roll_ctrl.control_attitude(dt, control_input);
-					_pitch_ctrl.control_attitude(control_input);
+					_pitch_ctrl.control_attitude(dt, control_input);
 
 					if (wheel_control) {
-						_wheel_ctrl.control_attitude(control_input);
+						_wheel_ctrl.control_attitude(dt, control_input);
 						_yaw_ctrl.reset_integrator();
 
 					} else {
 						// runs last, because is depending on output of roll and pitch attitude
-						_yaw_ctrl.control_attitude(control_input);
+						_yaw_ctrl.control_attitude(dt, control_input);
 						_wheel_ctrl.reset_integrator();
 					}
 
@@ -521,14 +523,14 @@ void FixedwingAttitudeControl::Run()
 					control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
 
 					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
-					float roll_u = _roll_ctrl.control_euler_rate(control_input);
+					float roll_u = _roll_ctrl.control_euler_rate(dt, control_input);
 					_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
 
 					if (!PX4_ISFINITE(roll_u)) {
 						_roll_ctrl.reset_integrator();
 					}
 
-					float pitch_u = _pitch_ctrl.control_euler_rate(control_input);
+					float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input);
 					_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
 
 					if (!PX4_ISFINITE(pitch_u)) {
@@ -538,10 +540,10 @@ void FixedwingAttitudeControl::Run()
 					float yaw_u = 0.0f;
 
 					if (wheel_control) {
-						yaw_u = _wheel_ctrl.control_bodyrate(control_input);
+						yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input);
 
 					} else {
-						yaw_u = _yaw_ctrl.control_euler_rate(control_input);
+						yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input);
 					}
 
 					_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
@@ -597,13 +599,13 @@ void FixedwingAttitudeControl::Run()
 				_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw);
 				_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
 
-				float roll_u = _roll_ctrl.control_bodyrate(control_input);
+				float roll_u = _roll_ctrl.control_bodyrate(dt, control_input);
 				_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
 
-				float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
+				float pitch_u = _pitch_ctrl.control_bodyrate(dt, control_input);
 				_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
 
-				float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
+				float yaw_u = _yaw_ctrl.control_bodyrate(dt, control_input);
 				_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
 
 				_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ?
@@ -638,7 +640,7 @@ void FixedwingAttitudeControl::Run()
 
 		/* lazily publish the setpoint only once available */
 		_actuators.timestamp = hrt_absolute_time();
-		_actuators.timestamp_sample = _att.timestamp;
+		_actuators.timestamp_sample = att.timestamp;
 
 		/* Only publish if any of the proper modes are enabled */
 		if (_vcontrol_mode.flag_control_rates_enabled ||

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -115,7 +115,6 @@ private:
 
 	actuator_controls_s			_actuators {};		/**< actuator control inputs */
 	manual_control_setpoint_s		_manual_control_setpoint {};		/**< r/c channel data */
-	vehicle_attitude_s			_att {};		/**< vehicle attitude setpoint */
 	vehicle_attitude_setpoint_s		_att_sp {};		/**< vehicle attitude setpoint */
 	vehicle_control_mode_s			_vcontrol_mode {};	/**< vehicle control mode */
 	vehicle_local_position_s		_local_pos {};		/**< local position */
@@ -124,6 +123,8 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
+	hrt_abstime _last_run{0};
+
 	float _flaps_applied{0.0f};
 	float _flaperons_applied{0.0f};
 

src/modules/fw_att_control/ecl_controller.h

@@ -79,9 +79,9 @@ public:
 	ECL_Controller();
 	virtual ~ECL_Controller() = default;
 
-	virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0;
+	virtual float control_attitude(const float dt, const ECL_ControlData &ctl_data) = 0;
-	virtual float control_euler_rate(const struct ECL_ControlData &ctl_data) = 0;
+	virtual float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) = 0;
-	virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0;
+	virtual float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) = 0;
 
 	/* Setters */
 	void set_time_constant(float time_constant);

src/modules/fw_att_control/ecl_pitch_controller.cpp

@@ -43,9 +43,8 @@
 #include <lib/ecl/geo/geo.h>
 #include <mathlib/mathlib.h>
 
-float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
+float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
 {
-
 	/* Do not calculate control signal with bad inputs */
 	if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
 	      PX4_ISFINITE(ctl_data.roll) &&
@@ -64,7 +63,7 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
 	return _rate_setpoint;
 }
 
-float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(PX4_ISFINITE(ctl_data.roll) &&
@@ -79,22 +78,10 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
-	/* get the usual dt estimate */
-	uint64_t dt_micros = hrt_elapsed_time(&_last_run);
-	_last_run = hrt_absolute_time();
-	float dt = (float)dt_micros * 1e-6f;
-
-	/* lock integral for long intervals */
-	bool lock_integrator = ctl_data.lock_integrator;
-
-	if (dt_micros > 500000) {
-		lock_integrator = true;
-	}
-
 	/* Calculate body angular rate error */
 	_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;
 
-	if (!lock_integrator && _k_i > 0.0f) {
+	if (!ctl_data.lock_integrator && _k_i > 0.0f) {
 
 		/* Integral term scales with 1/IAS^2 */
 		float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
@@ -124,7 +111,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
-float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data)
+float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
 {
 	/* Transform setpoint to body angular rates (jacobian) */
 	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
@@ -132,5 +119,5 @@ float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_
 
 	set_bodyrate_setpoint(_bodyrate_setpoint);
 
-	return control_bodyrate(ctl_data);
+	return control_bodyrate(dt, ctl_data);
 }

src/modules/fw_att_control/ecl_pitch_controller.h

@@ -60,9 +60,9 @@ public:
 	ECL_PitchController() = default;
 	~ECL_PitchController() = default;
 
-	float control_attitude(const struct ECL_ControlData &ctl_data) override;
+	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
+	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
+	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
 
 	/* Additional Setters */
 	void set_max_rate_pos(float max_rate_pos)

src/modules/fw_att_control/ecl_roll_controller.cpp

@@ -43,7 +43,7 @@
 #include <lib/ecl/geo/geo.h>
 #include <mathlib/mathlib.h>
 
-float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
+float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
@@ -61,7 +61,7 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
 	return _rate_setpoint;
 }
 
-float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(PX4_ISFINITE(ctl_data.pitch) &&
@@ -75,22 +75,10 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
-	/* get the usual dt estimate */
-	uint64_t dt_micros = hrt_elapsed_time(&_last_run);
-	_last_run = hrt_absolute_time();
-	float dt = (float)dt_micros * 1e-6f;
-
-	/* lock integral for long intervals */
-	bool lock_integrator = ctl_data.lock_integrator;
-
-	if (dt_micros > 500000) {
-		lock_integrator = true;
-	}
-
 	/* Calculate body angular rate error */
 	_rate_error = _bodyrate_setpoint - ctl_data.body_x_rate;
 
-	if (!lock_integrator && _k_i > 0.0f) {
+	if (!ctl_data.lock_integrator && _k_i > 0.0f) {
 
 		/* Integral term scales with 1/IAS^2 */
 		float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
@@ -120,12 +108,12 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
-float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_data)
+float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
 {
 	/* Transform setpoint to body angular rates (jacobian) */
 	_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
 
 	set_bodyrate_setpoint(_bodyrate_setpoint);
 
-	return control_bodyrate(ctl_data);
+	return control_bodyrate(dt, ctl_data);
 }

src/modules/fw_att_control/ecl_roll_controller.h

@@ -58,9 +58,9 @@ public:
 	ECL_RollController() = default;
 	~ECL_RollController() = default;
 
-	float control_attitude(const struct ECL_ControlData &ctl_data) override;
+	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
+	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
+	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
 };
 
 #endif // ECL_ROLL_CONTROLLER_H

src/modules/fw_att_control/ecl_wheel_controller.cpp

@@ -46,7 +46,7 @@
 
 using matrix::wrap_pi;
 
-float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
@@ -56,25 +56,13 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
-	/* get the usual dt estimate */
-	uint64_t dt_micros = hrt_elapsed_time(&_last_run);
-	_last_run = hrt_absolute_time();
-	float dt = (float)dt_micros * 1e-6f;
-
-	/* lock integral for long intervals */
-	bool lock_integrator = ctl_data.lock_integrator;
-
-	if (dt_micros > 500000) {
-		lock_integrator = true;
-	}
-
 	/* input conditioning */
 	float min_speed = 1.0f;
 
 	/* Calculate body angular rate error */
 	_rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error
 
-	if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
+	if (!ctl_data.lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
 
 		float id = _rate_error * dt * ctl_data.groundspeed_scaler;
 
@@ -101,7 +89,7 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
-float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
+float ECL_WheelController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&

src/modules/fw_att_control/ecl_wheel_controller.h

@@ -58,11 +58,11 @@ public:
 	ECL_WheelController() = default;
 	~ECL_WheelController() = default;
 
-	float control_attitude(const struct ECL_ControlData &ctl_data) override;
+	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
 
-	float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
+	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
 
-	float control_euler_rate(const struct ECL_ControlData &ctl_data) override { (void)ctl_data; return 0; }
+	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override { (void)ctl_data; return 0; }
 };
 
 #endif // ECL_HEADING_CONTROLLER_H

src/modules/fw_att_control/ecl_yaw_controller.cpp

@@ -43,9 +43,8 @@
 #include <lib/ecl/geo/geo.h>
 #include <mathlib/mathlib.h>
 
-float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
+float ECL_YawController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
 {
-
 	/* Do not calculate control signal with bad inputs */
 	if (!(PX4_ISFINITE(ctl_data.roll) &&
 	      PX4_ISFINITE(ctl_data.pitch) &&
@@ -95,7 +94,7 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
 	return _rate_setpoint;
 }
 
-float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(PX4_ISFINITE(ctl_data.roll) &&
@@ -110,22 +109,10 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
 
-	/* get the usual dt estimate */
-	uint64_t dt_micros = hrt_elapsed_time(&_last_run);
-	_last_run = hrt_absolute_time();
-	float dt = (float)dt_micros * 1e-6f;
-
-	/* lock integral for long intervals */
-	bool lock_integrator = ctl_data.lock_integrator;
-
-	if (dt_micros > 500000) {
-		lock_integrator = true;
-	}
-
 	/* Calculate body angular rate error */
 	_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate;
 
-	if (!lock_integrator && _k_i > 0.0f) {
+	if (!ctl_data.lock_integrator && _k_i > 0.0f) {
 
 		/* Integral term scales with 1/IAS^2 */
 		float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
@@ -155,7 +142,7 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
-float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data)
+float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
 {
 	/* Transform setpoint to body angular rates (jacobian) */
 	_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
@@ -163,5 +150,5 @@ float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_da
 
 	set_bodyrate_setpoint(_bodyrate_setpoint);
 
-	return control_bodyrate(ctl_data);
+	return control_bodyrate(dt, ctl_data);
 }

src/modules/fw_att_control/ecl_yaw_controller.h

@@ -58,9 +58,9 @@ public:
 	ECL_YawController() = default;
 	~ECL_YawController() = default;
 
-	float control_attitude(const struct ECL_ControlData &ctl_data) override;
+	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
+	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override;
-	float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
+	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override;
 
 protected:
 	float _max_rate{0.0f};