diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 370df73f97..f581d9707a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -87,11 +87,6 @@ then fi fi - if [ $OUTPUT_MODE == tap_esc ] - then - set FMU_MODE rcin - fi - if [ $OUTPUT_MODE == fmu ] then if fmu mode_$FMU_MODE $FMU_ARGS diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index aed84aef79..87d1ced50f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -398,6 +398,11 @@ else tune_control play -t 2 fi + if [ $IO_PRESENT == no -o $USE_IO == no ] + then + rc_input start + fi + # # Sensors System (start before Commander so Preflight checks are properly run). # Commander Needs to be this early for in-air-restarts. diff --git a/cmake/configs/nuttx_aerocore2_default.cmake b/cmake/configs/nuttx_aerocore2_default.cmake index 7b758cca98..1c5b975753 100644 --- a/cmake/configs/nuttx_aerocore2_default.cmake +++ b/cmake/configs/nuttx_aerocore2_default.cmake @@ -24,6 +24,7 @@ set(config_module_list modules/sensors #drivers/pwm_input #drivers/camera_trigger + drivers/rc_input # # System commands diff --git a/cmake/configs/nuttx_aerofc-v1_default.cmake b/cmake/configs/nuttx_aerofc-v1_default.cmake index d2e7828a90..d3b2f4a39d 100644 --- a/cmake/configs/nuttx_aerofc-v1_default.cmake +++ b/cmake/configs/nuttx_aerofc-v1_default.cmake @@ -17,6 +17,7 @@ set(config_module_list drivers/px4fmu drivers/stm32 drivers/pwm_out_sim + drivers/rc_input drivers/tap_esc modules/sensors diff --git a/cmake/configs/nuttx_av-x-v1_default.cmake b/cmake/configs/nuttx_av-x-v1_default.cmake index f1ee9835d1..b52f383f14 100644 --- a/cmake/configs/nuttx_av-x-v1_default.cmake +++ b/cmake/configs/nuttx_av-x-v1_default.cmake @@ -23,6 +23,7 @@ set(config_module_list drivers/pwm_out_sim drivers/px4flow drivers/px4fmu + drivers/rc_input drivers/stm32 drivers/stm32/adc drivers/tap_esc diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake index 2e8d31c203..4f85a9155c 100644 --- a/cmake/configs/nuttx_mindpx-v2_default.cmake +++ b/cmake/configs/nuttx_mindpx-v2_default.cmake @@ -29,6 +29,7 @@ set(config_module_list drivers/px4flow drivers/px4fmu drivers/rgbled + drivers/rc_input #drivers/rgbled_pwm drivers/stm32 drivers/stm32/adc diff --git a/cmake/configs/nuttx_nxphlite-v3_default.cmake b/cmake/configs/nuttx_nxphlite-v3_default.cmake index ec77802fb6..d376cdde63 100644 --- a/cmake/configs/nuttx_nxphlite-v3_default.cmake +++ b/cmake/configs/nuttx_nxphlite-v3_default.cmake @@ -32,6 +32,7 @@ set(config_module_list drivers/pwm_out_sim drivers/px4flow drivers/px4fmu + drivers/rc_input drivers/rgbled drivers/rgbled_pwm drivers/tap_esc diff --git a/cmake/configs/nuttx_omnibus-f4sd_default.cmake b/cmake/configs/nuttx_omnibus-f4sd_default.cmake index 9e627d9547..26f2d47960 100644 --- a/cmake/configs/nuttx_omnibus-f4sd_default.cmake +++ b/cmake/configs/nuttx_omnibus-f4sd_default.cmake @@ -20,6 +20,7 @@ set(config_module_list drivers/gps drivers/px4flow drivers/px4fmu + drivers/rc_input drivers/rgbled drivers/stm32 drivers/stm32/adc diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index eb8db504bc..0e2717f305 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -27,6 +27,7 @@ set(config_module_list drivers/px4flow drivers/px4fmu drivers/rgbled + drivers/rc_input drivers/stm32 drivers/stm32/adc drivers/stm32/tone_alarm diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake index 77e9679eff..f08961f341 100644 --- a/cmake/configs/nuttx_px4fmu-v5_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake @@ -15,24 +15,24 @@ set(config_module_list drivers/batt_smbus drivers/blinkm + drivers/camera_trigger + drivers/gps drivers/imu/bma180 drivers/imu/bmi055 drivers/imu/bmi160 - drivers/camera_trigger - drivers/gps - drivers/irlock - drivers/mkblctrl drivers/imu/mpu6000 drivers/imu/mpu9250 + drivers/irlock + drivers/mkblctrl drivers/oreoled drivers/pwm_input drivers/pwm_out_sim drivers/px4flow drivers/px4fmu drivers/px4io + drivers/rc_input drivers/rgbled - # Enable the line below to put the three leds into PWM RGB mode - #drivers/rgbled_pwm + #drivers/rgbled_pwm # Enable to put the three leds into PWM RGB mode drivers/stm32 drivers/stm32/adc drivers/stm32/tone_alarm diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake index 4e2b1fdc21..144ee28126 100644 --- a/cmake/configs/nuttx_tap-v1_default.cmake +++ b/cmake/configs/nuttx_tap-v1_default.cmake @@ -14,6 +14,7 @@ set(config_module_list drivers/imu/mpu6000 drivers/px4fmu drivers/rgbled_pwm + drivers/rc_input drivers/stm32 drivers/stm32/adc drivers/stm32/tone_alarm diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 553e0390fc..cd4c6a8e79 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -190,10 +190,6 @@ struct pwm_output_rc_config { /** start DSM bind */ #define DSM_BIND_START _PX4_IOC(_PWM_SERVO_BASE, 10) -#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ -#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ -#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ - /** power up DSM receiver */ #define DSM_BIND_POWER_UP _PX4_IOC(_PWM_SERVO_BASE, 11) diff --git a/src/drivers/px4fmu/CMakeLists.txt b/src/drivers/px4fmu/CMakeLists.txt index 33c261497f..1f04bc13ce 100644 --- a/src/drivers/px4fmu/CMakeLists.txt +++ b/src/drivers/px4fmu/CMakeLists.txt @@ -37,10 +37,8 @@ px4_add_module( COMPILE_FLAGS SRCS fmu.cpp - crsf_telemetry.cpp DEPENDS circuit_breaker mixer - rc pwm_limit ) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 896c2c4a8b..3b2bca1a4c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -47,12 +47,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include #include #include @@ -65,36 +59,15 @@ #include #include #include -#include #include #include #include -#include -#include - -#include "crsf_telemetry.h" - -#ifdef HRT_PPM_CHANNEL -# include -#endif #define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */ static constexpr uint8_t CYCLE_COUNT = 10; /* safety switch must be held for 1 second to activate */ static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; -#if defined(PX4_CPU_UUID_WORD32_FORMAT) -# define CPU_UUID_FORMAT PX4_CPU_UUID_WORD32_FORMAT -#else -# define CPU_UUID_FORMAT "%0X" -#endif - -#if defined(PX4_CPU_UUID_WORD32_SEPARATOR) -# define CPU_UUID_SEPARATOR PX4_CPU_UUID_WORD32_SEPARATOR -#else -# define CPU_UUID_SEPARATOR " " -#endif - /* * Define the various LED flash sequences for each system state. */ @@ -109,7 +82,6 @@ enum PortMode { PORT_MODE_UNSET = 0, PORT_FULL_GPIO, PORT_FULL_PWM, - PORT_RC_IN, PORT_PWM6, PORT_PWM4, PORT_PWM3, @@ -195,34 +167,12 @@ public: void update_pwm_trims(); private: - enum RC_SCAN { - RC_SCAN_PPM = 0, - RC_SCAN_SBUS, - RC_SCAN_DSM, - RC_SCAN_SUMD, - RC_SCAN_ST24, - RC_SCAN_CRSF - }; - enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS; - - char const *RC_SCAN_STRING[6] = { - "PPM", - "SBUS", - "DSM", - "SUMD", - "ST24", - "CRSF" - }; enum class MotorOrdering : int32_t { PX4 = 0, Betaflight = 1 }; - hrt_abstime _rc_scan_begin = 0; - bool _rc_scan_locked = false; - bool _report_lock = true; - hrt_abstime _cycle_timestamp = 0; hrt_abstime _last_safety_check = 0; hrt_abstime _time_last_mix = 0; @@ -236,19 +186,13 @@ private: unsigned _current_update_rate; bool _run_as_task; static struct work_s _work; - int _vehicle_cmd_sub; + int _armed_sub; int _param_sub; - 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; int _class_instance; - int _rcs_fd; - uint8_t _rcs_buf[SBUS_BUFFER_SIZE]; bool _throttle_armed; bool _pwm_on; @@ -265,9 +209,6 @@ private: pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; - uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; - uint16_t _raw_rc_count; - static pwm_limit_t _pwm_limit; static actuator_armed_s _armed; uint16_t _failsafe_pwm[_max_actuators]; @@ -287,8 +228,6 @@ private: bool _airmode; ///< multicopter air-mode MotorOrdering _motor_ordering; - CRSFTelemetry *_crsf_telemetry; - perf_counter_t _perf_control_latency; static bool arm_nothrottle() @@ -336,13 +275,6 @@ private: PX4FMU(const PX4FMU &) = delete; PX4FMU operator=(const PX4FMU &) = delete; - void fill_rc_in(uint16_t raw_rc_count_local, - uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], - hrt_abstime now, bool frame_drop, bool failsafe, - unsigned frame_drops, int rssi); - - void set_rc_scan_state(RC_SCAN _rc_scan_state); - void rc_io_invert(bool invert, uint32_t uxart_base); void safety_check_button(void); void flash_safety_button(void); @@ -370,18 +302,11 @@ PX4FMU::PX4FMU(bool run_as_task) : _pwm_alt_rate_channels(0), _current_update_rate(0), _run_as_task(run_as_task), - _vehicle_cmd_sub(-1), _armed_sub(-1), _param_sub(-1), - _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), _class_instance(0), - _rcs_fd(-1), _throttle_armed(false), _pwm_on(false), _pwm_mask(0), @@ -390,7 +315,6 @@ PX4FMU::PX4FMU(bool run_as_task) : _groups_required(0), _groups_subscribed(0), _poll_fds_num(0), - _raw_rc_count(0), _failsafe_pwm{0}, _disarmed_pwm{0}, _reverse_pwm_mask(0), @@ -404,7 +328,6 @@ PX4FMU::PX4FMU(bool run_as_task) : _thr_mdl_fac(0.0f), _airmode(false), _motor_ordering(MotorOrdering::PX4), - _crsf_telemetry(nullptr), _perf_control_latency(perf_alloc(PC_ELAPSED, "fmu control latency")) { for (unsigned i = 0; i < _max_actuators; i++) { @@ -432,16 +355,6 @@ PX4FMU::PX4FMU(bool run_as_task) : _armed.force_failsafe = false; _armed.in_esc_calibration_mode = false; - // rc input, published to ORB - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; - // initialize it as RC lost - _rc_in.rc_lost = true; - - // initialize raw_rc values and count - for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) { - _raw_rc_values[i] = UINT16_MAX; - } - // If there is no safety button, disable it on boot. #ifndef GPIO_BTN_SAFETY _safety_off = true; @@ -462,7 +375,6 @@ PX4FMU::~PX4FMU() orb_unsubscribe(_armed_sub); orb_unsubscribe(_param_sub); - orb_unadvertise(_to_input_rc); orb_unadvertise(_outputs_pub); orb_unadvertise(_to_safety); orb_unadvertise(_to_mixer_status); @@ -470,20 +382,12 @@ PX4FMU::~PX4FMU() /* make sure servos are off */ up_pwm_servo_deinit(); -#ifdef RC_SERIAL_PORT - dsm_deinit(); -#endif - /* note - someone else is responsible for restoring the GPIO config */ /* clean up the alternate device node */ unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); perf_free(_perf_control_latency); - - if (_crsf_telemetry) { - delete (_crsf_telemetry); - } } int @@ -516,30 +420,10 @@ PX4FMU::init() _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); -#ifdef RC_SERIAL_PORT - -# ifdef RF_RADIO_POWER_CONTROL - // power radio on - RF_RADIO_POWER_CONTROL(true); -# endif - _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - // dsm_init sets some file static variables and returns a file descriptor - _rcs_fd = dsm_init(RC_SERIAL_PORT); - // assume SBUS input and immediately switch it to - // so that if Single wire mode on TX there will be only - // a short contention - sbus_config(_rcs_fd, board_supports_single_wire(RC_UXART_BASE)); -# ifdef GPIO_PPM_IN - // disable CPPM input by mapping it away from the timer capture input - px4_arch_unconfiggpio(GPIO_PPM_IN); -# endif -#endif - // Getting initial parameter values update_params(); @@ -1113,85 +997,6 @@ PX4FMU::capture_callback(uint32_t chan_index, fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, overflow); } -void -PX4FMU::fill_rc_in(uint16_t raw_rc_count_local, - uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], - hrt_abstime now, bool frame_drop, bool failsafe, - unsigned frame_drops, int rssi = -1) -{ - // fill rc_in struct for publishing - _rc_in.channel_count = raw_rc_count_local; - - if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { - _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; - } - - unsigned valid_chans = 0; - - for (unsigned i = 0; i < _rc_in.channel_count; i++) { - _rc_in.values[i] = raw_rc_values_local[i]; - - if (raw_rc_values_local[i] != UINT16_MAX) { - valid_chans++; - } - - // once filled, reset values back to default - _raw_rc_values[i] = UINT16_MAX; - } - - _rc_in.timestamp = now; - _rc_in.timestamp_last_signal = _rc_in.timestamp; - _rc_in.rc_ppm_frame_length = 0; - - /* fake rssi if no value was provided */ - if (rssi == -1) { - - /* set RSSI if analog RSSI input is present */ - if (_analog_rc_rssi_stable) { - float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f; - - if (rssi_analog > 100.0f) { - rssi_analog = 100.0f; - } - - if (rssi_analog < 0.0f) { - rssi_analog = 0.0f; - } - - _rc_in.rssi = rssi_analog; - - } else { - _rc_in.rssi = 255; - } - - } else { - _rc_in.rssi = rssi; - } - - if (valid_chans == 0) { - _rc_in.rssi = 0; - } - - _rc_in.rc_failsafe = failsafe; - _rc_in.rc_lost = (valid_chans == 0); - _rc_in.rc_lost_frame_count = frame_drops; - _rc_in.rc_total_frame_count = 0; -} - -#ifdef RC_SERIAL_PORT -void PX4FMU::set_rc_scan_state(RC_SCAN newState) -{ -// PX4_WARN("RCscan: %s failed, trying %s", PX4FMU::RC_SCAN_STRING[_rc_scan_state], PX4FMU::RC_SCAN_STRING[newState]); - _rc_scan_begin = 0; - _rc_scan_state = newState; -} - -void PX4FMU::rc_io_invert(bool invert, uint32_t uxart_base) -{ - INVERT_RC_INPUT(invert, uxart_base); -} -#endif - void PX4FMU::update_pwm_out_state(bool on) { @@ -1228,7 +1033,7 @@ PX4FMU::cycle() _current_update_rate = 0; } - int poll_timeout = 5; // needs to be small enough so that we don't miss RC input data + int poll_timeout = 5; if (!_run_as_task) { /* @@ -1491,358 +1296,12 @@ PX4FMU::cycle() update_pwm_out_state(pwm_on); } -#ifdef RC_SERIAL_PORT - /* vehicle command */ - orb_check(_vehicle_cmd_sub, &updated); - - if (updated) { - struct vehicle_command_s cmd; - orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub, &cmd); - - // Check for a pairing command - if ((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) { - if (!_armed.armed) { - if ((int)cmd.param1 == 0) { - // DSM binding command - int dsm_bind_mode = (int)cmd.param2; - - int dsm_bind_pulses = 0; - - if (dsm_bind_mode == 0) { - dsm_bind_pulses = DSM2_BIND_PULSES; - - } else if (dsm_bind_mode == 1) { - dsm_bind_pulses = DSMX_BIND_PULSES; - - } else { - dsm_bind_pulses = DSMX8_BIND_PULSES; - } - - ioctl(nullptr, DSM_BIND_START, dsm_bind_pulses); - } - - } else { - PX4_WARN("system armed, bind request rejected"); - } - } - } - -#endif - orb_check(_param_sub, &updated); if (updated) { this->update_params(); } - /* 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) { - - if (_analog_rc_rssi_volt < 0.0f) { - _analog_rc_rssi_volt = adc.channel_value[i]; - } - - _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; - } - } - } - } - -#endif - - bool rc_updated = false; - -#ifdef RC_SERIAL_PORT - // This block scans for a supported serial RC input and locks onto the first one found - // Scan for 300 msec, then switch protocol - constexpr hrt_abstime rc_scan_max = 300 * 1000; - - bool sbus_failsafe, sbus_frame_drop; - unsigned frame_drops; - bool dsm_11_bit; - - - if (_report_lock && _rc_scan_locked) { - _report_lock = false; - //PX4_WARN("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); - } - - // read all available data from the serial RC input UART - int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); - - switch (_rc_scan_state) { - case RC_SCAN_SBUS: - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; - // Configure serial port for SBUS - sbus_config(_rcs_fd, board_supports_single_wire(RC_UXART_BASE)); - rc_io_invert(true, RC_UXART_BASE); - - } else if (_rc_scan_locked - || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - // parse new data - if (newBytes > 0) { - 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); - - if (rc_updated) { - // we have a new SBUS frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; - fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, - sbus_frame_drop, sbus_failsafe, frame_drops); - _rc_scan_locked = true; - } - } - - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_DSM); - } - - break; - - case RC_SCAN_DSM: - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; - // // Configure serial port for DSM - dsm_config(_rcs_fd); - rc_io_invert(false, RC_UXART_BASE); - - } else if (_rc_scan_locked - || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - if (newBytes > 0) { - int8_t dsm_rssi; - - // parse new data - rc_updated = dsm_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, - &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS); - - if (rc_updated) { - // we have a new DSM frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; - fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, - false, false, frame_drops, dsm_rssi); - _rc_scan_locked = true; - } - } - - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_ST24); - } - - break; - - case RC_SCAN_ST24: - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; - // Configure serial port for DSM - dsm_config(_rcs_fd); - rc_io_invert(false, RC_UXART_BASE); - - } else if (_rc_scan_locked - || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - if (newBytes > 0) { - // parse new data - uint8_t st24_rssi, lost_count; - - rc_updated = false; - - for (unsigned i = 0; i < (unsigned)newBytes; i++) { - /* set updated flag if one complete packet was parsed */ - st24_rssi = RC_INPUT_RSSI_MAX; - rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, - &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); - } - - // The st24 will keep outputting RC channels and RSSI even if RC has been lost. - // The only way to detect RC loss is therefore to look at the lost_count. - - if (rc_updated) { - if (lost_count == 0) { - // we have a new ST24 frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24; - fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, - false, false, frame_drops, st24_rssi); - _rc_scan_locked = true; - - } else { - // if the lost count > 0 means that there is an RC loss - _rc_in.rc_lost = true; - } - } - } - - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_SUMD); - } - - break; - - case RC_SCAN_SUMD: - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; - // Configure serial port for DSM - dsm_config(_rcs_fd); - rc_io_invert(false, RC_UXART_BASE); - - } else if (_rc_scan_locked - || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - if (newBytes > 0) { - // parse new data - uint8_t sumd_rssi, rx_count; - bool sumd_failsafe; - - rc_updated = false; - - for (unsigned i = 0; i < (unsigned)newBytes; i++) { - /* set updated flag if one complete packet was parsed */ - sumd_rssi = RC_INPUT_RSSI_MAX; - rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count, - &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe)); - } - - if (rc_updated) { - // we have a new SUMD frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD; - fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, - false, sumd_failsafe, frame_drops, sumd_rssi); - _rc_scan_locked = true; - } - } - - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_PPM); - } - - break; - - case RC_SCAN_PPM: - // skip PPM if it's not supported -#ifdef HRT_PPM_CHANNEL - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; - // Configure timer input pin for CPPM - px4_arch_configgpio(GPIO_PPM_IN); - rc_io_invert(false, RC_UXART_BASE); - - } else if (_rc_scan_locked || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - // see if we have new PPM input data - if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) { - // we have a new PPM frame. Publish it. - rc_updated = true; - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; - 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 { - // disable CPPM input by mapping it away from the timer capture input - px4_arch_unconfiggpio(GPIO_PPM_IN); - // Scan the next protocol - set_rc_scan_state(RC_SCAN_CRSF); - } - -#else // skip PPM if it's not supported - set_rc_scan_state(RC_SCAN_CRSF); - -#endif // HRT_PPM_CHANNEL - - break; - - case RC_SCAN_CRSF: - if (_rc_scan_begin == 0) { - _rc_scan_begin = _cycle_timestamp; - // Configure serial port for CRSF - crsf_config(_rcs_fd); - rc_io_invert(false, RC_UXART_BASE); - - } else if (_rc_scan_locked - || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { - - // parse new data - if (newBytes > 0) { - rc_updated = crsf_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, - input_rc_s::RC_INPUT_MAX_CHANNELS); - - if (rc_updated) { - // we have a new CRSF frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF; - fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, false, false, 0); - - // Enable CRSF Telemetry only on the Omnibus, because on Pixhawk (-related) boards - // we cannot write to the RC UART - // It might work on FMU-v5. Or another option is to use a different UART port -#ifdef CONFIG_ARCH_BOARD_OMNIBUS_F4SD - - if (!_rc_scan_locked && !_crsf_telemetry) { - _crsf_telemetry = new CRSFTelemetry(_rcs_fd); - } - -#endif - - _rc_scan_locked = true; - - if (_crsf_telemetry) { - _crsf_telemetry->update(_cycle_timestamp); - } - } - } - - } else { - // Scan the next protocol - set_rc_scan_state(RC_SCAN_SBUS); - } - - break; - } - -#else // RC_SERIAL_PORT not defined -#ifdef HRT_PPM_CHANNEL - - // see if we have new PPM input data - if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) { - // we have a new PPM frame. Publish it. - rc_updated = true; - fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp, false, false, 0); - _rc_in.rc_ppm_frame_length = ppm_frame_length; - _rc_in.timestamp_last_signal = ppm_last_valid_decode; - } - -#endif // HRT_PPM_CHANNEL -#endif // RC_SERIAL_PORT - - if (rc_updated) { - int instance = _class_instance; - orb_publish_auto(ORB_ID(input_rc), &_to_input_rc, &_rc_in, &instance, ORB_PRIO_DEFAULT); - - } else if (!rc_updated && ((hrt_absolute_time() - _rc_in.timestamp_last_signal) > 1000 * 1000)) { - _rc_scan_locked = false; - } - if (_run_as_task) { if (should_exit()) { break; @@ -2581,41 +2040,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } -#ifdef SPEKTRUM_POWER - - case DSM_BIND_START: - /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ - PX4_INFO("DSM_BIND_START: DSM%s RX", (arg == 0) ? "2" : ((arg == 1) ? "-X" : "-X8")); - - if (arg == DSM2_BIND_PULSES || - arg == DSMX_BIND_PULSES || - arg == DSMX8_BIND_PULSES) { - - dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0); - - dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0); - usleep(500000); - - dsm_bind(DSM_CMD_BIND_POWER_UP, 0); - usleep(72000); - - irqstate_t flags = px4_enter_critical_section(); - dsm_bind(DSM_CMD_BIND_SEND_PULSES, arg); - px4_leave_critical_section(flags); - usleep(50000); - - dsm_bind(DSM_CMD_BIND_REINIT_UART, 0); - - ret = OK; - - } else { - PX4_ERR("DSM bind failed"); - ret = -EINVAL; - } - - break; -#endif - case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; @@ -3074,10 +2498,6 @@ PX4FMU::fmu_new_mode(PortMode new_mode) #endif break; - case PORT_RC_IN: - servo_mode = PX4FMU::MODE_NONE; - break; - case PORT_PWM1: /* select 2-pin PWM mode */ servo_mode = PX4FMU::MODE_1PWM; @@ -3144,22 +2564,6 @@ PX4FMU::fmu_new_mode(PortMode new_mode) namespace { -void -bind_spektrum() -{ - int fd = open(PX4FMU_DEVICE_PATH, O_RDWR); - - if (fd < 0) { - PX4_ERR("open fail"); - return; - } - - /* specify 11ms DSMX. RX will automatically fall back to 22ms or DSM2 if necessary */ - ioctl(fd, DSM_BIND_START, DSMX8_BIND_PULSES); - - close(fd); -} - int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz) { return PX4FMU::set_i2c_bus_clock(bus, clock_hz); @@ -3414,11 +2818,6 @@ int PX4FMU::custom_command(int argc, char *argv[]) PortMode new_mode = PORT_MODE_UNSET; const char *verb = argv[0]; - if (!strcmp(verb, "bind")) { - bind_spektrum(); - return 0; - } - /* does not operate on a FMU instance */ if (!strcmp(verb, "i2c")) { if (argc > 2) { @@ -3478,9 +2877,6 @@ int PX4FMU::custom_command(int argc, char *argv[]) if (!strcmp(verb, "mode_gpio")) { new_mode = PORT_FULL_GPIO; - } else if (!strcmp(verb, "mode_rcin")) { - new_mode = PORT_RC_IN; - } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; @@ -3493,7 +2889,6 @@ int PX4FMU::custom_command(int argc, char *argv[]) #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - } else if (!strcmp(verb, "mode_pwm4")) { new_mode = PORT_PWM4; @@ -3548,13 +2943,6 @@ This module is responsible for driving the output and reading the input pins. Fo px4io driver is used for main ones. It listens on the actuator_controls topics, does the mixing and writes the PWM outputs. -In addition it does the RC input parsing and auto-selecting the method. Supported methods are: -- PPM -- SBUS -- DSM -- SUMD -- ST24 -- TBS Crossfire (CRSF) The module is configured via mode_* commands. This defines which of the first N pins the driver should occupy. By using mode_pwm4 for example, pins 5 and 6 can be used by the camera trigger driver or by a PWM rangefinder @@ -3589,7 +2977,6 @@ mixer files. PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start the fmu if not running already"); PRINT_MODULE_USAGE_COMMAND("mode_gpio"); - PRINT_MODULE_USAGE_COMMAND_DESCR("mode_rcin", "Only do RC input, no PWM outputs"); PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "Select all available pins as PWM"); #if defined(BOARD_HAS_PWM) PRINT_MODULE_USAGE_COMMAND("mode_pwm1"); @@ -3604,7 +2991,6 @@ mixer files. #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 PRINT_MODULE_USAGE_COMMAND("mode_pwm6"); #endif - PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a DSM bind command (module must be running)"); PRINT_MODULE_USAGE_COMMAND_DESCR("sensor_reset", "Do a sensor reset (SPI bus)"); PRINT_MODULE_USAGE_ARG("", "Delay time in ms between reset and re-enabling", true); @@ -3630,11 +3016,6 @@ int PX4FMU::print_status() PX4_INFO("Max update rate: %i Hz", _current_update_rate); } - PX4_INFO("RC scan state: %s, locked: %s", RC_SCAN_STRING[_rc_scan_state], _rc_scan_locked ? "yes" : "no"); - PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no"); -#ifdef RC_SERIAL_PORT - PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames()); -#endif const char *mode_str = nullptr; switch (_mode) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3600507eee..0a75dd226c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -67,6 +67,8 @@ #include #include +#include + #include #include #include diff --git a/src/drivers/rc_input/CMakeLists.txt b/src/drivers/rc_input/CMakeLists.txt new file mode 100644 index 0000000000..45552a58c7 --- /dev/null +++ b/src/drivers/rc_input/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__rc_input + MAIN rc_input + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS + RCInput.cpp + crsf_telemetry.cpp + DEPENDS + rc + ) diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp new file mode 100644 index 0000000000..f995b4f6da --- /dev/null +++ b/src/drivers/rc_input/RCInput.cpp @@ -0,0 +1,799 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "RCInput.hpp" + +#include "crsf_telemetry.h" + +using namespace time_literals; + +#define SCHEDULE_INTERVAL 4000 /**< The schedule interval in usec (250 Hz) */ + +#if defined(SPEKTRUM_POWER) +static bool bind_spektrum(int arg); +#endif /* SPEKTRUM_POWER */ + +work_s RCInput::_work = {}; + +RCInput::RCInput(bool run_as_task) +{ + // rc input, published to ORB + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; + + // initialize it as RC lost + _rc_in.rc_lost = true; + + // initialize raw_rc values and count + for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) { + _raw_rc_values[i] = UINT16_MAX; + } +} + +RCInput::~RCInput() +{ + orb_unadvertise(_to_input_rc); + + orb_unsubscribe(_adc_sub); + orb_unsubscribe(_vehicle_cmd_sub); + +#ifdef RC_SERIAL_PORT + dsm_deinit(); +#endif + + if (_crsf_telemetry) { + delete (_crsf_telemetry); + } +} + +int +RCInput::init() +{ + _adc_sub = orb_subscribe(ORB_ID(adc_report)); + +#ifdef RC_SERIAL_PORT + +# ifdef RF_RADIO_POWER_CONTROL + // power radio on + RF_RADIO_POWER_CONTROL(true); +# endif + _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + // dsm_init sets some file static variables and returns a file descriptor + _rcs_fd = dsm_init(RC_SERIAL_PORT); + // assume SBUS input and immediately switch it to + // so that if Single wire mode on TX there will be only + // a short contention + sbus_config(_rcs_fd, board_supports_single_wire(RC_UXART_BASE)); +# ifdef GPIO_PPM_IN + // disable CPPM input by mapping it away from the timer capture input + px4_arch_unconfiggpio(GPIO_PPM_IN); +# endif +#endif + + return 0; +} + +int +RCInput::task_spawn(int argc, char *argv[]) +{ + bool run_as_task = false; + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "t", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 't': + run_as_task = true; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return -1; + } + + + if (!run_as_task) { + + /* schedule a cycle to start things */ + int ret = work_queue(HPWORK, &_work, (worker_t)&RCInput::cycle_trampoline, nullptr, 0); + + if (ret < 0) { + return ret; + } + + _task_id = task_id_is_work_queue; + + } else { + + /* start the IO interface task */ + + _task_id = px4_task_spawn_cmd("rc_input", + SCHED_DEFAULT, + SCHED_PRIORITY_SLOW_DRIVER, + 1000, + (px4_main_t)&run_trampoline, + nullptr); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + } + + return PX4_OK; +} + +void +RCInput::cycle_trampoline(void *arg) +{ + RCInput *dev = reinterpret_cast(arg); + + // check if the trampoline is called for the first time + if (!dev) { + dev = new RCInput(false); + + if (!dev) { + PX4_ERR("alloc failed"); + return; + } + + if (dev->init() != 0) { + PX4_ERR("init failed"); + delete dev; + return; + } + + _object = dev; + } + + dev->cycle(); +} + +void +RCInput::fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi = -1) +{ + // fill rc_in struct for publishing + _rc_in.channel_count = raw_rc_count_local; + + if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + unsigned valid_chans = 0; + + for (unsigned i = 0; i < _rc_in.channel_count; i++) { + _rc_in.values[i] = raw_rc_values_local[i]; + + if (raw_rc_values_local[i] != UINT16_MAX) { + valid_chans++; + } + + // once filled, reset values back to default + _raw_rc_values[i] = UINT16_MAX; + } + + _rc_in.timestamp = now; + _rc_in.timestamp_last_signal = _rc_in.timestamp; + _rc_in.rc_ppm_frame_length = 0; + + /* fake rssi if no value was provided */ + if (rssi == -1) { + + /* set RSSI if analog RSSI input is present */ + if (_analog_rc_rssi_stable) { + float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f; + + if (rssi_analog > 100.0f) { + rssi_analog = 100.0f; + } + + if (rssi_analog < 0.0f) { + rssi_analog = 0.0f; + } + + _rc_in.rssi = rssi_analog; + + } else { + _rc_in.rssi = 255; + } + + } else { + _rc_in.rssi = rssi; + } + + if (valid_chans == 0) { + _rc_in.rssi = 0; + } + + _rc_in.rc_failsafe = failsafe; + _rc_in.rc_lost = (valid_chans == 0); + _rc_in.rc_lost_frame_count = frame_drops; + _rc_in.rc_total_frame_count = 0; +} + +#ifdef RC_SERIAL_PORT +void RCInput::set_rc_scan_state(RC_SCAN newState) +{ +// PX4_WARN("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]); + _rc_scan_begin = 0; + _rc_scan_state = newState; +} + +void RCInput::rc_io_invert(bool invert, uint32_t uxart_base) +{ + INVERT_RC_INPUT(invert, uxart_base); +} +#endif + +void +RCInput::run() +{ + if (init() != 0) { + PX4_ERR("init failed"); + exit_and_cleanup(); + return; + } + + cycle(); +} + +void +RCInput::cycle() +{ + while (true) { + + _cycle_timestamp = hrt_absolute_time(); + +#if defined(SPEKTRUM_POWER) + /* vehicle command */ + bool vehicle_command_updated = false; + orb_check(_vehicle_cmd_sub, &vehicle_command_updated); + + if (vehicle_command_updated) { + vehicle_command_s vcmd = {}; + orb_copy(ORB_ID(vehicle_command), _vehicle_cmd_sub, &vcmd); + + // Check for a pairing command + if ((unsigned int)vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) { + if (!_rc_scan_locked /* !_armed.armed */) { // TODO: add armed check? + if ((int)vcmd.param1 == 0) { + // DSM binding command + int dsm_bind_mode = (int)vcmd.param2; + + int dsm_bind_pulses = 0; + + if (dsm_bind_mode == 0) { + dsm_bind_pulses = DSM2_BIND_PULSES; + + } else if (dsm_bind_mode == 1) { + dsm_bind_pulses = DSMX_BIND_PULSES; + + } else { + dsm_bind_pulses = DSMX8_BIND_PULSES; + } + + bind_spektrum(dsm_bind_pulses); + } + + } else { + PX4_WARN("system armed, bind request rejected"); + } + } + } + +#endif /* SPEKTRUM_POWER */ + + /* update ADC sampling */ +#ifdef ADC_RC_RSSI_CHANNEL + bool adc_updated = false; + orb_check(_adc_sub, &adc_updated); + + if (adc_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) { + + if (_analog_rc_rssi_volt < 0.0f) { + _analog_rc_rssi_volt = adc.channel_value[i]; + } + + _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; + } + } + } + } + +#endif /* ADC_RC_RSSI_CHANNEL */ + + bool rc_updated = false; + +#ifdef RC_SERIAL_PORT + // This block scans for a supported serial RC input and locks onto the first one found + // Scan for 300 msec, then switch protocol + constexpr hrt_abstime rc_scan_max = 300_ms; + + bool sbus_failsafe, sbus_frame_drop; + unsigned frame_drops; + bool dsm_11_bit; + + if (_report_lock && _rc_scan_locked) { + _report_lock = false; + //PX4_WARN("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); + } + + int newBytes = 0; + + if (_run_as_task) { + // TODO: needs work (poll _rcs_fd) + // int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100); + // then update priority to SCHED_PRIORITY_FAST_DRIVER + // read all available data from the serial RC input UART + newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); + + } else { + // read all available data from the serial RC input UART + newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); + } + + switch (_rc_scan_state) { + case RC_SCAN_SBUS: + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // Configure serial port for SBUS + sbus_config(_rcs_fd, board_supports_single_wire(RC_UXART_BASE)); + rc_io_invert(true, RC_UXART_BASE); + + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + // parse new data + if (newBytes > 0) { + 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); + + if (rc_updated) { + // we have a new SBUS frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; + fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, + sbus_frame_drop, sbus_failsafe, frame_drops); + _rc_scan_locked = true; + } + } + + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_DSM); + } + + break; + + case RC_SCAN_DSM: + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // // Configure serial port for DSM + dsm_config(_rcs_fd); + rc_io_invert(false, RC_UXART_BASE); + + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + if (newBytes > 0) { + int8_t dsm_rssi; + + // parse new data + rc_updated = dsm_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, + &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (rc_updated) { + // we have a new DSM frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; + fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, + false, false, frame_drops, dsm_rssi); + _rc_scan_locked = true; + } + } + + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_ST24); + } + + break; + + case RC_SCAN_ST24: + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // Configure serial port for DSM + dsm_config(_rcs_fd); + rc_io_invert(false, RC_UXART_BASE); + + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + if (newBytes > 0) { + // parse new data + uint8_t st24_rssi, lost_count; + + rc_updated = false; + + for (unsigned i = 0; i < (unsigned)newBytes; i++) { + /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; + rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, + &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); + } + + // The st24 will keep outputting RC channels and RSSI even if RC has been lost. + // The only way to detect RC loss is therefore to look at the lost_count. + + if (rc_updated) { + if (lost_count == 0) { + // we have a new ST24 frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24; + fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, + false, false, frame_drops, st24_rssi); + _rc_scan_locked = true; + + } else { + // if the lost count > 0 means that there is an RC loss + _rc_in.rc_lost = true; + } + } + } + + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_SUMD); + } + + break; + + case RC_SCAN_SUMD: + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // Configure serial port for DSM + dsm_config(_rcs_fd); + rc_io_invert(false, RC_UXART_BASE); + + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + if (newBytes > 0) { + // parse new data + uint8_t sumd_rssi, rx_count; + bool sumd_failsafe; + + rc_updated = false; + + for (unsigned i = 0; i < (unsigned)newBytes; i++) { + /* set updated flag if one complete packet was parsed */ + sumd_rssi = RC_INPUT_RSSI_MAX; + rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count, + &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe)); + } + + if (rc_updated) { + // we have a new SUMD frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD; + fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, + false, sumd_failsafe, frame_drops, sumd_rssi); + _rc_scan_locked = true; + } + } + + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_PPM); + } + + break; + + case RC_SCAN_PPM: + // skip PPM if it's not supported +#ifdef HRT_PPM_CHANNEL + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // Configure timer input pin for CPPM + px4_arch_configgpio(GPIO_PPM_IN); + rc_io_invert(false, RC_UXART_BASE); + + } else if (_rc_scan_locked || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + // see if we have new PPM input data + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) { + // we have a new PPM frame. Publish it. + rc_updated = true; + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; + 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 { + // disable CPPM input by mapping it away from the timer capture input + px4_arch_unconfiggpio(GPIO_PPM_IN); + // Scan the next protocol + set_rc_scan_state(RC_SCAN_CRSF); + } + +#else // skip PPM if it's not supported + set_rc_scan_state(RC_SCAN_CRSF); + +#endif // HRT_PPM_CHANNEL + + break; + + case RC_SCAN_CRSF: + if (_rc_scan_begin == 0) { + _rc_scan_begin = _cycle_timestamp; + // Configure serial port for CRSF + crsf_config(_rcs_fd); + rc_io_invert(false, RC_UXART_BASE); + + } else if (_rc_scan_locked + || _cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + // parse new data + if (newBytes > 0) { + rc_updated = crsf_parse(_cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, + input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (rc_updated) { + // we have a new CRSF frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF; + fill_rc_in(_raw_rc_count, _raw_rc_values, _cycle_timestamp, false, false, 0); + + // Enable CRSF Telemetry only on the Omnibus, because on Pixhawk (-related) boards + // we cannot write to the RC UART + // It might work on FMU-v5. Or another option is to use a different UART port +#ifdef CONFIG_ARCH_BOARD_OMNIBUS_F4SD + + if (!_rc_scan_locked && !_crsf_telemetry) { + _crsf_telemetry = new CRSFTelemetry(_rcs_fd); + } + +#endif /* CONFIG_ARCH_BOARD_OMNIBUS_F4SD */ + + _rc_scan_locked = true; + + if (_crsf_telemetry) { + _crsf_telemetry->update(_cycle_timestamp); + } + } + } + + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_SBUS); + } + + break; + } + +#else // RC_SERIAL_PORT not defined +#ifdef HRT_PPM_CHANNEL + + // see if we have new PPM input data + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) { + // we have a new PPM frame. Publish it. + rc_updated = true; + fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp, false, false, 0); + _rc_in.rc_ppm_frame_length = ppm_frame_length; + _rc_in.timestamp_last_signal = ppm_last_valid_decode; + } + +#endif // HRT_PPM_CHANNEL +#endif // RC_SERIAL_PORT + + if (rc_updated) { + int instance; + orb_publish_auto(ORB_ID(input_rc), &_to_input_rc, &_rc_in, &instance, ORB_PRIO_DEFAULT); + + } else if (!rc_updated && ((hrt_absolute_time() - _rc_in.timestamp_last_signal) > 1_s)) { + _rc_scan_locked = false; + } + + if (_run_as_task) { + if (should_exit()) { + break; + } + + } else { + if (should_exit()) { + exit_and_cleanup(); + + } else { + /* schedule next cycle */ + work_queue(HPWORK, &_work, (worker_t)&RCInput::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL)); + } + + break; + } + } +} + +#if defined(SPEKTRUM_POWER) +bool bind_spektrum(int arg) +{ + int ret = PX4_ERROR; + + /* specify 11ms DSMX. RX will automatically fall back to 22ms or DSM2 if necessary */ + + /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ + PX4_INFO("DSM_BIND_START: DSM%s RX", (arg == 0) ? "2" : ((arg == 1) ? "-X" : "-X8")); + + if (arg == DSM2_BIND_PULSES || + arg == DSMX_BIND_PULSES || + arg == DSMX8_BIND_PULSES) { + + dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0); + + dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0); + usleep(500000); + + dsm_bind(DSM_CMD_BIND_POWER_UP, 0); + usleep(72000); + + irqstate_t flags = px4_enter_critical_section(); + dsm_bind(DSM_CMD_BIND_SEND_PULSES, arg); + px4_leave_critical_section(flags); + + usleep(50000); + + dsm_bind(DSM_CMD_BIND_REINIT_UART, 0); + + ret = OK; + + } else { + PX4_ERR("DSM bind failed"); + ret = -EINVAL; + } + + return (ret == PX4_OK); +} +#endif /* SPEKTRUM_POWER */ + +RCInput *RCInput::instantiate(int argc, char *argv[]) +{ + // No arguments to parse. We also know that we should run as task + return new RCInput(true); +} + +int RCInput::custom_command(int argc, char *argv[]) +{ +#if defined(SPEKTRUM_POWER) + const char *verb = argv[0]; + + if (!strcmp(verb, "bind")) { + bind_spektrum(DSMX8_BIND_PULSES); + return 0; + } + +#endif /* SPEKTRUM_POWER */ + + /* start the FMU if not running */ + if (!is_running()) { + int ret = RCInput::task_spawn(argc, argv); + + if (ret) { + return ret; + } + } + + return print_usage("unknown command"); +} + +int RCInput::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module does the RC input parsing and auto-selecting the method. Supported methods are: +- PPM +- SBUS +- DSM +- SUMD +- ST24 +- TBS Crossfire (CRSF) + +### Implementation +By default the module runs on the work queue, to reduce RAM usage. It can also be run in its own thread, +specified via start flag -t, to reduce latency. +When running on the work queue, it schedules at a fixed frequency. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rc_input", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)"); + PRINT_MODULE_USAGE_PARAM_FLAG('t', "Run as separate task instead of the work queue", true); + +#if defined(SPEKTRUM_POWER) + PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a DSM bind command (module must be running)"); +#endif /* SPEKTRUM_POWER */ + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int RCInput::print_status() +{ + PX4_INFO("Running %s", (_run_as_task ? "as task" : "on work queue")); + + if (!_run_as_task) { + PX4_INFO("Max update rate: %i Hz", _current_update_rate); + } + + PX4_INFO("RC scan state: %s, locked: %s", RC_SCAN_STRING[_rc_scan_state], _rc_scan_locked ? "yes" : "no"); + PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no"); +#ifdef RC_SERIAL_PORT + PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames()); +#endif + + return 0; +} + +extern "C" __EXPORT int rc_input_main(int argc, char *argv[]); + +int +rc_input_main(int argc, char *argv[]) +{ + return RCInput::main(argc, argv); +} diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp new file mode 100644 index 0000000000..3491733350 --- /dev/null +++ b/src/drivers/rc_input/RCInput.hpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "crsf_telemetry.h" + +#ifdef HRT_PPM_CHANNEL +# include +#endif + +class RCInput : public ModuleBase +{ +public: + + RCInput(bool run_as_task); + virtual ~RCInput(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static RCInput *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + /** + * run the main loop: if running as task, continuously iterate, otherwise execute only one single cycle + */ + void cycle(); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + int init(); + +private: + enum RC_SCAN { + RC_SCAN_PPM = 0, + RC_SCAN_SBUS, + RC_SCAN_DSM, + RC_SCAN_SUMD, + RC_SCAN_ST24, + RC_SCAN_CRSF + }; + enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS; + + char const *RC_SCAN_STRING[6] = { + "PPM", + "SBUS", + "DSM", + "SUMD", + "ST24", + "CRSF" + }; + + hrt_abstime _rc_scan_begin{0}; + bool _rc_scan_locked{false}; + bool _report_lock{true}; + + hrt_abstime _cycle_timestamp{0}; + + unsigned _current_update_rate{0}; + bool _run_as_task{false}; + + static struct work_s _work; + + int _vehicle_cmd_sub{-1}; + int _adc_sub{-1}; + + rc_input_values _rc_in{}; + + float _analog_rc_rssi_volt{-1.0f}; + bool _analog_rc_rssi_stable{false}; + + orb_advert_t _to_input_rc{nullptr}; + + int _rcs_fd{-1}; + + uint8_t _rcs_buf[SBUS_BUFFER_SIZE] {}; + + uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; + uint16_t _raw_rc_count{}; + + CRSFTelemetry *_crsf_telemetry{nullptr}; + + static void cycle_trampoline(void *arg); + int start(); + + void fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi); + + void set_rc_scan_state(RC_SCAN _rc_scan_state); + + void rc_io_invert(bool invert, uint32_t uxart_base); + +}; diff --git a/src/drivers/px4fmu/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp similarity index 99% rename from src/drivers/px4fmu/crsf_telemetry.cpp rename to src/drivers/rc_input/crsf_telemetry.cpp index 1d218f0206..46bf069c9f 100644 --- a/src/drivers/px4fmu/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -204,4 +204,3 @@ bool CRSFTelemetry::send_flight_mode() return crsf_send_telemetry_flight_mode(_uart_fd, flight_mode); } - diff --git a/src/drivers/px4fmu/crsf_telemetry.h b/src/drivers/rc_input/crsf_telemetry.h similarity index 100% rename from src/drivers/px4fmu/crsf_telemetry.h rename to src/drivers/rc_input/crsf_telemetry.h diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index 330ada0f20..be69d03d21 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -42,6 +42,8 @@ #pragma once #include + +#include #include __BEGIN_DECLS @@ -85,4 +87,8 @@ enum DSM_CMD { /* DSM bind states */ DSM_CMD_BIND_REINIT_UART }; +#define DSM2_BIND_PULSES 3 /* DSM_BIND_START parameter, pulses required to start dsm2 pairing */ +#define DSMX_BIND_PULSES 7 /* DSM_BIND_START parameter, pulses required to start dsmx pairing */ +#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START parameter, pulses required to start 8 or more channel dsmx pairing */ + __END_DECLS