Skip to content

Commit

Permalink
AC_AttitudeControl: record sample time in attitude controller
Browse files Browse the repository at this point in the history
  • Loading branch information
andyp1per committed Sep 4, 2024
1 parent 67f7c6f commit 8bfd640
Showing 1 changed file with 8 additions and 1 deletion.
9 changes: 8 additions & 1 deletion libraries/AC_AttitudeControl/AC_AttitudeControl.h
Original file line number Diff line number Diff line change
Expand Up @@ -73,9 +73,14 @@ class AC_AttitudeControl {

// set_dt / get_dt - dt is the time since the last time the attitude controllers were updated
// _dt should be set based on the time of the last IMU read used by these controllers
// sample_time_us is the time of the last IMU read used by these controllers
// the attitude controller should run updates for active controllers on each loop to ensure normal operation
void set_dt(float dt) { _dt = dt; }
void set_dt(float dt, uint64_t sample_time_us) {
_dt = dt;
_sample_time_us = sample_time_us;
}
float get_dt() const { return _dt; }
uint64_t get_sample_time_us() const { return _sample_time_us; }

// pid accessors
AC_P& get_angle_roll_p() { return _p_angle_roll; }
Expand Down Expand Up @@ -489,6 +494,8 @@ class AC_AttitudeControl {

// Intersampling period in seconds
float _dt;
// scheduler sample time in microseconds (could be different to rate loop)
uint64_t _sample_time_us;

// This represents a 321-intrinsic rotation in NED frame to the target (setpoint)
// attitude used in the attitude controller, in radians.
Expand Down

0 comments on commit 8bfd640

Please sign in to comment.