dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity
px4-firmware/apps/px4io/comms.c

330 lines
8.2 KiB
C
Raw Blame History

/****************************************************************************
*
* Copyright (C) 2012 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.
*
****************************************************************************/
/**
* @file comms.c
*
* FMU communication for the PX4IO module.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <debug.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <poll.h>
#include <termios.h>
#include <nuttx/clock.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/hx_stream.h>
#include <systemlib/perf_counter.h>
#define DEBUG
#include "px4io.h"
#define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */
#define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */
#define FMU_STATUS_INTERVAL 1000000 /* 100ms */
static int fmu_fd;
static hx_stream_t stream;
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)
{
/* initialise the FMU interface */
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;
struct termios t;
/* 115200bps, no parity, one stop bit */
tcgetattr(fmu_fd, &t);
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(fmu_fd, TCSANOW, &t);
/* init the ADC */
adc_init();
}
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;
}
}
}
static void
comms_handle_config(const void *buffer, size_t length)
{
const struct px4io_config *cfg = (struct px4io_config *)buffer;
if (length != sizeof(*cfg))
return;
/* fetch the rc mappings */
for (unsigned i = 0; i < 4; i++) {
system_state.rc_map[i] = cfg->rc_map[i];
}
/* fetch the rc channel attributes */
for (unsigned i = 0; i < 4; i++) {
system_state.rc_min[i] = cfg->rc_min[i];
system_state.rc_trim[i] = cfg->rc_trim[i];
system_state.rc_max[i] = cfg->rc_max[i];
system_state.rc_rev[i] = cfg->rc_rev[i];
system_state.rc_dz[i] = cfg->rc_dz[i];
}
}
static void
comms_handle_command(const void *buffer, size_t length)
{
const struct px4io_command *cmd = (struct px4io_command *)buffer;
if (length != sizeof(*cmd))
return;
irqstate_t flags = irqsave();
/* fetch new PWM output values */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
system_state.fmu_channel_data[i] = cmd->output_control[i];
/* 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;
system_state.manual_override_ok = cmd->manual_override_ok;
system_state.mixer_fmu_available = true;
system_state.fmu_data_received_time = hrt_absolute_time();
/* set PWM update rate if changed (after limiting) */
uint16_t new_servo_rate = cmd->servo_rate;
/* reject faster than 500 Hz updates */
if (new_servo_rate > 500) {
new_servo_rate = 500;
}
/* reject slower than 50 Hz updates */
if (new_servo_rate < 50) {
new_servo_rate = 50;
}
if (system_state.servo_rate != new_servo_rate) {
up_pwm_servo_set_rate(new_servo_rate);
system_state.servo_rate = new_servo_rate;
}
/*
* update servo values immediately.
* the updates are done in addition also
* in the mainloop, since this function will only
* update with a connected FMU.
*/
mixer_tick();
/* handle relay state changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
if (system_state.relays[i] != cmd->relay_state[i]) {
switch (i) {
case 0:
POWER_ACC1(cmd->relay_state[i]);
break;
case 1:
POWER_ACC2(cmd->relay_state[i]);
break;
case 2:
POWER_RELAY1(cmd->relay_state[i]);
break;
case 3:
POWER_RELAY2(cmd->relay_state[i]);
break;
}
}
system_state.relays[i] = cmd->relay_state[i];
}
irqrestore(flags);
}
static void
comms_handle_frame(void *arg, const void *buffer, size_t length)
{
const uint16_t *type = (const uint16_t *)buffer;
/* make sure it's what we are expecting */
if (length > 2) {
switch (*type) {
case F2I_MAGIC:
comms_handle_command(buffer, length);
break;
case F2I_CONFIG_MAGIC:
comms_handle_config(buffer, length);
break;
case F2I_MIXER_MAGIC:
mixer_handle_text(buffer, length);
break;
default:
break;
}
}
}
View Git Blame Copy Permalink