Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

fxos8701cq:Clean up & paramater based low pass fileter

Browse Source 
Added paramater based low pass fileter settting from
      IMU_ACCEL_CUTOFF.

      removed gotos.
This commit is contained in:
David Sidrane
2018-08-22 15:36:45 -07:00
committed by Lorenz Meier
parent 3f65efe1b0
commit 28125aca3d
1 changed files with 20 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/fxos8701cq/fxos8701cq.cpp
+20 -6
View File
@@ -555,20 +555,33 @@ FXOS8701CQ::init()
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
PX4_ERR("SPI init failed");
goto out;
return PX4_ERROR;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
goto out;
return PX4_ERROR;
}
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr) {
goto out;
return PX4_ERROR;
}
// set software low pass filter for controllers
param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
float accel_cut = FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
if (accel_cut_ph != PARAM_INVALID && param_get(accel_cut_ph, &accel_cut) == PX4_OK) {
PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut));
accel_set_driver_lowpass_filter(FXOS8701C_ACCEL_DEFAULT_RATE, accel_cut);
} else {
PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
}
reset();
@@ -578,7 +591,7 @@ FXOS8701CQ::init()
if (ret != OK) {
PX4_ERR("MAG init failed");
goto out;
return PX4_ERROR;
}
/* fill report structures */
@@ -594,6 +607,7 @@ FXOS8701CQ::init()
if (_mag->_mag_topic == nullptr) {
PX4_ERR("ADVERT ERR");
return PX4_ERROR;
}
@@ -609,10 +623,10 @@ FXOS8701CQ::init()
if (_accel_topic == nullptr) {
PX4_ERR("ADVERT ERR");
return PX4_ERROR;
}
out:
return ret;
return PX4_OK;
}