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

AP_InertialSensor: publish delta_velocity_dt

This commit is contained in:
Jonathan Challinger 2015-03-21 13:18:58 -07:00
parent 4147825b87
commit 423160eaf8
5 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -125,9 +125,13 @@ public:
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); }
float get_delta_velocity_dt(uint8_t i) const {
return _delta_velocity_dt[i];
}
float get_delta_velocity() const { return get_delta_velocity_dt(_primary_accel); }
/// Fetch the current accelerometer values
///
/// @returns vector of current accelerations in m/s/s
@ -250,6 +254,7 @@ private:
// Most recent accelerometer reading
Vector3f _accel[INS_MAX_INSTANCES];
Vector3f _delta_velocity[INS_MAX_INSTANCES];
float _delta_velocity_dt[INS_MAX_INSTANCES];
bool _delta_velocity_valid[INS_MAX_INSTANCES];
// Most recent gyro reading

3
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -57,10 +57,11 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
}
}
void AP_InertialSensor_Backend::_publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity)
void AP_InertialSensor_Backend::_publish_delta_velocity(uint8_t instance, const Vector3f &delta_velocity, float dt)
{
// publish delta velocity
_imu._delta_velocity[instance] = delta_velocity;
_imu._delta_velocity_dt[instance] = dt;
_imu._delta_velocity_valid[instance] = true;
}

2
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -64,7 +64,7 @@ protected:
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_velocity(uint8_t instance, const Vector3f &delta_velocity, float dt);
void _publish_delta_angle(uint8_t instance, const Vector3f &delta_angle);
// rotate gyro vector, offset and publish

5
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -28,6 +28,7 @@ AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_delta_angle_accumulator[i].zero();
_delta_velocity_accumulator[i].zero();
_delta_velocity_dt[i] = 0.0f;
}
}
@ -210,7 +211,7 @@ bool AP_InertialSensor_PX4::update(void)
// so we only want to do this if we have new data from it
if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
_publish_accel(_accel_instance[k], accel, false);
_publish_delta_velocity(_accel_instance[k], _delta_velocity_accumulator[k]);
_publish_delta_velocity(_accel_instance[k], _delta_velocity_accumulator[k], _delta_velocity_dt[k]);
_last_accel_update_timestamp[k] = _last_accel_timestamp[k];
}
}
@ -229,6 +230,7 @@ bool AP_InertialSensor_PX4::update(void)
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_delta_angle_accumulator[i].zero();
_delta_velocity_accumulator[i].zero();
_delta_velocity_dt[i] = 0.0f;
}
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
@ -263,6 +265,7 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
// integrate delta velocity accumulator
_delta_velocity_accumulator[i] += delVel;
_delta_velocity_dt[i] += dt;
// save last timestamp
_last_accel_timestamp[i] = accel_report.timestamp;

1
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -78,6 +78,7 @@ private:
Vector3f _delta_angle_accumulator[INS_MAX_INSTANCES];
Vector3f _delta_velocity_accumulator[INS_MAX_INSTANCES];
float _delta_velocity_dt[INS_MAX_INSTANCES];
Vector3f _last_delAng[INS_MAX_INSTANCES];
Vector3f _last_gyro[INS_MAX_INSTANCES];