dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

df_lsm9ds1_wrapper: astyle

This commit is contained in:
Julian Oes 2016-06-24 17:05:27 +02:00 committed by Lorenz Meier
parent ef729ab2d8
commit 4c0ed8bdd5
1 changed files with 45 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
View File

@ -109,11 +109,11 @@ private:
void _update_accel_calibration();
void _update_gyro_calibration();
void _update_mag_calibration();
void _update_mag_calibration();
orb_advert_t _accel_topic;
orb_advert_t _gyro_topic;
orb_advert_t _mag_topic;
orb_advert_t _mag_topic;
orb_advert_t _mavlink_log_pub;
int _param_update_sub;
@ -144,7 +144,7 @@ private:
float z_offset;
float z_scale;
} _mag_calibration;
math::Matrix<3, 3> _rotation_matrix;
int _accel_orb_class_instance;
@ -174,15 +174,15 @@ DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) :
LSM9DS1(IMU_DEVICE_ACC_GYRO, IMU_DEVICE_MAG),
_accel_topic(nullptr),
_gyro_topic(nullptr),
_mag_topic(nullptr),
_mag_topic(nullptr),
_mavlink_log_pub(nullptr),
_param_update_sub(-1),
_accel_calibration{},
_gyro_calibration{},
_mag_calibration{},
_mag_calibration{},
_accel_orb_class_instance(-1),
_gyro_orb_class_instance(-1),
_mag_orb_class_instance(-1),
_mag_orb_class_instance(-1),
_accel_int(LSM9DS1_NEVER_AUTOPUBLISH_US, false),
_gyro_int(LSM9DS1_NEVER_AUTOPUBLISH_US, true),
_publish_count(0),
@ -195,7 +195,7 @@ DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) :
_publish_perf(perf_alloc(PC_ELAPSED, "lsm9ds1_publish")),
_last_accel_range_hit_time(0),
_last_accel_range_hit_count(0),
_mag_enabled(mag_enabled)
_mag_enabled(mag_enabled)
{
// Set sane default calibration values
_accel_calibration.x_scale = 1.0f;
@ -259,18 +259,18 @@ int DfLsm9ds1Wrapper::start()
return -1;
}
if (_mag_enabled) {
mag_report mag_report = {};
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
&_mag_orb_class_instance, ORB_PRIO_DEFAULT);
if (_mag_enabled) {
mag_report mag_report = {};
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
&_mag_orb_class_instance, ORB_PRIO_DEFAULT);
if (_mag_topic == nullptr) {
PX4_ERR("sensor_mag advert fail");
return -1;
}
}
if (_mag_topic == nullptr) {
PX4_ERR("sensor_mag advert fail");
return -1;
}
}
/* Subscribe to param update topic. */
/* Subscribe to param update topic. */
if (_param_update_sub < 0) {
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
}
@ -295,7 +295,7 @@ int DfLsm9ds1Wrapper::start()
/* Force getting the calibration values. */
_update_accel_calibration();
_update_gyro_calibration();
_update_mag_calibration();
_update_mag_calibration();
return 0;
}
@ -625,11 +625,11 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
accel_report accel_report = {};
gyro_report gyro_report = {};
mag_report mag_report = {};
mag_report mag_report = {};
accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
mag_report.timestamp = accel_report.timestamp;
mag_report.timestamp = accel_report.timestamp;
// TODO: get these right
gyro_report.scaling = -1.0f;
gyro_report.range_rad_s = -1.0f;
@ -639,12 +639,13 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
accel_report.range_m_s2 = -1.0f;
accel_report.device_id = m_id.dev_id;
if (_mag_enabled) {
mag_report.scaling = -1.0f;
mag_report.range_ga = -1.0f;
mag_report.device_id = m_id.dev_id;
}
// TODO: remove these (or get the values)
if (_mag_enabled) {
mag_report.scaling = -1.0f;
mag_report.range_ga = -1.0f;
mag_report.device_id = m_id.dev_id;
}
// TODO: remove these (or get the values)
gyro_report.x_raw = NAN;
gyro_report.y_raw = NAN;
gyro_report.z_raw = NAN;
@ -653,11 +654,11 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
accel_report.y_raw = NAN;
accel_report.z_raw = NAN;
if (_mag_enabled) {
mag_report.x_raw = NAN;
mag_report.y_raw = NAN;
mag_report.z_raw = NAN;
}
if (_mag_enabled) {
mag_report.x_raw = NAN;
mag_report.y_raw = NAN;
mag_report.z_raw = NAN;
}
math::Vector<3> gyro_val_filt;
math::Vector<3> accel_val_filt;
@ -675,11 +676,11 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
accel_report.y = accel_val_filt(1);
accel_report.z = accel_val_filt(2);
if (_mag_enabled) {
if (_mag_enabled) {
math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale,
(data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
(data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
(data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
(data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
mag_val = _rotation_matrix * mag_val;
@ -687,7 +688,7 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
mag_report.y = mag_val(1);
mag_report.z = mag_val(2);
}
gyro_report.x_integral = gyro_val_integ(0);
gyro_report.y_integral = gyro_val_integ(1);
gyro_report.z_integral = gyro_val_integ(2);
@ -707,9 +708,10 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
if (_mag_topic != nullptr) {
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
}
if (_mag_topic != nullptr) {
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
}
/* Notify anyone waiting for data. */
DevMgr::updateNotify(*this);
@ -778,7 +780,7 @@ int start(bool mag_enabled, enum Rotation rotation)
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
IMU_DEVICE_MAG, h.getError());
IMU_DEVICE_MAG, h.getError());
return -1;
}
@ -862,9 +864,9 @@ df_lsm9ds1_wrapper_main(int argc, char *argv[])
const char *verb = argv[myoptind];
if (!strcmp(verb, "start_without_mag")) {
ret = df_lsm9ds1_wrapper::start(false, rotation);
}
if (!strcmp(verb, "start_without_mag")) {
ret = df_lsm9ds1_wrapper::start(false, rotation);
}
else if (!strcmp(verb, "start")) {
ret = df_lsm9ds1_wrapper::start(true, rotation);