diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-05 16:04:45 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-05 16:04:45 +0100 |
commit | e7f2c053c241849e3ea035fcd22a0e29945b3415 (patch) | |
tree | 704d9d21d46ba670ef1ec0286f24aa272e6cca25 /apps/sensors | |
parent | 39659e57f817c4f49be595d6cecf05e67e2d89db (diff) | |
download | px4-firmware-e7f2c053c241849e3ea035fcd22a0e29945b3415.tar.gz px4-firmware-e7f2c053c241849e3ea035fcd22a0e29945b3415.tar.bz2 px4-firmware-e7f2c053c241849e3ea035fcd22a0e29945b3415.zip |
Quickly separated low-level raw RC from mapped / scaled RC, supports FMU PPM and IO PPM / Spektrum now
Diffstat (limited to 'apps/sensors')
-rw-r--r-- | apps/sensors/sensors.cpp | 198 |
1 files changed, 115 insertions, 83 deletions
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 453502b05..3e9f35eaf 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -58,11 +58,13 @@ #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> #include <drivers/drv_baro.h> +#include <drivers/drv_rc_input.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/perf_counter.h> + #include <systemlib/ppm_decode.h> #include <uORB/uORB.h> @@ -143,6 +145,7 @@ private: int _gyro_sub; /**< raw gyro data subscription */ int _accel_sub; /**< raw accel data subscription */ int _mag_sub; /**< raw mag data subscription */ + int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -162,6 +165,7 @@ private: float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; float ex[_rc_max_chan_count]; + float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; float mag_offset[3]; @@ -331,6 +335,7 @@ Sensors::Sensors() : _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), + _rc_sub(-1), _baro_sub(-1), _vstatus_sub(-1), _params_sub(-1), @@ -464,14 +469,13 @@ Sensors::parameters_update() 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]); + _parameters.scaling_factor[i] = (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; + if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) { + _parameters.scaling_factor[i] = 0; } - _rc.chan[i].mid = _parameters.trim[i]; } /* update RC function mappings */ @@ -856,98 +860,125 @@ Sensors::adc_poll(struct sensor_combined_s &raw) void Sensors::ppm_poll() { - struct manual_control_setpoint_s manual_control; - - /* check to see whether a new frame has been decoded */ - if (_ppm_last_valid == ppm_last_valid_decode) - return; - /* require at least two chanels to consider the signal valid */ - if (ppm_decoded_channels < 4) - return; - - unsigned channel_limit = ppm_decoded_channels; - if (channel_limit > _rc_max_chan_count) - channel_limit = _rc_max_chan_count; - - /* we are accepting this decode */ - _ppm_last_valid = ppm_last_valid_decode; - - /* Read out values from HRT */ - for (unsigned int i = 0; i < channel_limit; i++) { - _rc.chan[i].raw = ppm_buffer[i]; - - /* scale around the mid point differently for lower and upper range */ - 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 = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); - - } else { - /* in the configured dead zone, output zero */ - _rc.chan[i].scaled = 0.0f; + /* fake low-level driver, directly pulling from driver variables */ + static orb_advert_t rc_input_pub = -1; + struct rc_input_values raw; + + raw.timestamp = ppm_last_valid_decode; + + if (ppm_decoded_channels > 1) { + + for (int i = 0; i < ppm_decoded_channels; i++) { + raw.values[i] = ppm_buffer[i]; } - /* reverse channel if required */ - if (i == _rc.function[THROTTLE]) { - if ((int)_parameters.rev[i] == -1) { - _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; - } + raw.channel_count = ppm_decoded_channels; + + /* publish to object request broker */ + if (rc_input_pub <= 0) { + rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw); } else { - _rc.chan[i].scaled *= _parameters.rev[i]; + orb_publish(ORB_ID(input_rc), rc_input_pub, &raw); } + } - /* 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; - } + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - _rc.chan_count = ppm_decoded_channels; - _rc.timestamp = ppm_last_valid_decode; + if (rc_updated) { + struct rc_input_values rc_input; - manual_control.timestamp = ppm_last_valid_decode; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; - if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; - if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { - manual_control.roll *= _parameters.rc_scale_roll; - } + struct manual_control_setpoint_s manual_control; - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; - if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; - if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + /* require at least two chanels to consider the signal valid */ + if (rc_input.channel_count < 2) + return; - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; - if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; - if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; - if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* throttle input */ - 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; + unsigned channel_limit = rc_input.channel_count; + if (channel_limit > _rc_max_chan_count) + channel_limit = _rc_max_chan_count; - /* mode switch input */ - manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; - if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; - if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + /* we are accepting this message */ + _ppm_last_valid = rc_input.timestamp; - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + /* Read out values from raw message */ + for (unsigned int i = 0; i < channel_limit; i++) { + + /* scale around the mid point differently for lower and upper range */ + if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + } else if (rc_input.values[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 = -((_parameters.trim[i] - rc_input.values[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 */ + if (i == _rc.function[THROTTLE]) { + if ((int)_parameters.rev[i] == -1) { + _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; + } + } else { + _rc.chan[i].scaled *= _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_count = rc_input.channel_count; + _rc.timestamp = rc_input.timestamp; + + manual_control.timestamp = rc_input.timestamp; + + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; + if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; + if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; + if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { + manual_control.roll *= _parameters.rc_scale_roll; + } + + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; + if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; + if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; + if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } + + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; + if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; + if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; + if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { + manual_control.yaw *= _parameters.rc_scale_yaw; + } + + /* throttle input */ + 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; + + /* mode switch input */ + manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; + if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; + if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + } } #endif @@ -979,6 +1010,7 @@ Sensors::task_main() _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); |