fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/drivers/px4fmu/fmu.cpp
View File

@ -184,6 +184,7 @@ private:
int _adc_sub;
struct rc_input_values _rc_in;
float _analog_rc_rssi_volt;
bool _analog_rc_rssi_stable;
orb_advert_t _to_input_rc;
orb_advert_t _outputs_pub;
unsigned _num_outputs;
@ -367,6 +368,7 @@ PX4FMU::PX4FMU() :
_adc_sub(-1),
_rc_in{},
_analog_rc_rssi_volt(-1.0f),
_analog_rc_rssi_stable(false),
_to_input_rc(nullptr),
_outputs_pub(nullptr),
_num_outputs(0),
@ -858,7 +860,7 @@ PX4FMU::fill_rc_in(uint16_t raw_rc_count,
if (rssi == -1) {
/* 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;
if (_rc_in.rssi > 100) {
@ -1202,13 +1204,15 @@ PX4FMU::cycle()
for (unsigned i = 0; i < adc_chans; i++) {
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 && adc.channel_value[i] > 2.5f) {
if (_analog_rc_rssi_volt < 0.0f) {
_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;
}
}
}