aboutsummaryrefslogtreecommitdiff
path: root/src/modules/sensors/sensors.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/sensors/sensors.cpp')
-rw-r--r--src/modules/sensors/sensors.cpp27
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