Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: support HIL functions for delta_velocity and delta_angles

Browse Source
This commit is contained in:
Andrew Tridgell 2015-06-15 18:13:06 +10:00
parent f603c1ef69
commit 727be87d84
2 changed files with 55 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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)

20
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -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;
};