diff options
Diffstat (limited to 'apps/px4io/comms.c')
-rw-r--r-- | apps/px4io/comms.c | 15 |
1 files changed, 9 insertions, 6 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index df07c34d8..c208db3ad 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -71,6 +71,8 @@ static struct px4io_report report; static void comms_handle_frame(void *arg, const void *buffer, size_t length); +perf_counter_t comms_rx_errors; + static void comms_init(void) { @@ -78,6 +80,9 @@ comms_init(void) fmu_fd = open("/dev/ttyS1", O_RDWR); stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); + comms_rx_errors = perf_alloc(PC_COUNT, "rx_err"); + hx_stream_set_counters(stream, 0, 0, comms_rx_errors); + /* default state in the report to FMU */ report.i2f_magic = I2F_MAGIC; @@ -173,14 +178,12 @@ comms_handle_command(const void *buffer, size_t length) irqstate_t flags = irqsave(); /* fetch new PWM output values */ - for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) { - system_state.fmu_channel_data[i] = cmd->servo_command[i]; - } + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) + system_state.fmu_channel_data[i] = cmd->output_control[i]; - /* if IO is armed and FMU gets disarmed, IO must also disarm */ - if (system_state.arm_ok && !cmd->arm_ok) { + /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ + if (system_state.arm_ok && !cmd->arm_ok) system_state.armed = false; - } system_state.arm_ok = cmd->arm_ok; system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; |