diff options
Diffstat (limited to 'src/modules/sensors/sensors.cpp')
-rw-r--r-- | src/modules/sensors/sensors.cpp | 27 |
1 files changed, 24 insertions, 3 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b268b1b36..8cfe35c42 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,7 +229,7 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ - + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -252,6 +252,8 @@ private: int board_rotation; int external_mag_rotation; + + float board_offset[3]; int rc_map_roll; int rc_map_pitch; @@ -341,6 +343,8 @@ private: param_t board_rotation; param_t external_mag_rotation; + + param_t board_offset[3]; } _parameter_handles; /**< handles for interesting parameters */ @@ -587,6 +591,11 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + + /* rotation offsets */ + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -640,7 +649,7 @@ Sensors::parameters_update() if (!isfinite(tmpScaleFactor) || (tmpRevFactor < 0.000001f) || (tmpRevFactor > 0.2f)) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0.0f; rc_valid = false; @@ -791,6 +800,18 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); + param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); + param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); + + /** fine tune board offset on parameter update **/ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); + + _board_rotation = _board_rotation * board_rotation_offset; return OK; } @@ -1294,7 +1315,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { + if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor |