diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index b23c1ef381..cb691f5f89 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1272,7 +1272,12 @@ check_sample: } now = hal.scheduler->micros(); - _delta_time = (now - _last_sample_usec) * 1.0e-6f; + if (_hil_mode && _hil.delta_time > 0) { + _delta_time = _hil.delta_time; + _hil.delta_time = 0; + } else { + _delta_time = (now - _last_sample_usec) * 1.0e-6f; + } _last_sample_usec = now; #if 0 @@ -1336,6 +1341,37 @@ void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro) } } +/* + set delta time for next ins.update() + */ +void AP_InertialSensor::set_delta_time(float delta_time) +{ + _hil.delta_time = delta_time; +} + +/* + set delta velocity for next update + */ +void AP_InertialSensor::set_delta_velocity(uint8_t instance, float deltavt, const Vector3f &deltav) +{ + if (instance < INS_MAX_INSTANCES) { + _delta_velocity_valid[instance] = true; + _delta_velocity[instance] = deltav; + _delta_velocity_dt[instance] = deltavt; + } +} + +/* + set delta angle for next update + */ +void AP_InertialSensor::set_delta_angle(uint8_t instance, const Vector3f &deltaa) +{ + if (instance < INS_MAX_INSTANCES) { + _delta_angle_valid[instance] = true; + _delta_angle[instance] = deltaa; + } +} + #if INS_VIBRATION_CHECK // calculate vibration levels and check for accelerometer clipping (called by a backends) void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index c10a785cc9..6e61b83de1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -112,7 +112,6 @@ public: /// const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; } const Vector3f &get_gyro(void) const { return get_gyro(_primary_gyro); } - void set_gyro(uint8_t instance, const Vector3f &gyro); // set gyro offsets in radians/sec const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; } @@ -144,7 +143,6 @@ public: /// const Vector3f &get_accel(uint8_t i) const { return _accel[i]; } const Vector3f &get_accel(void) const { return get_accel(_primary_accel); } - void set_accel(uint8_t instance, const Vector3f &accel); uint32_t get_gyro_error_count(uint8_t i) const { return _gyro_error_count[i]; } uint32_t get_accel_error_count(uint8_t i) const { return _accel_error_count[i]; } @@ -233,6 +231,17 @@ public: uint32_t get_accel_clip_count(uint8_t instance) const; #endif + /* + HIL set functions. The minimum for HIL is set_accel() and + set_gyro(). The others are option for higher fidelity log + playback + */ + void set_accel(uint8_t instance, const Vector3f &accel); + void set_gyro(uint8_t instance, const Vector3f &gyro); + void set_delta_time(float delta_time); + void set_delta_velocity(uint8_t instance, float deltavt, const Vector3f &deltav); + void set_delta_angle(uint8_t instance, const Vector3f &deltaa); + private: // load backend drivers @@ -347,6 +356,13 @@ private: LowPassFilterVector3f _accel_vibe_filter; #endif + /* + state for HIL support + */ + struct { + float delta_time; + } _hil {}; + DataFlash_Class *_dataflash; };