diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-12 10:34:49 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-12 10:34:49 +0200 |
commit | cb57fdb28c35f6c472a6b21bc66e96cbf5d45c82 (patch) | |
tree | 4f431ffe6b7b33b43bfbafa34ce2f96189627658 /apps/sensors | |
parent | b7c8b7d9f1423e8f5465035a083bde58400fdb3b (diff) | |
download | px4-firmware-cb57fdb28c35f6c472a6b21bc66e96cbf5d45c82.tar.gz px4-firmware-cb57fdb28c35f6c472a6b21bc66e96cbf5d45c82.tar.bz2 px4-firmware-cb57fdb28c35f6c472a6b21bc66e96cbf5d45c82.zip |
Added ground estimator, fixed RC calibration
Diffstat (limited to 'apps/sensors')
-rw-r--r-- | apps/sensors/sensors.cpp | 20 |
1 files changed, 11 insertions, 9 deletions
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index de481eb1f..3adf280e5 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -195,9 +195,9 @@ private: int rc_map_throttle; int rc_map_mode_sw; - int rc_scale_roll; - int rc_scale_pitch; - int rc_scale_yaw; + float rc_scale_roll; + float rc_scale_pitch; + float rc_scale_yaw; float battery_voltage_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -960,17 +960,19 @@ Sensors::ppm_poll() _rc.chan[i].scale = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor * 10000; /* scale around the mid point differently for lower and upper range */ - if (ppm_buffer[i] > _rc.chan[i].mid + _parameters.dz[i]) { - _rc.chan[i].scaled = ((ppm_buffer[i] - _parameters.trim[i]) / (_parameters.max[i] - _parameters.trim[i])); - } else if ((ppm_buffer[i] < _rc_chan[i].mid - _parameters.dz[i])) { - _rc.chan[i].scaled = -1.0 + ((ppm_buffer[i] - _parameters.min[i]) / (_parameters.trim[i] - _parameters.min[i])); + if (ppm_buffer[i] > (_parameters.trim[i] + _parameters.dz[i])) { + _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + } else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) { + /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ + _rc.chan[i].scaled = -1.0 + ((ppm_buffer[i] - _parameters.min[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + } else { /* in the configured dead zone, output zero */ _rc.chan[i].scaled = 0.0f; } /* reverse channel if required */ - _rc.chan[i] *= _parameters.rev[i]; + _rc.chan[i].scaled *= _parameters.rev[i]; /* handle any parameter-induced blowups */ if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) @@ -1001,7 +1003,7 @@ Sensors::ppm_poll() if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled/2.0f; + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; |