dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fixed up RC inputs, removed magic numbers, added dead zones, needs testing

This commit is contained in:
Lorenz Meier 2012-09-12 00:26:55 +02:00
parent b573804456
commit b7c8b7d9f1
2 changed files with 43 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
apps/sensors/sensor_params.c
View File

@ -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);

44
apps/sensors/sensors.cpp
View File

@ -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;