Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

FMU driver: Make auto-detection of analog RSSI more stable

Browse Source
This commit is contained in:
Lorenz Meier
2016-05-01 16:36:15 +02:00
parent be467f606b
commit 4ec4deabbd
1 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/fmu.cpp
+8 -4
View File
@@ -184,6 +184,7 @@ private:
int _adc_sub; int _adc_sub;
struct rc_input_values _rc_in; struct rc_input_values _rc_in;
float _analog_rc_rssi_volt; float _analog_rc_rssi_volt;
bool _analog_rc_rssi_stable;
orb_advert_t _to_input_rc; orb_advert_t _to_input_rc;
orb_advert_t _outputs_pub; orb_advert_t _outputs_pub;
unsigned _num_outputs; unsigned _num_outputs;
@@ -367,6 +368,7 @@ PX4FMU::PX4FMU() :
_adc_sub(-1), _adc_sub(-1),
_rc_in{}, _rc_in{},
_analog_rc_rssi_volt(-1.0f), _analog_rc_rssi_volt(-1.0f),
_analog_rc_rssi_stable(false),
_to_input_rc(nullptr), _to_input_rc(nullptr),
_outputs_pub(nullptr), _outputs_pub(nullptr),
_num_outputs(0), _num_outputs(0),
@@ -858,7 +860,7 @@ PX4FMU::fill_rc_in(uint16_t raw_rc_count,
if (rssi == -1) { if (rssi == -1) {
/* set RSSI if analog RSSI input is present */ /* set RSSI if analog RSSI input is present */
if (_analog_rc_rssi_volt > 0.0f) { if (_analog_rc_rssi_stable) {
_rc_in.rssi = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f; _rc_in.rssi = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
if (_rc_in.rssi > 100) { if (_rc_in.rssi > 100) {
@@ -1202,13 +1204,15 @@ PX4FMU::cycle()
for (unsigned i = 0; i < adc_chans; i++) { for (unsigned i = 0; i < adc_chans; i++) {
if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) { if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
/* only allow this to be used if we see a high RSSI once */ if (_analog_rc_rssi_volt < 0.0f) {
if (_analog_rc_rssi_volt < 0.0f && adc.channel_value[i] > 2.5f) {
_analog_rc_rssi_volt = adc.channel_value[i]; _analog_rc_rssi_volt = adc.channel_value[i];
} }
if (_analog_rc_rssi_volt > 0.0f) {
_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc.channel_value[i] * 0.005f; _analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc.channel_value[i] * 0.005f;
/* only allow this to be used if we see a high RSSI once */
if (_analog_rc_rssi_volt > 2.5f) {
_analog_rc_rssi_stable = true;
} }
} }
} }