diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index ff1afcd135..f4d6238a49 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -358,6 +358,7 @@ AP_InertialSensor::AP_InertialSensor() : _delta_velocity_acc_dt[i] = 0; _delta_angle_acc[i].zero(); + _delta_angle_acc_dt[i] = 0; _last_delta_angle[i].zero(); _last_raw_gyro[i].zero(); } @@ -911,6 +912,7 @@ void AP_InertialSensor::update(void) _delta_velocity_acc[i].zero(); _delta_velocity_acc_dt[i] = 0; _delta_angle_acc[i].zero(); + _delta_angle_acc_dt[i] = 0; } // adjust health status if a sensor has a non-zero error count @@ -1106,6 +1108,17 @@ float AP_InertialSensor::get_delta_velocity_dt(uint8_t i) const return get_delta_time(); } +/* + return delta_time for the delta_angle + */ +float AP_InertialSensor::get_delta_angle_dt(uint8_t i) const +{ + if (_delta_angle_valid[i]) { + return _delta_angle_dt[i]; + } + return get_delta_time(); +} + /* support for setting accel and gyro vectors, for use by HIL diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index f299203dcb..fff8ba44d1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -99,6 +99,9 @@ public: bool get_delta_angle(uint8_t i, Vector3f &delta_angle) const; bool get_delta_angle(Vector3f &delta_angle) const { return get_delta_angle(_primary_gyro, delta_angle); } + float get_delta_angle_dt(uint8_t i) const; + float get_delta_angle_dt() const { return get_delta_angle_dt(_primary_accel); } + //get delta velocity if available bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const; bool get_delta_velocity(Vector3f &delta_velocity) const { return get_delta_velocity(_primary_accel, delta_velocity); } @@ -302,7 +305,10 @@ private: // Most recent gyro reading Vector3f _gyro[INS_MAX_INSTANCES]; Vector3f _delta_angle[INS_MAX_INSTANCES]; + float _delta_angle_dt[INS_MAX_INSTANCES]; bool _delta_angle_valid[INS_MAX_INSTANCES]; + // time accumulator for delta angle accumulator + float _delta_angle_acc_dt[INS_MAX_INSTANCES]; Vector3f _delta_angle_acc[INS_MAX_INSTANCES]; Vector3f _last_delta_angle[INS_MAX_INSTANCES]; Vector3f _last_raw_gyro[INS_MAX_INSTANCES]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 8e7355e10a..fbe54e5b31 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -54,6 +54,7 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f & // publish delta angle _imu._delta_angle[instance] = _imu._delta_angle_acc[instance]; + _imu._delta_angle_dt[instance] = _imu._delta_angle_acc_dt[instance]; _imu._delta_angle_valid[instance] = true; } @@ -87,6 +88,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, // referenced paper, but in simulation little difference was found between // integrating together and integrating separately (see examples/coning.py) _imu._delta_angle_acc[instance] += delta_angle + delta_coning; + _imu._delta_angle_acc_dt[instance] += dt; // save previous delta angle for coning correction _imu._last_delta_angle[instance] = delta_angle;