mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-05 15:58:52 +08:00
scan working for SBUS and DSM
This commit is contained in:
Mark Whitehorn
committed by
Lorenz Meier
Lorenz Meier
parent
0f3878a48a
commit
02030d9b36
+140
-78
@@ -119,6 +119,13 @@ public:
|
||||
int set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
|
||||
|
||||
private:
|
||||
enum RC_SCAN {
|
||||
RC_SCAN_SBUS = 0,
|
||||
RC_SCAN_DSM,
|
||||
RC_SCAN_SUMD,
|
||||
RC_SCAN_ST24
|
||||
};
|
||||
|
||||
static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS;
|
||||
|
||||
Mode _mode;
|
||||
@@ -629,6 +636,9 @@ PX4FMU::cycle()
|
||||
|
||||
update_pwm_rev_mask();
|
||||
|
||||
#ifdef RC_SERIAL_PORT
|
||||
_rcs_fd = :: open(RC_SERIAL_PORT, O_RDWR | O_NONBLOCK);
|
||||
#endif
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
@@ -799,93 +809,145 @@ PX4FMU::cycle()
|
||||
|
||||
bool rc_updated = false;
|
||||
|
||||
#ifdef DSM_SERIAL_PORT
|
||||
uint8_t n_bytes = 0;
|
||||
uint8_t *bytes;
|
||||
bool dsm_11_bit;
|
||||
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
|
||||
uint16_t raw_rc_count;
|
||||
#ifdef RC_SERIAL_PORT
|
||||
// This block scans for a supported serial RC input and locks onto the first one found
|
||||
static hrt_abstime rc_scan_last_lock = 0;
|
||||
static hrt_abstime rc_scan_begin = 0;
|
||||
// static bool rc_scan_locked = false;
|
||||
static enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS;
|
||||
|
||||
bool dsm_updated = dsm_input(_rcs_fd, raw_rc_values, &raw_rc_count, &dsm_11_bit, &n_bytes, &bytes,
|
||||
input_rc_s::RC_INPUT_MAX_CHANNELS);
|
||||
// Scan for one second, then switch protocol
|
||||
constexpr hrt_abstime rc_scan_max = 1000 * 1000;
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
if (dsm_updated) {
|
||||
switch (_rc_scan_state) {
|
||||
case RC_SCAN_SBUS:
|
||||
if (rc_scan_begin == 0) {
|
||||
warnx("fmu: RC_SCAN_SBUS: begin");
|
||||
rc_scan_begin = now;
|
||||
// // Configure serial port for SBUS
|
||||
sbus_config(_rcs_fd, false);
|
||||
// /* for PixRacer R07, this signal is active low */
|
||||
// /* for PixRacer R12, this signal is active high */
|
||||
stm32_gpiowrite(GPIO_SBUS_INV, 0);
|
||||
} else if (hrt_absolute_time() - rc_scan_last_lock < rc_scan_max
|
||||
|| hrt_absolute_time() - rc_scan_begin < rc_scan_max) {
|
||||
|
||||
warnx("%llu, %u, %u, %u, %u, %u\n", hrt_absolute_time(), dsm_updated,
|
||||
raw_rc_values[0], raw_rc_values[1], raw_rc_values[2],
|
||||
raw_rc_values[3]);
|
||||
bool sbus_failsafe, sbus_frame_drop;
|
||||
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
|
||||
uint16_t raw_rc_count;
|
||||
|
||||
// if (dsm_11_bit) {
|
||||
// raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
||||
// read port
|
||||
rc_updated = sbus_input(_rcs_fd, &raw_rc_values[0], &raw_rc_count,
|
||||
&sbus_failsafe, &sbus_frame_drop,
|
||||
input_rc_s::RC_INPUT_MAX_CHANNELS);
|
||||
if (rc_updated) {
|
||||
// we have a new SBUS frame. Publish it.
|
||||
_rc_in.channel_count = raw_rc_count;
|
||||
|
||||
if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
|
||||
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
|
||||
_rc_in.values[i] = raw_rc_values[i];
|
||||
}
|
||||
|
||||
_rc_in.timestamp_publication = hrt_absolute_time();
|
||||
_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
|
||||
|
||||
_rc_in.rc_ppm_frame_length = 0;
|
||||
_rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2);
|
||||
_rc_in.rc_failsafe = sbus_failsafe;
|
||||
_rc_in.rc_lost = false;
|
||||
_rc_in.rc_lost_frame_count = sbus_dropped_frames();
|
||||
_rc_in.rc_total_frame_count = 0;
|
||||
|
||||
rc_scan_last_lock = now;
|
||||
// rc_scan_locked = true;
|
||||
}
|
||||
} else {
|
||||
// This triggers the port re-configuration
|
||||
rc_scan_begin = 0;
|
||||
// rc_scan_locked = false;
|
||||
// Scan the next protocol
|
||||
_rc_scan_state = RC_SCAN_DSM;
|
||||
}
|
||||
break;
|
||||
|
||||
case RC_SCAN_DSM:
|
||||
if (rc_scan_begin == 0) {
|
||||
warnx("fmu: RC_SCAN_DSM: begin");
|
||||
rc_scan_begin = now;
|
||||
// // Configure serial port for DSM
|
||||
dsm_config(_rcs_fd);
|
||||
// /* for PixRacer R07, this signal is active low */
|
||||
// /* for PixRacer R12, this signal is active high */
|
||||
stm32_gpiowrite(GPIO_SBUS_INV, 1);
|
||||
} else if (hrt_absolute_time() - rc_scan_last_lock < rc_scan_max
|
||||
|| hrt_absolute_time() - rc_scan_begin < rc_scan_max) {
|
||||
|
||||
uint8_t n_bytes = 0;
|
||||
uint8_t *bytes;
|
||||
bool dsm_11_bit;
|
||||
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
|
||||
uint16_t raw_rc_count;
|
||||
|
||||
rc_updated = dsm_input(_rcs_fd, raw_rc_values, &raw_rc_count, &dsm_11_bit, &n_bytes, &bytes,
|
||||
input_rc_s::RC_INPUT_MAX_CHANNELS);
|
||||
|
||||
if (rc_updated) {
|
||||
|
||||
// warnx("%llu, %u, %u, %u, %u, %u\n", hrt_absolute_time(), rc_updated,
|
||||
// raw_rc_values[0], raw_rc_values[1], raw_rc_values[2],
|
||||
// raw_rc_values[3]);
|
||||
//
|
||||
// } else {
|
||||
// raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
||||
// }
|
||||
//
|
||||
// raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
// raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
// if (dsm_11_bit) {
|
||||
// raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
||||
//
|
||||
// } else {
|
||||
// raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
||||
// }
|
||||
//
|
||||
// raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
// raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
|
||||
// we have a new SBUS frame. Publish it.
|
||||
_rc_in.channel_count = raw_rc_count;
|
||||
// we have a new SBUS frame. Publish it.
|
||||
_rc_in.channel_count = raw_rc_count;
|
||||
|
||||
if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
|
||||
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
|
||||
if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
|
||||
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
|
||||
_rc_in.values[i] = raw_rc_values[i];
|
||||
}
|
||||
|
||||
_rc_in.timestamp_publication = hrt_absolute_time();
|
||||
_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
|
||||
|
||||
_rc_in.rc_ppm_frame_length = 0;
|
||||
// _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2);
|
||||
// _rc_in.rc_failsafe = sbus_failsafe;
|
||||
_rc_in.rc_lost = false;
|
||||
// _rc_in.rc_lost_frame_count = sbus_dropped_frames();
|
||||
_rc_in.rc_total_frame_count = 0;
|
||||
|
||||
rc_scan_last_lock = now;
|
||||
// rc_scan_locked = true;
|
||||
}
|
||||
} else {
|
||||
// This triggers the port re-configuration
|
||||
rc_scan_begin = 0;
|
||||
// rc_scan_locked = false;
|
||||
// Scan the next protocol
|
||||
_rc_scan_state = RC_SCAN_SBUS;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
|
||||
_rc_in.values[i] = raw_rc_values[i];
|
||||
}
|
||||
|
||||
_rc_in.timestamp_publication = hrt_absolute_time();
|
||||
_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
|
||||
|
||||
_rc_in.rc_ppm_frame_length = 0;
|
||||
// _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2);
|
||||
// _rc_in.rc_failsafe = sbus_failsafe;
|
||||
_rc_in.rc_lost = false;
|
||||
// _rc_in.rc_lost_frame_count = sbus_dropped_frames();
|
||||
_rc_in.rc_total_frame_count = 0;
|
||||
|
||||
rc_updated = true;
|
||||
break;
|
||||
case RC_SCAN_ST24:
|
||||
case RC_SCAN_SUMD:
|
||||
break;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef SBUS_SERIAL_PORT
|
||||
bool sbus_failsafe, sbus_frame_drop;
|
||||
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
|
||||
uint16_t raw_rc_count;
|
||||
|
||||
// read port
|
||||
bool sbus_updated = sbus_input(_rcs_fd, &raw_rc_values[0], &raw_rc_count,
|
||||
&sbus_failsafe, &sbus_frame_drop,
|
||||
input_rc_s::RC_INPUT_MAX_CHANNELS);
|
||||
|
||||
if (sbus_updated) {
|
||||
// we have a new SBUS frame. Publish it.
|
||||
_rc_in.channel_count = raw_rc_count;
|
||||
|
||||
if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
|
||||
_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
|
||||
_rc_in.values[i] = raw_rc_values[i];
|
||||
}
|
||||
|
||||
_rc_in.timestamp_publication = hrt_absolute_time();
|
||||
_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
|
||||
|
||||
_rc_in.rc_ppm_frame_length = 0;
|
||||
_rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2);
|
||||
_rc_in.rc_failsafe = sbus_failsafe;
|
||||
_rc_in.rc_lost = false;
|
||||
_rc_in.rc_lost_frame_count = sbus_dropped_frames();
|
||||
_rc_in.rc_total_frame_count = 0;
|
||||
|
||||
rc_updated = true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef HRT_PPM_CHANNEL
|
||||
|
||||
Reference in New Issue
Block a user