void comms_check(void) { static hrt_abstime last_report_time; hrt_abstime now, delta; uint8_t c; /* should we send a report to the FMU? */ now = hrt_absolute_time(); delta = now - last_report_time; if ((delta > FMU_MIN_REPORT_INTERVAL) && (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { system_state.fmu_report_due = false; last_report_time = now; /* populate the report */ for (unsigned i = 0; i < system_state.rc_channels; i++) report.rc_channel[i] = system_state.rc_channel_data[i]; report.channel_count = system_state.rc_channels; report.armed = system_state.armed; /* and send it */ hx_stream_send(stream, &report, sizeof(report)); } /* feed any received bytes to the HDLC receive engine */ while (read(fmu_fd, &c, 1) == 1) hx_stream_rx(stream, c); }
void PX4IO::io_recv() { uint8_t c; /* handle bytes from IO */ while (::read(_fd, &c, 1) == 1) hx_stream_rx(_io_stream, c); }
void interface_tick() { /* process incoming bytes */ hx_stream_rx(if_stream); }
void comms_main(void) { comms_init(); struct pollfd fds; fds.fd = fmu_fd; fds.events = POLLIN; debug("FMU: ready"); for (;;) { /* wait for serial data, but no more than 10ms */ poll(&fds, 1, 10); /* * Pull bytes from FMU and feed them to the HX engine. * Limit the number of bytes we actually process on any one iteration. */ if (fds.revents & POLLIN) { char buf[32]; ssize_t count = read(fmu_fd, buf, sizeof(buf)); for (int i = 0; i < count; i++) hx_stream_rx(stream, buf[i]); } /* * Decide if it's time to send an update to the FMU. */ static hrt_abstime last_report_time; hrt_abstime now, delta; /* should we send a report to the FMU? */ now = hrt_absolute_time(); delta = now - last_report_time; if ((delta > FMU_MIN_REPORT_INTERVAL) && (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { system_state.fmu_report_due = false; last_report_time = now; /* populate the report */ for (unsigned i = 0; i < system_state.rc_channels; i++) { report.rc_channel[i] = system_state.rc_channel_data[i]; } report.channel_count = system_state.rc_channels; report.armed = system_state.armed; report.battery_mv = system_state.battery_mv; report.adc_in = system_state.adc_in5; report.overcurrent = system_state.overcurrent; /* and send it */ hx_stream_send(stream, &report, sizeof(report)); } /* * Fetch ADC values, check overcurrent flags, etc. */ static hrt_abstime last_status_time; if ((now - last_status_time) > FMU_STATUS_INTERVAL) { /* * Coefficients here derived by measurement of the 5-16V * range on one unit: * * V counts * 5 1001 * 6 1219 * 7 1436 * 8 1653 * 9 1870 * 10 2086 * 11 2303 * 12 2522 * 13 2738 * 14 2956 * 15 3172 * 16 3389 * * slope = 0.0046067 * intercept = 0.3863 * * Intercept corrected for best results @ 12V. */ unsigned counts = adc_measure(ADC_VBATT); system_state.battery_mv = (4150 + (counts * 46)) / 10; system_state.adc_in5 = adc_measure(ADC_IN5); system_state.overcurrent = (OVERCURRENT_SERVO ? (1 << 0) : 0) | (OVERCURRENT_ACC ? (1 << 1) : 0); last_status_time = now; } } }
void comms_check(void) { /* * Check for serial data */ int ret = poll(pollfds, pollcount, 0); if (ret > 0) { /* * Pull bytes from FMU and feed them to the HX engine. * Limit the number of bytes we actually process on any one iteration. */ if (pollfds[0].revents & POLLIN) { char buf[32]; ssize_t count = read(fmu_fd, buf, sizeof(buf)); for (int i = 0; i < count; i++) hx_stream_rx(stream, buf[i]); } /* * Pull bytes from the serial RX port and feed them to the decoder * if we care about serial RX data. */ if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) { switch (system_state.serial_rx_mode) { case RX_MODE_DSM_10BIT: case RX_MODE_DSM_11BIT: dsm_input(rx_fd); break; case RX_MODE_FUTABA_SBUS: sbus_input(rx_fd); break; default: break; } } } /* * Decide if it's time to send an update to the FMU. */ static hrt_abstime last_report_time; hrt_abstime now, delta; /* should we send a report to the FMU? */ now = hrt_absolute_time(); delta = now - last_report_time; if ((delta > FMU_MIN_REPORT_INTERVAL) && (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { system_state.fmu_report_due = false; last_report_time = now; /* populate the report */ for (int i = 0; i < system_state.rc_channels; i++) report.rc_channel[i] = system_state.rc_channel_data[i]; report.channel_count = system_state.rc_channels; report.armed = system_state.armed; /* and send it */ hx_stream_send(stream, &report, sizeof(report)); } }