Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: remove separate calls to get delta-times for vel and ang

Browse Source
This commit is contained in:
Peter Barker 2021-02-12 08:34:37 +11:00 committed by Peter Barker
parent a0fcd74b52
commit 2b0bf45891
2 changed files with 24 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1748,8 +1748,15 @@ check_sample:
/*
get delta angles
*/
bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle) const
bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const
{
if (_delta_angle_valid[i] && _delta_angle_dt[i] > 0) {
delta_angle_dt = _delta_angle_dt[i];
} else {
delta_angle_dt = get_delta_time();
}
delta_angle_dt = MIN(delta_angle_dt, _loop_delta_t_max);
if (_delta_angle_valid[i]) {
delta_angle = _delta_angle[i];
return true;
@ -1765,8 +1772,15 @@ bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle) const
/*
get delta velocity if available
*/
bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const
bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const
{
if (_delta_velocity_valid[i]) {
delta_velocity_dt = _delta_velocity_dt[i];
} else {
delta_velocity_dt = get_delta_time();
}
delta_velocity_dt = MIN(delta_velocity_dt, _loop_delta_t_max);
if (_delta_velocity_valid[i]) {
delta_velocity = _delta_velocity[i];
return true;
@ -1777,37 +1791,6 @@ bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity)
return false;
}
/*
return delta_time for the delta_velocity
*/
float AP_InertialSensor::get_delta_velocity_dt(uint8_t i) const
{
float ret;
if (_delta_velocity_valid[i]) {
ret = _delta_velocity_dt[i];
} else {
ret = get_delta_time();
}
ret = MIN(ret, _loop_delta_t_max);
return ret;
}
/*
return delta_time for the delta_angle
*/
float AP_InertialSensor::get_delta_angle_dt(uint8_t i) const
{
float ret;
if (_delta_angle_valid[i] && _delta_angle_dt[i] > 0) {
ret = _delta_angle_dt[i];
} else {
ret = get_delta_time();
}
ret = MIN(ret, _loop_delta_t_max);
return ret;
}
/*
support for setting accel and gyro vectors, for use by HIL
*/

18
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -132,18 +132,16 @@ public:
const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); }
//get delta angle if available
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_gyro); }
bool get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const;
bool get_delta_angle(Vector3f &delta_angle, float &delta_angle_dt) const {
return get_delta_angle(_primary_gyro, delta_angle, delta_angle_dt);
}
//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); }
float get_delta_velocity_dt(uint8_t i) const;
float get_delta_velocity_dt() const { return get_delta_velocity_dt(_primary_accel); }
bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const;
bool get_delta_velocity(Vector3f &delta_velocity, float &delta_velocity_dt) const {
return get_delta_velocity(_primary_accel, delta_velocity, delta_velocity_dt);
}
/// Fetch the current accelerometer values
///