3 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 7 #include <drivers/drv_pwm_output.h> 8 #include <drivers/drv_hrt.h> 19 _perf_rcin = perf_alloc(PC_ELAPSED,
"APM_rcin");
20 _rc_sub = orb_subscribe(ORB_ID(input_rc));
27 #if HAL_RCINPUT_WITH_AP_RADIO 39 if (
_rcin.rc_failsafe) {
60 uint8_t n =
_rcin.channel_count;
76 if (ch >=
_rcin.channel_count) {
80 uint16_t
v =
_rcin.values[ch];
83 #if HAL_RCINPUT_WITH_AP_RADIO 84 if (radio && ch == 0) {
98 for (uint8_t i = 0; i < len; i++){
129 case input_rc_s::RC_INPUT_SOURCE_UNKNOWN:
return "UNKNOWN";
130 case input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM:
return "PX4FMU_PPM";
131 case input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM:
return "PX4IO_PPM";
132 case input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
return "PX4IO_SPEKTRUM";
133 case input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS:
return "PX4IO_SBUS";
134 case input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24:
return "PX4IO_ST24";
135 case input_rc_s::RC_INPUT_SOURCE_MAVLINK:
return "MAVLINK";
136 case input_rc_s::RC_INPUT_SOURCE_QURT:
return "QURT";
137 case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SPEKTRUM:
return "PX4FMU_SPEKTRUM";
138 case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS:
return "PX4FMU_SBUS";
139 case input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24:
return "PX4FMU_ST24";
140 case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD:
return "PX4FMU_SUMD";
141 case input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM:
return "PX4FMU_DSM";
142 case input_rc_s::RC_INPUT_SOURCE_PX4IO_SUMD:
return "PX4IO_SUMD";
143 case input_rc_s::RC_INPUT_SOURCE_PX4FMU_SRXL:
return "PX4FMU_SRXL";
144 case input_rc_s::RC_INPUT_SOURCE_PX4IO_SRXL:
return "PX4IO_SRXL";
145 default:
return "ERROR";
153 bool rc_updated =
false;
154 if (orb_check(
_rc_sub, &rc_updated) == 0 && rc_updated) {
164 #if HAL_RCINPUT_WITH_AP_RADIO 165 if (radio && radio->last_recv_us() != last_radio_us) {
166 last_radio_us = radio->last_recv_us();
168 _rcin.timestamp_last_signal = last_radio_us;
169 _rcin.channel_count = radio->num_channels();
170 for (uint8_t i=0; i<
_rcin.channel_count; i++) {
171 _rcin.values[i] = radio->read(i);
184 int fd =
open(
"/dev/px4io", 0);
186 fd =
open(
"/dev/px4fmu", 0);
189 hal.
console->
printf(
"RCInput: failed to open /dev/px4io or /dev/px4fmu\n");
193 #if HAL_RCINPUT_WITH_AP_RADIO 195 radio->start_recv_bind();
199 uint32_t mode = (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES);
200 int ret = ioctl(fd, DSM_BIND_START, mode);
int open(const char *pathname, int flags)
POSIX Open a file with integer mode flags.
AP_HAL::UARTDriver * console
Interface definition for the various Ground Control System.
static AP_Radio * instance(void)
virtual void printf(const char *,...) FMT_PRINTF(2
int close(int fileno)
POSIX Close a file with fileno handel.
void send_text(MAV_SEVERITY severity, const char *fmt,...)
void panic(const char *errormsg,...) FMT_PRINTF(1
static const AP_HAL::HAL & hal