From b7c8b7d9f1423e8f5465035a083bde58400fdb3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Sep 2012 00:26:55 +0200 Subject: fixed up RC inputs, removed magic numbers, added dead zones, needs testing --- apps/sensors/sensor_params.c | 10 +++++----- apps/sensors/sensors.cpp | 44 ++++++++++++++++++++++++++++++++++++++------ 2 files changed, 43 insertions(+), 11 deletions(-) (limited to 'apps/sensors') diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 370a2e0cc..e6f2f7e76 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -64,35 +64,35 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC1_DZ, 0.1f); +PARAM_DEFINE_FLOAT(RC1_DZ, 30.0f); PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC2_DZ, 0.1f); +PARAM_DEFINE_FLOAT(RC2_DZ, 30.0f); PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC3_DZ, 0.03f); +PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 0.1f); +PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f); PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); PARAM_DEFINE_FLOAT(RC5_MAX, 2000); PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC5_DZ, 0.1f); +PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC6_MIN, 1000); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 7d72fb85d..de481eb1f 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -179,6 +179,8 @@ private: float trim[_rc_max_chan_count]; float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; + float dz[_rc_max_chan_count]; + float ex[_rc_max_chan_count]; float gyro_offset[3]; float mag_offset[3]; @@ -205,6 +207,8 @@ private: param_t trim[_rc_max_chan_count]; param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; + param_t dz[_rc_max_chan_count]; + param_t ex[_rc_max_chan_count]; param_t rc_type; param_t gyro_offset[3]; @@ -358,10 +362,6 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) { - _parameter_handles.min[0] = param_find("RC1_MIN"); - _parameter_handles.max[0] = param_find("RC1_MAX"); - _parameter_handles.trim[0] = param_find("RC1_TRIM"); - _parameter_handles.rev[0] = param_find("RC1_REV"); /* basic r/c parameters */ for (unsigned i = 1; i < _rc_max_chan_count; i++) { @@ -382,6 +382,14 @@ Sensors::Sensors() : /* channel reverse */ sprintf(nbuf, "RC%d_REV", i + 1); _parameter_handles.rev[i] = param_find(nbuf); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles.dz[i] = param_find(nbuf); + + /* channel exponential gain */ + sprintf(nbuf, "RC%d_EXP", i + 1); + _parameter_handles.ex[i] = param_find(nbuf); } _parameter_handles.rc_type = param_find("RC_TYPE"); @@ -464,6 +472,12 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) { warnx("Failed getting rev for chan %d", i); } + if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) { + warnx("Failed getting dead zone for chan %d", i); + } + if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) { + warnx("Failed getting exponential gain for chan %d", i); + } _rc.chan[i].scaling_factor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); @@ -940,11 +954,29 @@ Sensors::ppm_poll() _ppm_last_valid = ppm_last_valid_decode; /* Read out values from HRT */ - for (unsigned int i = 0; i < ppm_decoded_channels; i++) { + for (unsigned int i = 0; (i < ppm_decoded_channels && i < 8); i++) { _rc.chan[i].raw = ppm_buffer[i]; /* Set the range to +-, then scale up */ _rc.chan[i].scale = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor * 10000; - _rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor; + + /* 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])); + } else { + /* in the configured dead zone, output zero */ + _rc.chan[i].scaled = 0.0f; + } + + /* reverse channel if required */ + _rc.chan[i] *= _parameters.rev[i]; + + /* handle any parameter-induced blowups */ + if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) + _rc.chan[i].scaled = 0.0f; + + //_rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor; } _rc.chan_count = ppm_decoded_channels; -- cgit v1.2.3