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: Report RSSI for RC

Browse Source 
Move RSSI sampling into FMU driver from sensors
This commit is contained in:
Lorenz Meier 2016-05-01 15:46:55 +02:00
parent 3335d32c0a
commit bdd2070dd7
2 changed files with 58 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -87,6 +87,7 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/adc_report.h>
#ifdef HRT_PPM_CHANNEL
@ -180,7 +181,9 @@ private:
struct work_s _work;
int _armed_sub;
int _param_sub;
int _adc_sub;
struct rc_input_values _rc_in;
float _analog_rc_rssi_volt;
orb_advert_t _to_input_rc;
orb_advert_t _outputs_pub;
unsigned _num_outputs;
@ -361,7 +364,9 @@ PX4FMU::PX4FMU() :
_work{},
_armed_sub(-1),
_param_sub(-1),
_adc_sub(-1),
_rc_in{},
_analog_rc_rssi_volt(-1.0f),
_to_input_rc(nullptr),
_outputs_pub(nullptr),
_num_outputs(0),
@ -835,8 +840,13 @@ PX4FMU::fill_rc_in(uint16_t raw_rc_count,
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
unsigned valid_chans = 0;
for (unsigned i = 0; i < _rc_in.channel_count; i++) {
_rc_in.values[i] = raw_rc_values[i];
if (raw_rc_values[i] != UINT16_MAX) {
valid_chans++;
}
}
_rc_in.timestamp_publication = now;
@ -845,15 +855,29 @@ PX4FMU::fill_rc_in(uint16_t raw_rc_count,
/* fake rssi if no value was provided */
if (rssi == -1) {
_rc_in.rssi =
(!frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2);
/* set RSSI if analog RSSI input is present */
if (_analog_rc_rssi_volt > 0.0f) {
_rc_in.rssi = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
if (_rc_in.rssi > 100) {
_rc_in.rssi = 100;
}
} else {
_rc_in.rssi =
(!frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2);
}
} else {
_rc_in.rssi = rssi;
}
if (valid_chans == 0) {
_rc_in.rssi = 0;
}
_rc_in.rc_failsafe = failsafe;
_rc_in.rc_lost = false;
_rc_in.rc_lost = (valid_chans > 0);
_rc_in.rc_lost_frame_count = frame_drops;
_rc_in.rc_total_frame_count = 0;
}
@ -909,6 +933,7 @@ PX4FMU::cycle()
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_param_sub = orb_subscribe(ORB_ID(parameter_update));
_adc_sub = orb_subscribe(ORB_ID(adc_report));
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
@ -1162,6 +1187,32 @@ PX4FMU::cycle()
}
}
/* update ADC sampling */
#ifdef ADC_RC_RSSI_CHANNEL
orb_check(_adc_sub, &updated);
if (updated) {
struct adc_report_s adc;
orb_copy(ORB_ID(adc_report), _adc_sub, &adc);
const unsigned adc_chans = sizeof(adc.channel_id) / sizeof(adc.channel_id[0]);
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) {
_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;
}
}
}
}
#endif
bool rc_updated = false;
#ifdef RC_SERIAL_PORT
@ -1178,7 +1229,7 @@ PX4FMU::cycle()
if (_report_lock && _rc_scan_locked) {
_report_lock = false;
warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
//warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
}
// read all available data from the serial RC input UART
@ -1347,6 +1398,8 @@ PX4FMU::cycle()
fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp,
false, false, 0);
_rc_scan_locked = true;
_rc_in.rc_ppm_frame_length = ppm_frame_length;
_rc_in.timestamp_last_signal = ppm_last_valid_decode;
}
} else {

23
src/modules/sensors/sensors.cpp
View File

@ -249,7 +249,6 @@ private:
struct airspeed_s _airspeed;
struct rc_parameter_map_s _rc_parameter_map;
float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
float _analog_rc_rssi_volt; /**< analog RSSI indicator */
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */
@ -553,7 +552,6 @@ Sensors::Sensors() :
_airspeed_validator(),
_param_rc_values{},
_analog_rc_rssi_volt(-1.0f),
_board_rotation{},
_mag_rotation{}
{
@ -1699,21 +1697,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
#endif
#ifdef ADC_RC_RSSI_CHANNEL
} else if (ADC_RC_RSSI_CHANNEL == buf_adc[i].am_channel) {
float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f;
if (voltage > 0.2f) {
if (_analog_rc_rssi_volt < 0.0f) {
_analog_rc_rssi_volt = voltage;
}
_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.999f + voltage * 0.001f;
}
#endif
}
}
@ -1924,12 +1907,6 @@ Sensors::rc_poll()
_rc.channel_count = rc_input.channel_count;
_rc.rssi = rc_input.rssi;
/* overwrite RSSI if analog RSSI input is present */
if (_analog_rc_rssi_volt > 0.2f) {
_rc.rssi = (_analog_rc_rssi_volt / 3.0f) * 100.0f;
}
_rc.signal_lost = signal_lost;
_rc.timestamp = rc_input.timestamp_last_signal;
_rc.frame_drop_count = rc_input.rc_lost_frame_count;