Browse Source

df_lsm9ds1_wrapper: astyle

sbg
Julian Oes 9 years ago committed by Lorenz Meier
parent
commit
4c0ed8bdd5
  1. 82
      src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp

82
src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp

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

Loading…
Cancel
Save