Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: always provide delta_velocity and delta_angles 

this makes the NavEKF code simpler
This commit is contained in:
Andrew Tridgell 2015-06-17 13:01:51 +10:00
parent a115182041
commit bc0ae630a1
2 changed files with 48 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1301,6 +1301,51 @@ check_sample:
_have_sample = true; _have_sample = true;
} }
/*
get delta angles
*/
bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle) const
{
if (_delta_angle_valid[i]) {
delta_angle = _delta_angle[i];
return true;
} else if (get_gyro_health(i)) {
// provide delta angle from raw gyro, so we use the same code
// at higher level
delta_angle = get_gyro(i) * get_delta_time();
return true;
}
return false;
}
/*
get delta velocity if available
*/
bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const
{
if (_delta_velocity_valid[i]) {
delta_velocity = _delta_velocity[i];
return true;
} else if (get_accel_health(i)) {
delta_velocity = get_accel(i) * get_delta_time();
return true;
}
return false;
}
/*
return delta_time for the delta_velocity
*/
float AP_InertialSensor::get_delta_velocity_dt(uint8_t i) const
{
if (_delta_velocity_valid[i]) {
return _delta_velocity_dt[i];
}
return get_delta_time();
}
/* /*
support for setting accel and gyro vectors, for use by HIL support for setting accel and gyro vectors, for use by HIL
*/ */

15
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -118,23 +118,14 @@ public:
const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); } const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); }
//get delta angle if available //get delta angle if available
bool get_delta_angle(uint8_t i, Vector3f &delta_angle) const { bool get_delta_angle(uint8_t i, Vector3f &delta_angle) const;
if(_delta_angle_valid[i]) delta_angle = _delta_angle[i];
return _delta_angle_valid[i];
}
bool get_delta_angle(Vector3f &delta_angle) const { return get_delta_angle(_primary_gyro, delta_angle); } bool get_delta_angle(Vector3f &delta_angle) const { return get_delta_angle(_primary_gyro, delta_angle); }
//get delta velocity if available //get delta velocity if available
bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const { bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const;
if(_delta_velocity_valid[i]) delta_velocity = _delta_velocity[i];
return _delta_velocity_valid[i];
}
bool get_delta_velocity(Vector3f &delta_velocity) const { return get_delta_velocity(_primary_accel, delta_velocity); } 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(uint8_t i) const;
return _delta_velocity_dt[i];
}
float get_delta_velocity_dt() const { return get_delta_velocity_dt(_primary_accel); } float get_delta_velocity_dt() const { return get_delta_velocity_dt(_primary_accel); }
/// Fetch the current accelerometer values /// Fetch the current accelerometer values