Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

drivers/rc_input: don't accept RX_PAIR cmd when armed

Browse Source
This commit is contained in:
Daniel Agar
2021-03-24 11:39:23 -04:00
parent 435ef38eda
commit 2d6deb4f1c
2 changed files with 55 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/rc_input/RCInput.cpp
+50 -37
View File
@@ -259,19 +259,6 @@ void RCInput::rc_io_invert(bool invert)
}
}
void RCInput::answer_command(const vehicle_command_s &cmd, uint8_t result)
{
/* publish ACK */
uORB::Publication<vehicle_command_ack_s> vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
vehicle_command_ack_s command_ack{};
command_ack.command = cmd.command;
command_ack.result = result;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
command_ack.timestamp = hrt_absolute_time();
vehicle_command_ack_pub.publish(command_ack);
}
void RCInput::Run()
{
if (should_exit()) {
@@ -301,6 +288,14 @@ void RCInput::Run()
updateParams();
}
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.copy(&vehicle_status)) {
_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
}
}
const hrt_abstime cycle_timestamp = hrt_absolute_time();
@@ -309,12 +304,12 @@ void RCInput::Run()
if (_vehicle_cmd_sub.update(&vcmd)) {
// Check for a pairing command
if ((unsigned int)vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
uint8_t cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
#if defined(SPEKTRUM_POWER)
if (!_rc_scan_locked /* !_armed.armed */) { // TODO: add armed check?
if (!_rc_scan_locked && !_armed) {
if ((int)vcmd.param1 == 0) {
// DSM binding command
int dsm_bind_mode = (int)vcmd.param2;
@@ -335,39 +330,55 @@ void RCInput::Run()
cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
} else {
cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
#endif /* SPEKTRUM_POWER */
answer_command(vcmd, cmd_ret);
#endif // SPEKTRUM_POWER
// publish acknowledgement
vehicle_command_ack_s command_ack{};
command_ack.command = vcmd.command;
command_ack.result = cmd_ret;
command_ack.target_system = vcmd.source_system;
command_ack.target_component = vcmd.source_component;
command_ack.timestamp = hrt_absolute_time();
uORB::Publication<vehicle_command_ack_s> vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
vehicle_command_ack_pub.publish(command_ack);
}
}
/* update ADC sampling */
#ifdef ADC_RC_RSSI_CHANNEL
adc_report_s adc;
if (_adc_sub.update(&adc)) {
for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
float adc_volt = adc.raw_data[i] *
adc.v_ref /
adc.resolution;
#if defined(ADC_RC_RSSI_CHANNEL)
if (_analog_rc_rssi_volt < 0.0f) {
_analog_rc_rssi_volt = adc_volt;
}
// update ADC sampling
if (_adc_report_sub.updated()) {
adc_report_s adc;
_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc_volt * 0.005f;
if (_adc_report_sub.copy(&adc)) {
for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
float adc_volt = adc.raw_data[i] *
adc.v_ref /
adc.resolution;
/* 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;
if (_analog_rc_rssi_volt < 0.0f) {
_analog_rc_rssi_volt = adc_volt;
}
_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc_volt * 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;
}
}
}
}
}
#endif /* ADC_RC_RSSI_CHANNEL */
#endif // ADC_RC_RSSI_CHANNEL
bool rc_updated = false;
@@ -375,9 +386,7 @@ void RCInput::Run()
// Scan for 300 msec, then switch protocol
constexpr hrt_abstime rc_scan_max = 300_ms;
bool sbus_failsafe, sbus_frame_drop;
unsigned frame_drops = 0;
bool dsm_11_bit;
if (_report_lock && _rc_scan_locked) {
_report_lock = false;
@@ -411,6 +420,9 @@ void RCInput::Run()
// parse new data
if (newBytes > 0) {
bool sbus_failsafe = false;
bool sbus_frame_drop = false;
rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
@@ -441,7 +453,8 @@ void RCInput::Run()
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
if (newBytes > 0) {
int8_t dsm_rssi;
int8_t dsm_rssi = 0;
bool dsm_11_bit = false;
// parse new data
rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
src/drivers/rc_input/RCInput.hpp
+5 -9
View File
@@ -59,6 +59,7 @@
#include <uORB/topics/input_rc.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
#include "crsf_telemetry.h"
#include "ghst_telemetry.hpp"
@@ -125,14 +126,6 @@ private:
void rc_io_invert(bool invert);
/**
* Respond to a vehicle command with an ACK message
*
* @param cmd The command that was executed or denied (inbound)
* @param result The command result
*/
void answer_command(const vehicle_command_s &cmd, uint8_t result);
hrt_abstime _rc_scan_begin{0};
bool _initialized{false};
@@ -143,14 +136,17 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _adc_report_sub{ORB_ID(adc_report)};
uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};
uORB::Subscription _adc_sub{ORB_ID(adc_report)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
input_rc_s _rc_in{};
float _analog_rc_rssi_volt{-1.0f};
bool _analog_rc_rssi_stable{false};
bool _armed{false};
uORB::PublicationMulti<input_rc_s> _to_input_rc{ORB_ID(input_rc)};