aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: add get_vel_correction_for_sensor_offset

Browse Source
This commit is contained in:
Randy Mackay 2020-05-19 14:05:48 +09:00
parent 9b480ca755
commit 4639e8a698
2 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Math/AP_Math.cpp
View File

@ -338,6 +338,22 @@ bool rotation_equal(enum Rotation r1, enum Rotation r2)
return (v1 - v2).length() < 0.001;
}
/*
* return a velocity correction (in m/s in NED) for a sensor's position given it's position offsets
* sensor_offset_bf is in meters in body frame (Foward, Right, Down)
* rot_ef_to_bf is a rotation matrix to rotate from earth-frame (NED) to body frame
* angular_rate is rad/sec
*/
Vector3f get_vel_correction_for_sensor_offset(const Vector3f &sensor_offset_bf, const Matrix3f &rot_ef_to_bf, const Vector3f &angular_rate)
{
if (sensor_offset_bf.is_zero()) {
return Vector3f();
}
// correct velocity
const Vector3f vel_offset_body = angular_rate % sensor_offset_bf;
return rot_ef_to_bf.mul_transpose(vel_offset_body) * -1.0f;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// fill an array of float with NaN, used to invalidate memory in SITL

8
libraries/AP_Math/AP_Math.h
View File

@ -277,6 +277,14 @@ Vector3f rand_vec3f(void);
// return true if two rotations are equal
bool rotation_equal(enum Rotation r1, enum Rotation r2) WARN_IF_UNUSED;
/*
* return a velocity correction (in m/s in NED) for a sensor's position given it's position offsets
* sensor_offset_bf is in meters in body frame (Foward, Right, Down)
* rot_ef_to_bf is a rotation matrix to rotate from earth-frame (NED) to body frame
* angular_rate is rad/sec
*/
Vector3f get_vel_correction_for_sensor_offset(const Vector3f &sensor_offset_bf, const Matrix3f &rot_ef_to_bf, const Vector3f &angular_rate);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// fill an array of float with NaN, used to invalidate memory in SITL
void fill_nanf(float *f, uint16_t count);