Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: allow backends to publish delta velocities and angles

Browse Source
This commit is contained in:
Jonathan Challinger 2015-02-16 22:54:17 -08:00 committed by Andrew Tridgell
parent 155c173ed1
commit 2a547f329b
4 changed files with 70 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -248,6 +248,8 @@ AP_InertialSensor::AP_InertialSensor() :
_accel_error_count[i] = 0;
_gyro_error_count[i] = 0;
}
memset(_delta_velocity_valid,0,sizeof(_delta_velocity_valid));
memset(_delta_angle_valid,0,sizeof(_delta_angle_valid));
}
@ -1036,6 +1038,8 @@ void AP_InertialSensor::update(void)
// _publish_accel()
_gyro_healthy[i] = false;
_accel_healthy[i] = false;
_delta_velocity_valid[i] = false;
_delta_angle_valid[i] = false;
}
for (uint8_t i=0; i<_backend_count; i++) {
_backends[i]->update();

21
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -119,6 +119,23 @@ public:
const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
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(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(Vector3f &delta_velocity) const { return get_delta_velocity(_primary_accel, delta_velocity); }
/// Fetch the current accelerometer values
///
/// @returns vector of current accelerations in m/s/s
@ -239,9 +256,13 @@ private:
// Most recent accelerometer reading
Vector3f _accel[INS_MAX_INSTANCES];
Vector3f _delta_velocity[INS_MAX_INSTANCES];
bool _delta_velocity_valid[INS_MAX_INSTANCES];
// Most recent gyro reading
Vector3f _gyro[INS_MAX_INSTANCES];
Vector3f _delta_angle[INS_MAX_INSTANCES];
bool _delta_angle_valid[INS_MAX_INSTANCES];
// product id
AP_Int16 _product_id;

49
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -9,31 +9,58 @@ AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
_product_id(AP_PRODUCT_ID_NONE)
{}
void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vector3f &accel) {
const Vector3f &accel_scale = _imu._accel_scale[instance].get();
accel.rotate(_imu._board_orientation);
accel.x *= accel_scale.x;
accel.y *= accel_scale.y;
accel.z *= accel_scale.z;
accel -= _imu._accel_offset[instance];
}
void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) {
gyro.rotate(_imu._board_orientation);
gyro -= _imu._gyro_offset[instance];
}
void AP_InertialSensor_Backend::_publish_delta_angle(uint8_t instance, const Vector3f &delta_angle)
{
// publish delta angle
_imu._delta_angle[instance] = delta_angle;
_imu._delta_angle_valid[instance] = true;
}
/*
rotate gyro vector and add the gyro offset
*/
void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro)
void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro, bool rotate_and_correct)
{
_imu._gyro[instance] = gyro;
_imu._gyro[instance].rotate(_imu._board_orientation);
_imu._gyro[instance] -= _imu._gyro_offset[instance];
_imu._gyro_healthy[instance] = true;
if (rotate_and_correct) {
_rotate_and_correct_gyro(instance, _imu._gyro[instance]);
}
}
void AP_InertialSensor_Backend::_publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity)
{
// publish delta velocity
_imu._delta_velocity[instance] = delta_velocity;
_imu._delta_velocity_valid[instance] = true;
}
/*
rotate accel vector, scale and add the accel offset
*/
void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel)
void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel, bool rotate_and_correct)
{
_imu._accel[instance] = accel;
_imu._accel[instance].rotate(_imu._board_orientation);
const Vector3f &accel_scale = _imu._accel_scale[instance].get();
_imu._accel[instance].x *= accel_scale.x;
_imu._accel[instance].y *= accel_scale.y;
_imu._accel[instance].z *= accel_scale.z;
_imu._accel[instance] -= _imu._accel_offset[instance];
_imu._accel_healthy[instance] = true;
if (rotate_and_correct) {
_rotate_and_correct_accel(instance, _imu._accel[instance]);
}
}
// set accelerometer error_count

11
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -61,14 +61,17 @@ protected:
// access to frontend
AP_InertialSensor &_imu;
// rotate gyro vector and offset
void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro);
void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel);
void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro);
void _publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity);
void _publish_delta_angle(uint8_t instance, const Vector3f &delta_angle);
// rotate gyro vector, offset and publish
void _publish_gyro(uint8_t instance, const Vector3f &gyro);
void _publish_gyro(uint8_t instance, const Vector3f &gyro, bool rotate_and_correct = true);
// rotate accel vector, scale, offset and publish
void _publish_accel(uint8_t instance, const Vector3f &accel);
void _publish_accel(uint8_t instance, const Vector3f &accel, bool rotate_and_correct = true);
// set accelerometer error_count
void _set_accel_error_count(uint8_t instance, uint32_t error_count);