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

AP_InertialSensor: PX4: don't calculate delta velocity

Browse Source 
Delta velocity calculation will be unified.
This commit is contained in:
Gustavo Jose de Sousa 2015-09-08 11:37:36 -03:00 committed by Andrew Tridgell
parent e502f353c1
commit f769a39449
2 changed files with 3 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -29,8 +29,6 @@ AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
{ {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_delta_angle_accumulator[i].zero(); _delta_angle_accumulator[i].zero();
_delta_velocity_accumulator[i].zero();
_delta_velocity_dt[i] = 0.0f;
} }
} }
@ -218,7 +216,6 @@ bool AP_InertialSensor_PX4::update(void)
// so we only want to do this if we have new data from it // so we only want to do this if we have new data from it
if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) { if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
_publish_accel(_accel_instance[k], accel); _publish_accel(_accel_instance[k], accel);
_publish_delta_velocity(_accel_instance[k], _delta_velocity_accumulator[k], _delta_velocity_dt[k]);
_last_accel_update_timestamp[k] = _last_accel_timestamp[k]; _last_accel_update_timestamp[k] = _last_accel_timestamp[k];
} }
} }
@ -236,8 +233,6 @@ bool AP_InertialSensor_PX4::update(void)
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_delta_angle_accumulator[i].zero(); _delta_angle_accumulator[i].zero();
_delta_velocity_accumulator[i].zero();
_delta_velocity_dt[i] = 0.0f;
} }
if (_last_accel_filter_hz != _accel_filter_cutoff()) { if (_last_accel_filter_hz != _accel_filter_cutoff()) {
@ -265,16 +260,6 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
// apply filter for control path // apply filter for control path
_accel_in[i] = _accel_filter[i].apply(accel); _accel_in[i] = _accel_filter[i].apply(accel);
// get time since last sample
float dt = _accel_sample_time[i];
// compute delta velocity
Vector3f delVel = Vector3f(accel.x, accel.y, accel.z) * dt;
// integrate delta velocity accumulator
_delta_velocity_accumulator[i] += delVel;
_delta_velocity_dt[i] += dt;
// save last timestamp // save last timestamp
_last_accel_timestamp[i] = accel_report.timestamp; _last_accel_timestamp[i] = accel_report.timestamp;
@ -285,6 +270,9 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
_publish_temperature(frontend_instance, accel_report.temperature); _publish_temperature(frontend_instance, accel_report.temperature);
#ifdef AP_INERTIALSENSOR_PX4_DEBUG #ifdef AP_INERTIALSENSOR_PX4_DEBUG
// get time since last sample
float dt = _accel_sample_time[i];
_accel_dt_max[i] = max(_accel_dt_max[i],dt); _accel_dt_max[i] = max(_accel_dt_max[i],dt);
_accel_meas_count[i] ++; _accel_meas_count[i] ++;

2
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -79,8 +79,6 @@ private:
LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES]; LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES];
Vector3f _delta_angle_accumulator[INS_MAX_INSTANCES]; 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_delAng[INS_MAX_INSTANCES];
Vector3f _last_gyro[INS_MAX_INSTANCES]; Vector3f _last_gyro[INS_MAX_INSTANCES];