mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: always provide delta_velocity and delta_angles
this makes the NavEKF code simpler
This commit is contained in:
parent
a115182041
commit
bc0ae630a1
|
@ -1301,6 +1301,51 @@ check_sample:
|
|||
_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
|
||||
*/
|
||||
|
|
|
@ -118,23 +118,14 @@ 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 {
|
||||
if(_delta_angle_valid[i]) delta_angle = _delta_angle[i];
|
||||
return _delta_angle_valid[i];
|
||||
}
|
||||
|
||||
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); }
|
||||
|
||||
//get delta velocity if available
|
||||
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(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 {
|
||||
return _delta_velocity_dt[i];
|
||||
}
|
||||
float get_delta_velocity_dt(uint8_t i) const;
|
||||
float get_delta_velocity_dt() const { return get_delta_velocity_dt(_primary_accel); }
|
||||
|
||||
/// Fetch the current accelerometer values
|
||||
|
|
Loading…
Reference in New Issue