WIP on controllers

3 files changed, 13 insertions, 1 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 0faba287d..c1c2bc5af 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -51,6 +51,12 @@ ECL_PitchController::ECL_PitchController() :
 float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
 				   bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed)
 {
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+
+	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+
 	return 0.0f;
 }
 
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index b28ecdabe..cdb221b26 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -61,7 +61,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
 
 
-
+	float integrator_limit_scaled = 0.0f;
 
 	return 0.0f;
 }
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 0d8a0513f..6dd90f676 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -54,6 +54,12 @@ ECL_YawController::ECL_YawController() :
 float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
 				 float airspeed_min, float airspeed_max, float aspeed)
 {
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+
+	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+
 	return 0.0f;
 }