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

AP_InertialSensor: added get_delta_angle_dt() API

Browse Source
This commit is contained in:
Andrew Tridgell 2016-01-16 15:41:19 +11:00
parent f0d9f91650
commit ee453783eb
3 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

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

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

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

2
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

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