diff options
author | px4dev <px4@purgatory.org> | 2012-08-28 21:13:00 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-28 21:13:00 -0700 |
commit | 1e90fd5bec19aa8e892a5bef2579bd47c192e317 (patch) | |
tree | dee8854c55a3e83f4a02894c30bf3ac1bb0a6abd /apps/sensors/sensors.cpp | |
parent | 68ac20cc3a5c09379023b750ac6ddbe8bde8bbf1 (diff) | |
download | px4-firmware-1e90fd5bec19aa8e892a5bef2579bd47c192e317.tar.gz px4-firmware-1e90fd5bec19aa8e892a5bef2579bd47c192e317.tar.bz2 px4-firmware-1e90fd5bec19aa8e892a5bef2579bd47c192e317.zip |
Let's not leave the R/C channel scaling factor as a NAN or INF. It makes many things sad.
Also, clean up the calculation of same. Really, is it easier to type out the same calculation 8 times, or perhaps you might be interested in this thing we call a 'loop'…
Diffstat (limited to 'apps/sensors/sensors.cpp')
-rw-r--r-- | apps/sensors/sensors.cpp | 66 |
1 files changed, 22 insertions, 44 deletions
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 4ecd40671..205f88093 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -49,6 +49,7 @@ #include <stdbool.h> #include <stdio.h> #include <errno.h> +#include <math.h> #include <arch/board/up_hrt.h> @@ -431,34 +432,39 @@ Sensors::parameters_update() { const unsigned int nchans = 8; - /* min values */ + /* rc values */ for (unsigned int i = 0; i < nchans; i++) { + if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) { warnx("Failed getting min for chan %d", i); } - } - - /* trim values */ - for (unsigned int i = 0; i < nchans; i++) { if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) { warnx("Failed getting trim for chan %d", i); } - } - - /* max values */ - for (unsigned int i = 0; i < nchans; i++) { if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) { warnx("Failed getting max for chan %d", i); } - } - - /* channel reverse */ - for (unsigned int i = 0; i < nchans; i++) { if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) { warnx("Failed getting rev for chan %d", i); } + + _rc.chan[i].scaling_factor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + + /* handle blowup in the scaling factor calculation */ + if (isnan(_rc.chan[i].scaling_factor) || isinf(_rc.chan[i].scaling_factor)) { + _rc.chan[i].scaling_factor = 0; + } + + _rc.chan[i].mid = _parameters.trim[i]; } + /* update RC function mappings */ + _rc.function[0] = _parameters.rc_map_throttle - 1; + _rc.function[1] = _parameters.rc_map_roll - 1; + _rc.function[2] = _parameters.rc_map_pitch - 1; + _rc.function[3] = _parameters.rc_map_yaw - 1; + _rc.function[4] = _parameters.rc_map_mode_sw - 1; + /* remote control type */ if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { warnx("Failed getting remote control type"); @@ -763,8 +769,10 @@ void Sensors::parameter_update_poll(bool forced) { bool param_updated; + /* Check if any parameter has changed */ orb_check(_params_sub, ¶m_updated); + if (param_updated || forced) { /* read from param to clear updated flag */ @@ -772,6 +780,7 @@ Sensors::parameter_update_poll(bool forced) orb_copy(ORB_ID(parameter_update), _params_sub, &update); printf("PARAMS UPDATED\n"); + /* update parameters */ parameters_update(); @@ -815,37 +824,6 @@ Sensors::parameter_update_poll(bool forced) warn("WARNING: failed to set scale / offsets for mag"); close(fd); - /* update RC scalings and function mappings */ - _rc.chan[0].scaling_factor = (1.0f / ((_parameters.max[0] - _parameters.min[0]) / 2.0f) * _parameters.rev[0]); - _rc.chan[0].mid = _parameters.trim[0]; - - _rc.chan[1].scaling_factor = (1.0f / ((_parameters.max[1] - _parameters.min[1]) / 2.0f) * _parameters.rev[1]); - _rc.chan[1].mid = _parameters.trim[1]; - - _rc.chan[2].scaling_factor = (1.0f / ((_parameters.max[2] - _parameters.min[2]) / 2.0f) * _parameters.rev[2]); - _rc.chan[2].mid = _parameters.trim[2]; - - _rc.chan[3].scaling_factor = (1.0f / ((_parameters.max[3] - _parameters.min[3]) / 2.0f) * _parameters.rev[3]); - _rc.chan[3].mid = _parameters.trim[3]; - - _rc.chan[4].scaling_factor = (1.0f / ((_parameters.max[4] - _parameters.min[4]) / 2.0f) * _parameters.rev[4]); - _rc.chan[4].mid = _parameters.trim[4]; - - _rc.chan[5].scaling_factor = (1.0f / ((_parameters.max[5] - _parameters.min[5]) / 2.0f) * _parameters.rev[5]); - _rc.chan[5].mid = _parameters.trim[5]; - - _rc.chan[6].scaling_factor = (1.0f / ((_parameters.max[6] - _parameters.min[6]) / 2.0f) * _parameters.rev[6]); - _rc.chan[6].mid = _parameters.trim[6]; - - _rc.chan[7].scaling_factor = (1.0f / ((_parameters.max[7] - _parameters.min[7]) / 2.0f) * _parameters.rev[7]); - _rc.chan[7].mid = _parameters.trim[7]; - - _rc.function[0] = _parameters.rc_map_throttle - 1; - _rc.function[1] = _parameters.rc_map_roll - 1; - _rc.function[2] = _parameters.rc_map_pitch - 1; - _rc.function[3] = _parameters.rc_map_yaw - 1; - _rc.function[4] = _parameters.rc_map_mode_sw - 1; - #if 1 printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor*10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]); printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor*10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]); |