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_send()
{
lock();
/* send packet to IO while we're guaranteed it won't change */
hx_stream_send(_io_stream, &_next_command, sizeof(_next_command));
unlock();
}
static void
serial_callback(void *arg, const void *data, unsigned length)
{
const uint8_t *message = (const uint8_t *)data;
/* malformed frame, ignore it */
if (length < 2)
return;
/* it's a write operation, pass it to the register API */
if (message[0] & PX4IO_PAGE_WRITE) {
registers_set(message[0] & ~PX4IO_PAGE_WRITE, message[1],
(const uint16_t *)&message[2],
(length - 2) / 2);
return;
}
/* it's a read - must contain length byte */
if (length != 3)
return;
uint16_t *registers;
unsigned count;
tx_buf[0] = message[0];
tx_buf[1] = message[1];
/* get registers for response, send an empty reply on error */
if (registers_get(message[0], message[1], ®isters, &count) < 0)
count = 0;
/* fill buffer with message, limited by length */
#define TX_MAX ((sizeof(tx_buf) - 2) / 2)
if (count > TX_MAX)
count = TX_MAX;
if (count > message[2])
count = message[2];
memcpy(&tx_buf[2], registers, count * 2);
/* try to send the message */
hx_stream_send(if_stream, tx_buf, count * 2 + 2);
}
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));
}
}
