forked from Archive/PX4-Autopilot
Major workover of the PX4IO firmware for I2C operation.
This commit is contained in:
px4dev
parent
8ebe21b27b
commit
4e38615595
|
|
@ -39,11 +39,19 @@
|
|||
# Note that we pull a couple of specific files from the systemlib, since
|
||||
# we can't support it all.
|
||||
#
|
||||
CSRCS = $(wildcard *.c) \
|
||||
CSRCS = adc.c \
|
||||
controls.c \
|
||||
dsm.c \
|
||||
i2c.c \
|
||||
px4io.c \
|
||||
registers.c \
|
||||
safety.c \
|
||||
sbus.c \
|
||||
../systemlib/hx_stream.c \
|
||||
../systemlib/perf_counter.c \
|
||||
../systemlib/up_cxxinitialize.c
|
||||
CXXSRCS = $(wildcard *.cpp)
|
||||
|
||||
CXXSRCS = mixer.cpp
|
||||
|
||||
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
|
||||
|
||||
|
|
|
|||
|
|
@ -37,34 +37,22 @@
|
|||
* R/C inputs and servo outputs.
|
||||
*/
|
||||
|
||||
|
||||
#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 <termios.h>
|
||||
#include <string.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <nuttx/clock.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/hx_stream.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/ppm_decode.h>
|
||||
|
||||
#define DEBUG
|
||||
//#define DEBUG
|
||||
#include "px4io.h"
|
||||
|
||||
#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */
|
||||
#define RC_CHANNEL_HIGH_THRESH 1700
|
||||
#define RC_CHANNEL_LOW_THRESH 1300
|
||||
#define RC_CHANNEL_HIGH_THRESH 5000
|
||||
#define RC_CHANNEL_LOW_THRESH -5000
|
||||
|
||||
static void ppm_input(void);
|
||||
static bool ppm_input(uint16_t *values, uint16_t *num_values);
|
||||
|
||||
void
|
||||
controls_main(void)
|
||||
|
|
@ -79,9 +67,26 @@ controls_main(void)
|
|||
fds[1].fd = sbus_init("/dev/ttyS2");
|
||||
fds[1].events = POLLIN;
|
||||
|
||||
ASSERT(fds[0].fd >= 0);
|
||||
ASSERT(fds[1].fd >= 0);
|
||||
|
||||
/* default to a 1:1 input map */
|
||||
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
|
||||
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
|
||||
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 50;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
|
||||
}
|
||||
|
||||
for (;;) {
|
||||
/* run this loop at ~100Hz */
|
||||
poll(fds, 2, 10);
|
||||
int result = poll(fds, 2, 10);
|
||||
|
||||
ASSERT(result >= 0);
|
||||
|
||||
/*
|
||||
* Gather R/C control inputs from supported sources.
|
||||
|
|
@ -90,95 +95,212 @@ controls_main(void)
|
|||
* one control input source, they're going to fight each
|
||||
* other. Don't do that.
|
||||
*/
|
||||
bool locked = false;
|
||||
|
||||
bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
|
||||
if (dsm_updated)
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
|
||||
|
||||
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
|
||||
if (sbus_updated)
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
|
||||
|
||||
/*
|
||||
* Store RC channel count to detect switch to RC loss sooner
|
||||
* than just by timeout
|
||||
*/
|
||||
unsigned rc_channels = system_state.rc_channels;
|
||||
|
||||
if (fds[0].revents & POLLIN)
|
||||
locked |= dsm_input();
|
||||
|
||||
if (fds[1].revents & POLLIN)
|
||||
locked |= sbus_input();
|
||||
|
||||
/*
|
||||
* If we don't have lock from one of the serial receivers,
|
||||
* look for PPM. It shares an input with S.bus, so there's
|
||||
* a possibility it will mis-parse an S.bus frame.
|
||||
*
|
||||
* XXX each S.bus frame will cause a PPM decoder interrupt
|
||||
* storm (lots of edges). It might be sensible to actually
|
||||
* disable the PPM decoder completely if we have an alternate
|
||||
* receiver lock.
|
||||
* disable the PPM decoder completely if we have S.bus signal.
|
||||
*/
|
||||
if (!locked)
|
||||
ppm_input();
|
||||
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
|
||||
if (ppm_updated)
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
|
||||
|
||||
/* check for manual override status */
|
||||
if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
|
||||
/* force manual input override */
|
||||
system_state.mixer_manual_override = true;
|
||||
ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
|
||||
|
||||
} else {
|
||||
/* override not engaged, use FMU */
|
||||
system_state.mixer_manual_override = false;
|
||||
/*
|
||||
* In some cases we may have received a frame, but input has still
|
||||
* been lost.
|
||||
*/
|
||||
bool rc_input_lost = false;
|
||||
|
||||
/*
|
||||
* If we received a new frame from any of the RC sources, process it.
|
||||
*/
|
||||
if (dsm_updated | sbus_updated | ppm_updated) {
|
||||
|
||||
/* update RC-received timestamp */
|
||||
system_state.rc_channels_timestamp = hrt_absolute_time();
|
||||
|
||||
/* record a bitmask of channels assigned */
|
||||
unsigned assigned_channels = 0;
|
||||
|
||||
/* map raw inputs to mapped inputs */
|
||||
/* XXX mapping should be atomic relative to protocol */
|
||||
for (unsigned i = 0; i < r_raw_rc_count; i++) {
|
||||
|
||||
/* map the input channel */
|
||||
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] && PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
||||
|
||||
uint16_t raw = r_raw_rc_values[i];
|
||||
|
||||
/* implement the deadzone */
|
||||
if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) {
|
||||
raw += conf[PX4IO_P_RC_CONFIG_DEADZONE];
|
||||
if (raw > conf[PX4IO_P_RC_CONFIG_CENTER])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_CENTER];
|
||||
}
|
||||
if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) {
|
||||
raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE];
|
||||
if (raw < conf[PX4IO_P_RC_CONFIG_CENTER])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_CENTER];
|
||||
}
|
||||
|
||||
/* constrain to min/max values */
|
||||
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_MIN];
|
||||
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_MAX];
|
||||
|
||||
int16_t scaled = raw;
|
||||
|
||||
/* adjust to zero-relative (-500..500) */
|
||||
scaled -= 1500;
|
||||
|
||||
/* scale to fixed-point representation (-10000..10000) */
|
||||
scaled *= 20;
|
||||
|
||||
ASSERT(scaled >= -15000);
|
||||
ASSERT(scaled <= 15000);
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
|
||||
scaled = -scaled;
|
||||
|
||||
/* and update the scaled/mapped version */
|
||||
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
||||
ASSERT(mapped < MAX_CONTROL_CHANNELS);
|
||||
|
||||
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
|
||||
assigned_channels |= (1 << mapped);
|
||||
}
|
||||
}
|
||||
|
||||
/* set un-assigned controls to zero */
|
||||
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
|
||||
if (!(assigned_channels & (1 << i)))
|
||||
r_rc_values[i] = 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* If we got an update with zero channels, treat it as
|
||||
* a loss of input.
|
||||
*/
|
||||
if (assigned_channels == 0)
|
||||
rc_input_lost = true;
|
||||
|
||||
/*
|
||||
* Export the valid channel bitmap
|
||||
*/
|
||||
r_rc_valid = assigned_channels;
|
||||
}
|
||||
|
||||
/*
|
||||
* If we haven't seen any new control data in 200ms, assume we
|
||||
* have lost input and tell FMU.
|
||||
* have lost input.
|
||||
*/
|
||||
if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
|
||||
|
||||
if (system_state.rc_channels > 0) {
|
||||
/*
|
||||
* If the RC signal status (lost / present) has
|
||||
* just changed, request an update immediately.
|
||||
*/
|
||||
system_state.fmu_report_due = true;
|
||||
}
|
||||
|
||||
/* set the number of channels to zero - no inputs */
|
||||
system_state.rc_channels = 0;
|
||||
rc_input_lost = true;
|
||||
}
|
||||
|
||||
/*
|
||||
* PWM output updates are performed in addition on each comm update.
|
||||
* the updates here are required to ensure operation if FMU is not started
|
||||
* or stopped responding.
|
||||
* Handle losing RC input
|
||||
*/
|
||||
if (rc_input_lost) {
|
||||
|
||||
/* Clear the RC input status flags, clear manual override control */
|
||||
r_status_flags &= ~(
|
||||
PX4IO_P_STATUS_FLAGS_OVERRIDE |
|
||||
PX4IO_P_STATUS_FLAGS_RC_OK |
|
||||
PX4IO_P_STATUS_FLAGS_RC_PPM |
|
||||
PX4IO_P_STATUS_FLAGS_RC_DSM |
|
||||
PX4IO_P_STATUS_FLAGS_RC_SBUS);
|
||||
|
||||
/* Set the RC_LOST alarm */
|
||||
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
|
||||
|
||||
/* Mark the arrays as empty */
|
||||
r_raw_rc_count = 0;
|
||||
r_rc_valid = 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* Check for manual override.
|
||||
*
|
||||
* The OVERRIDE_OK feature must be set, and we must have R/C input.
|
||||
* Override is enabled if either the hardcoded channel / value combination
|
||||
* is selected, or the AP has requested it.
|
||||
*/
|
||||
if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
|
||||
|
||||
bool override = false;
|
||||
|
||||
/*
|
||||
* Check mapped channel 5; if the value is 'high' then the pilot has
|
||||
* requested override.
|
||||
*/
|
||||
if ((r_rc_valid & (1 << 4)) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH))
|
||||
override = true;
|
||||
|
||||
/*
|
||||
* Check for an explicit manual override request from the AP.
|
||||
*/
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE))
|
||||
override = true;
|
||||
|
||||
if (override) {
|
||||
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
|
||||
|
||||
/* mix new RC input control values to servos */
|
||||
if (dsm_updated | sbus_updated | ppm_updated)
|
||||
mixer_tick();
|
||||
|
||||
} else {
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
ppm_input(void)
|
||||
}
|
||||
}
|
||||
|
||||
static bool
|
||||
ppm_input(uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
/*
|
||||
* Look for new PPM input.
|
||||
*/
|
||||
if (ppm_last_valid_decode != 0) {
|
||||
bool result = false;
|
||||
|
||||
/* avoid racing with PPM updates */
|
||||
irqstate_t state = irqsave();
|
||||
|
||||
/*
|
||||
* Look for recent PPM input.
|
||||
*/
|
||||
if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) {
|
||||
|
||||
/* PPM data exists, copy it */
|
||||
system_state.rc_channels = ppm_decoded_channels;
|
||||
result = true;
|
||||
*num_values = ppm_decoded_channels;
|
||||
if (*num_values > MAX_CONTROL_CHANNELS)
|
||||
*num_values = MAX_CONTROL_CHANNELS;
|
||||
|
||||
for (unsigned i = 0; i < ppm_decoded_channels; i++) {
|
||||
system_state.rc_channel_data[i] = ppm_buffer[i];
|
||||
}
|
||||
for (unsigned i = 0; i < *num_values; i++)
|
||||
values[i] = ppm_buffer[i];
|
||||
|
||||
/* copy the timestamp and clear it */
|
||||
system_state.rc_channels_timestamp = ppm_last_valid_decode;
|
||||
/* clear validity */
|
||||
ppm_last_valid_decode = 0;
|
||||
}
|
||||
|
||||
irqrestore(state);
|
||||
|
||||
/* trigger an immediate report to the FMU */
|
||||
system_state.fmu_report_due = true;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -43,17 +43,13 @@
|
|||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include <systemlib/ppm_decode.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#define DEBUG
|
||||
|
||||
#include "px4io.h"
|
||||
#include "protocol.h"
|
||||
|
||||
#define DSM_FRAME_SIZE 16
|
||||
#define DSM_FRAME_CHANNELS 7
|
||||
|
|
@ -72,13 +68,13 @@ unsigned dsm_frame_drops;
|
|||
|
||||
static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
|
||||
static void dsm_guess_format(bool reset);
|
||||
static void dsm_decode(hrt_abstime now);
|
||||
static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
|
||||
|
||||
int
|
||||
dsm_init(const char *device)
|
||||
{
|
||||
if (dsm_fd < 0)
|
||||
dsm_fd = open(device, O_RDONLY);
|
||||
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
|
||||
|
||||
if (dsm_fd >= 0) {
|
||||
struct termios t;
|
||||
|
|
@ -106,7 +102,7 @@ dsm_init(const char *device)
|
|||
}
|
||||
|
||||
bool
|
||||
dsm_input(void)
|
||||
dsm_input(uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
ssize_t ret;
|
||||
hrt_abstime now;
|
||||
|
|
@ -143,7 +139,7 @@ dsm_input(void)
|
|||
|
||||
/* if the read failed for any reason, just give up here */
|
||||
if (ret < 1)
|
||||
goto out;
|
||||
return false;
|
||||
|
||||
last_rx_time = now;
|
||||
|
||||
|
|
@ -156,20 +152,14 @@ dsm_input(void)
|
|||
* If we don't have a full frame, return
|
||||
*/
|
||||
if (partial_frame_count < DSM_FRAME_SIZE)
|
||||
goto out;
|
||||
return false;
|
||||
|
||||
/*
|
||||
* Great, it looks like we might have a frame. Go ahead and
|
||||
* decode it.
|
||||
*/
|
||||
dsm_decode(now);
|
||||
partial_frame_count = 0;
|
||||
|
||||
out:
|
||||
/*
|
||||
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
|
||||
*/
|
||||
return (now - last_frame_time) < 200000;
|
||||
return dsm_decode(now, values, num_values);
|
||||
}
|
||||
|
||||
static bool
|
||||
|
|
@ -273,8 +263,8 @@ dsm_guess_format(bool reset)
|
|||
dsm_guess_format(true);
|
||||
}
|
||||
|
||||
static void
|
||||
dsm_decode(hrt_abstime frame_time)
|
||||
static bool
|
||||
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
|
||||
/*
|
||||
|
|
@ -295,7 +285,7 @@ dsm_decode(hrt_abstime frame_time)
|
|||
/* if we don't know the frame format, update the guessing state machine */
|
||||
if (channel_shift == 0) {
|
||||
dsm_guess_format(false);
|
||||
return;
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -323,8 +313,8 @@ dsm_decode(hrt_abstime frame_time)
|
|||
continue;
|
||||
|
||||
/* update the decoded channel count */
|
||||
if (channel >= system_state.rc_channels)
|
||||
system_state.rc_channels = channel + 1;
|
||||
if (channel >= *num_values)
|
||||
*num_values = channel + 1;
|
||||
|
||||
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
|
||||
if (channel_shift == 11)
|
||||
|
|
@ -355,13 +345,11 @@ dsm_decode(hrt_abstime frame_time)
|
|||
break;
|
||||
}
|
||||
|
||||
system_state.rc_channel_data[channel] = value;
|
||||
values[channel] = value;
|
||||
}
|
||||
|
||||
/* and note that we have received data from the R/C controller */
|
||||
/* XXX failsafe will cause problems here - need a strategy for detecting it */
|
||||
system_state.rc_channels_timestamp = frame_time;
|
||||
|
||||
/* trigger an immediate report to the FMU */
|
||||
system_state.fmu_report_due = true;
|
||||
/*
|
||||
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
|
||||
*/
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -259,7 +259,6 @@ i2c_tx_complete(void)
|
|||
/* XXX handle transmit-done */
|
||||
|
||||
/* prepare for the next transaction */
|
||||
memset(tx_buf, 0, sizeof(tx_buf));
|
||||
i2c_tx_setup();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -38,22 +38,13 @@
|
|||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/arch.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <errno.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <sched.h>
|
||||
|
||||
#include <debug.h>
|
||||
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/device/device.h>
|
||||
|
||||
#include <systemlib/mixer/mixer.h>
|
||||
|
||||
|
|
@ -78,10 +69,12 @@ extern "C" {
|
|||
bool mixer_servos_armed = false;
|
||||
|
||||
/* selected control values and count for mixing */
|
||||
static uint16_t *control_values;
|
||||
static int control_count;
|
||||
|
||||
static uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
|
||||
enum mixer_source {
|
||||
MIX_FMU,
|
||||
MIX_OVERRIDE,
|
||||
MIX_FAILSAFE
|
||||
};
|
||||
static mixer_source source;
|
||||
|
||||
static int mixer_callback(uintptr_t handle,
|
||||
uint8_t control_group,
|
||||
|
|
@ -93,23 +86,32 @@ static MixerGroup mixer_group(mixer_callback, 0);
|
|||
void
|
||||
mixer_tick(void)
|
||||
{
|
||||
bool should_arm;
|
||||
|
||||
/* check that we are receiving fresh data from the FMU */
|
||||
if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
|
||||
|
||||
/* too many frames without FMU input, time to go to failsafe */
|
||||
system_state.mixer_manual_override = true;
|
||||
system_state.mixer_fmu_available = false;
|
||||
lib_lowprintf("RX timeout\n");
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_FMU_OK;
|
||||
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
|
||||
debug("AP RX timeout");
|
||||
}
|
||||
|
||||
/*
|
||||
* Decide which set of inputs we're using.
|
||||
* Decide which set of controls we're using.
|
||||
*/
|
||||
|
||||
if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE)) {
|
||||
/* this is for planes, where manual override makes sense */
|
||||
if (system_state.manual_override_ok) {
|
||||
source = MIX_OVERRIDE;
|
||||
} else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
|
||||
source = MIX_FMU;
|
||||
} else {
|
||||
source = MIX_FAILSAFE;
|
||||
}
|
||||
|
||||
#if 0
|
||||
/* if everything is ok */
|
||||
|
||||
if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
|
||||
/* we have recent control data from the FMU */
|
||||
control_count = PX4IO_CONTROL_CHANNELS;
|
||||
|
|
@ -170,11 +172,10 @@ mixer_tick(void)
|
|||
control_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
/*
|
||||
* Run the mixers if we have any control data at all.
|
||||
* Run the mixers.
|
||||
*/
|
||||
if (control_count > 0) {
|
||||
float outputs[IO_SERVO_COUNT];
|
||||
unsigned mixed;
|
||||
|
||||
|
|
@ -182,31 +183,29 @@ mixer_tick(void)
|
|||
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
|
||||
|
||||
/* scale to PWM and update the servo outputs as required */
|
||||
for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
|
||||
if (i < mixed) {
|
||||
/* scale to servo output */
|
||||
system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
|
||||
/* save actuator values for FMU readback */
|
||||
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
|
||||
|
||||
/* scale to servo output */
|
||||
r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
|
||||
|
||||
} else {
|
||||
/* set to zero to inhibit PWM pulse output */
|
||||
system_state.servos[i] = 0;
|
||||
}
|
||||
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
|
||||
r_page_servos[i] = 0;
|
||||
|
||||
/*
|
||||
* If we are armed, update the servo output.
|
||||
* Update the servo outputs.
|
||||
*/
|
||||
if (system_state.armed && system_state.arm_ok) {
|
||||
up_pwm_servo_set(i, system_state.servos[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
|
||||
up_pwm_servo_set(i, r_page_servos[i]);
|
||||
|
||||
/*
|
||||
* Decide whether the servos should be armed right now.
|
||||
* A sufficient reason is armed state and either FMU or RC control inputs
|
||||
*/
|
||||
|
||||
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
|
||||
bool should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED);
|
||||
|
||||
if (should_arm && !mixer_servos_armed) {
|
||||
/* need to arm, but not armed */
|
||||
|
|
@ -226,21 +225,33 @@ mixer_callback(uintptr_t handle,
|
|||
uint8_t control_index,
|
||||
float &control)
|
||||
{
|
||||
/* if the control index refers to an input that's not valid, we can't return it */
|
||||
if (control_index >= control_count)
|
||||
if (control_group != 0)
|
||||
return -1;
|
||||
|
||||
/* scale from current PWM units (1000-2000) to mixer input values */
|
||||
if (system_state.manual_override_ok && system_state.mixer_manual_override && control_index == 3) {
|
||||
control = ((float)control_values[control_index] - 1000.0f) / 1000.0f;
|
||||
} else {
|
||||
control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
|
||||
switch (source) {
|
||||
case MIX_FMU:
|
||||
if (control_index < PX4IO_CONTROL_CHANNELS) {
|
||||
control = REG_TO_FLOAT(r_page_controls[control_index]);
|
||||
break;
|
||||
}
|
||||
return -1;
|
||||
|
||||
case MIX_OVERRIDE:
|
||||
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
|
||||
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
|
||||
break;
|
||||
}
|
||||
return -1;
|
||||
|
||||
case MIX_FAILSAFE:
|
||||
/* XXX we could allow for configuration of per-output failsafe values */
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static char mixer_text[256];
|
||||
static char mixer_text[256]; /* large enough for one mixer */
|
||||
static unsigned mixer_text_length = 0;
|
||||
|
||||
void
|
||||
|
|
@ -267,6 +278,7 @@ mixer_handle_text(const void *buffer, size_t length)
|
|||
debug("append %d", length);
|
||||
|
||||
/* check for overflow - this is really fatal */
|
||||
/* XXX could add just what will fit & try to parse, then repeat... */
|
||||
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
|
||||
return;
|
||||
|
||||
|
|
|
|||
|
|
@ -55,7 +55,9 @@
|
|||
*
|
||||
* As convention, values that would be floating point in other parts of
|
||||
* the PX4 system are expressed as signed integer values scaled by 10000,
|
||||
* e.g. control values range from -10000..10000.
|
||||
* e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and
|
||||
* SIGNED_TO_REG macros to convert between register representation and
|
||||
* the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float.
|
||||
*
|
||||
* Note that the implementation of readable pages prefers registers within
|
||||
* readable pages to be densely packed. Page numbers do not need to be
|
||||
|
|
@ -66,6 +68,12 @@
|
|||
#define PX4IO_INPUT_CHANNELS 12
|
||||
#define PX4IO_RELAY_CHANNELS 4
|
||||
|
||||
/* Per C, this is safe for all 2's complement systems */
|
||||
#define REG_TO_SIGNED(_reg) ((int16_t)(_reg))
|
||||
#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed))
|
||||
|
||||
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
|
||||
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
|
||||
|
||||
/* static configuration page */
|
||||
#define PX4IO_PAGE_CONFIG 0
|
||||
|
|
@ -91,13 +99,15 @@
|
|||
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */
|
||||
|
||||
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
|
||||
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
|
||||
#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */
|
||||
#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */
|
||||
#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */
|
||||
#define PX4IO_P_STATUS_ALARMS_AP_LOST (1 << 4)
|
||||
#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */
|
||||
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5)
|
||||
|
||||
#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */
|
||||
#define PX4IO_P_STATUS_TEMPERATURE 5 /* temperature in (units tbd) */
|
||||
|
|
@ -109,19 +119,26 @@
|
|||
#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* array of raw RC input values, microseconds */
|
||||
#define PX4IO_PAGE_RAW_RC_INPUT 4 /* 0..CONFIG_RC_INPUT_COUNT-1 */
|
||||
#define PX4IO_PAGE_RAW_RC_INPUT 4
|
||||
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
|
||||
#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */
|
||||
|
||||
/* array of scaled RC input values, -10000..10000 */
|
||||
#define PX4IO_PAGE_RC_INPUT 5 /* 0..CONFIG_RC_INPUT_COUNT-1 */
|
||||
#define PX4IO_PAGE_RC_INPUT 5
|
||||
#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */
|
||||
#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */
|
||||
|
||||
/* array of raw ADC values */
|
||||
#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
|
||||
|
||||
/* setup page */
|
||||
#define PX4IO_PAGE_SETUP 100
|
||||
#define PX4IO_P_SETUP_FEATURES 0
|
||||
#define PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK (1 << 0) /* OK to switch to manual override */
|
||||
|
||||
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
|
||||
#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */
|
||||
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE (1 << 1) /* request local override */
|
||||
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE (1 << 2) /* request switch to manual override */
|
||||
|
||||
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
|
||||
|
||||
|
|
@ -136,7 +153,17 @@
|
|||
/* raw text load to the mixer parser - ignores offset */
|
||||
#define PX4IO_PAGE_MIXERLOAD 102
|
||||
|
||||
|
||||
/* R/C channel config */
|
||||
#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
|
||||
#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */
|
||||
#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */
|
||||
#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */
|
||||
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */
|
||||
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
|
||||
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
|
||||
|
||||
/*
|
||||
* Old serial PX4FMU <-> PX4IO messaging protocol.
|
||||
|
|
|
|||
|
|
@ -37,22 +37,22 @@
|
|||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <stdio.h> // required for task_create
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <debug.h>
|
||||
#include <stdlib.h>
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <nuttx/clock.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
#include <stm32_uart.h>
|
||||
|
||||
#define DEBUG
|
||||
#include "px4io.h"
|
||||
|
||||
__EXPORT int user_start(int argc, char *argv[]);
|
||||
|
|
@ -70,8 +70,6 @@ int user_start(int argc, char *argv[])
|
|||
|
||||
/* reset all to zero */
|
||||
memset(&system_state, 0, sizeof(system_state));
|
||||
/* default to 50 Hz PWM outputs */
|
||||
system_state.servo_rate = 50;
|
||||
|
||||
/* configure the high-resolution time/callout interface */
|
||||
hrt_init();
|
||||
|
|
@ -83,7 +81,7 @@ int user_start(int argc, char *argv[])
|
|||
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
|
||||
|
||||
/* print some startup info */
|
||||
lib_lowprintf("\nPX4IO: starting\n");
|
||||
debug("\nPX4IO: starting\n");
|
||||
|
||||
/* default all the LEDs to off while we start */
|
||||
LED_AMBER(false);
|
||||
|
|
@ -108,11 +106,19 @@ int user_start(int argc, char *argv[])
|
|||
|
||||
|
||||
struct mallinfo minfo = mallinfo();
|
||||
lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
|
||||
debug("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
|
||||
|
||||
/* start the i2c test code */
|
||||
/* start the i2c handler */
|
||||
i2c_init();
|
||||
|
||||
/* we're done here, go run the communications loop */
|
||||
comms_main();
|
||||
/* add a performance counter for mixing */
|
||||
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
|
||||
|
||||
/* run the mixer at 100Hz (for now...) */
|
||||
for (;;) {
|
||||
poll(NULL, 0, 10);
|
||||
perf_begin(mixer_perf);
|
||||
mixer_tick();
|
||||
perf_end(mixer_perf);
|
||||
}
|
||||
}
|
||||
|
|
@ -75,6 +75,29 @@ extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */
|
|||
|
||||
extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
|
||||
extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
|
||||
extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
|
||||
|
||||
/*
|
||||
* Register aliases.
|
||||
*
|
||||
* Handy aliases for registers that are widely used.
|
||||
*/
|
||||
#define r_status_flags r_page_status[PX4IO_P_STATUS_FLAGS]
|
||||
#define r_status_alarms r_page_status[PX4IO_P_STATUS_ALARMS]
|
||||
|
||||
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
|
||||
#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
|
||||
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
|
||||
#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
|
||||
|
||||
#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
|
||||
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]
|
||||
#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES]
|
||||
#define r_setup_pwm_lowrate r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE]
|
||||
#define r_setup_pwm_highrate r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE]
|
||||
#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS]
|
||||
|
||||
#define r_control_values (&r_page_controls[0])
|
||||
|
||||
/*
|
||||
* System state structure.
|
||||
|
|
@ -84,9 +107,9 @@ extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
|
|||
*/
|
||||
struct sys_state_s {
|
||||
|
||||
bool armed; /* IO armed */
|
||||
bool arm_ok; /* FMU says OK to arm */
|
||||
uint16_t servo_rate; /* output rate of servos in Hz */
|
||||
// bool armed; /* IO armed */
|
||||
// bool arm_ok; /* FMU says OK to arm */
|
||||
// uint16_t servo_rate; /* output rate of servos in Hz */
|
||||
|
||||
/**
|
||||
* Remote control input(s) channel mappings
|
||||
|
|
@ -105,39 +128,39 @@ struct sys_state_s {
|
|||
/**
|
||||
* Data from the remote control input(s)
|
||||
*/
|
||||
unsigned rc_channels;
|
||||
uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
|
||||
// unsigned rc_channels;
|
||||
// uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
|
||||
uint64_t rc_channels_timestamp;
|
||||
|
||||
/**
|
||||
* Control signals from FMU.
|
||||
*/
|
||||
uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS];
|
||||
// uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS];
|
||||
|
||||
/**
|
||||
* Mixed servo outputs
|
||||
*/
|
||||
uint16_t servos[IO_SERVO_COUNT];
|
||||
// uint16_t servos[IO_SERVO_COUNT];
|
||||
|
||||
/**
|
||||
* Relay controls
|
||||
*/
|
||||
bool relays[PX4IO_RELAY_CHANNELS];
|
||||
// bool relays[PX4IO_RELAY_CHANNELS];
|
||||
|
||||
/**
|
||||
* If true, we are using the FMU controls, else RC input if available.
|
||||
*/
|
||||
bool mixer_manual_override;
|
||||
// bool mixer_manual_override;
|
||||
|
||||
/**
|
||||
* If true, FMU input is available.
|
||||
*/
|
||||
bool mixer_fmu_available;
|
||||
// bool mixer_fmu_available;
|
||||
|
||||
/**
|
||||
* If true, state that should be reported to FMU has been updated.
|
||||
*/
|
||||
bool fmu_report_due;
|
||||
// bool fmu_report_due;
|
||||
|
||||
/**
|
||||
* Last FMU receive time, in microseconds since system boot
|
||||
|
|
@ -147,12 +170,12 @@ struct sys_state_s {
|
|||
/**
|
||||
* If true, the RC signal has been lost for more than a timeout interval
|
||||
*/
|
||||
bool rc_lost;
|
||||
// bool rc_lost;
|
||||
|
||||
/**
|
||||
* If true, the connection to FMU has been lost for more than a timeout interval
|
||||
*/
|
||||
bool fmu_lost;
|
||||
// bool fmu_lost;
|
||||
|
||||
/**
|
||||
* If true, FMU is ready for autonomous position control. Only used for LED indication
|
||||
|
|
@ -162,41 +185,27 @@ struct sys_state_s {
|
|||
/**
|
||||
* If true, IO performs an on-board manual override with the RC channel values
|
||||
*/
|
||||
bool manual_override_ok;
|
||||
// bool manual_override_ok;
|
||||
|
||||
/*
|
||||
* Measured battery voltage in mV
|
||||
*/
|
||||
uint16_t battery_mv;
|
||||
// uint16_t battery_mv;
|
||||
|
||||
/*
|
||||
* ADC IN5 measurement
|
||||
*/
|
||||
uint16_t adc_in5;
|
||||
// uint16_t adc_in5;
|
||||
|
||||
/*
|
||||
* Power supply overcurrent status bits.
|
||||
*/
|
||||
uint8_t overcurrent;
|
||||
// uint8_t overcurrent;
|
||||
|
||||
};
|
||||
|
||||
extern struct sys_state_s system_state;
|
||||
|
||||
#if 0
|
||||
/*
|
||||
* Software countdown timers.
|
||||
*
|
||||
* Each timer counts down to zero at one tick per ms.
|
||||
*/
|
||||
#define TIMER_BLINK_AMBER 0
|
||||
#define TIMER_BLINK_BLUE 1
|
||||
#define TIMER_STATUS_PRINT 2
|
||||
#define TIMER_SANITY 7
|
||||
#define TIMER_NUM_TIMERS 8
|
||||
extern volatile int timers[TIMER_NUM_TIMERS];
|
||||
#endif
|
||||
|
||||
/*
|
||||
* GPIO handling.
|
||||
*/
|
||||
|
|
@ -249,9 +258,11 @@ extern uint16_t adc_measure(unsigned channel);
|
|||
|
||||
/*
|
||||
* R/C receiver handling.
|
||||
*
|
||||
* Input functions return true when they receive an update from the RC controller.
|
||||
*/
|
||||
extern void controls_main(void);
|
||||
extern int dsm_init(const char *device);
|
||||
extern bool dsm_input(void);
|
||||
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
|
||||
extern int sbus_init(const char *device);
|
||||
extern bool sbus_input(void);
|
||||
extern bool sbus_input(uint16_t *values, uint16_t *num_values);
|
||||
|
|
|
|||
|
|
@ -37,6 +37,11 @@
|
|||
* Implementation of the PX4IO register space.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "px4io.h"
|
||||
#include "protocol.h"
|
||||
|
||||
|
|
@ -45,8 +50,9 @@ static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
|
|||
/**
|
||||
* Setup registers
|
||||
*/
|
||||
uint16_t r_page_setup[] =
|
||||
volatile uint16_t r_page_setup[] =
|
||||
{
|
||||
[PX4IO_P_SETUP_FEATURES] = 0,
|
||||
[PX4IO_P_SETUP_ARMING] = 0,
|
||||
[PX4IO_P_SETUP_PWM_RATES] = 0,
|
||||
[PX4IO_P_SETUP_PWM_LOWRATE] = 50,
|
||||
|
|
@ -54,14 +60,16 @@ uint16_t r_page_setup[] =
|
|||
[PX4IO_P_SETUP_RELAYS] = 0,
|
||||
};
|
||||
|
||||
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)
|
||||
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK)
|
||||
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \
|
||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)
|
||||
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
|
||||
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PXIO_RELAY_CHANNELS) - 1)
|
||||
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
|
||||
|
||||
/**
|
||||
* Control values from the FMU.
|
||||
*/
|
||||
uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
|
||||
volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
|
||||
|
||||
/**
|
||||
* Static configuration parameters.
|
||||
|
|
@ -108,13 +116,26 @@ uint16_t r_page_servos[IO_SERVO_COUNT];
|
|||
/**
|
||||
* Scaled/routed RC input
|
||||
*/
|
||||
uint16_t r_page_rc_input[MAX_CONTROL_CHANNELS];
|
||||
uint16_t r_page_rc_input[] = {
|
||||
[PX4IO_P_RC_VALID] = 0,
|
||||
[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
* Raw RC input
|
||||
*/
|
||||
uint16_t r_page_raw_rc_input[MAX_CONTROL_CHANNELS];
|
||||
uint16_t r_page_raw_rc_input[] =
|
||||
{
|
||||
[PX4IO_P_RAW_RC_COUNT] = 0,
|
||||
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
* R/C channel input configuration.
|
||||
*/
|
||||
uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID PX4IO_P_RC_CONFIG_OPTIONS_REVERSE
|
||||
|
||||
void
|
||||
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
|
||||
|
|
@ -136,7 +157,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
|||
}
|
||||
|
||||
/* XXX we should cause a mixer tick ASAP */
|
||||
system_state.mixer_fmu_available = true;
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
|
||||
break;
|
||||
|
||||
/* handle text going to the mixer parser */
|
||||
|
|
@ -168,7 +189,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
switch (offset) {
|
||||
case PX4IO_P_STATUS_ALARMS:
|
||||
/* clear bits being written */
|
||||
r_page_status[PX4IO_P_STATUS_ALARMS] &= ~value;
|
||||
r_status_alarms &= ~value;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -179,20 +200,30 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
|
||||
case PX4IO_PAGE_SETUP:
|
||||
switch (offset) {
|
||||
case PX4IO_P_SETUP_FEATURES:
|
||||
|
||||
value &= PX4IO_P_SETUP_FEATURES_VALID;
|
||||
r_setup_features = value;
|
||||
|
||||
/* update manual override state - disable if no longer OK */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && !(value & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK))
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
|
||||
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_ARMING:
|
||||
|
||||
value &= PX4IO_P_SETUP_ARMING_VALID;
|
||||
r_page_setup[PX4IO_P_SETUP_ARMING] = value;
|
||||
r_setup_arming = value;
|
||||
|
||||
/* update arming state - disarm if no longer OK */
|
||||
if (system_state.armed && !(value & PX4IO_P_SETUP_ARMING_ARM_OK))
|
||||
system_state.armed = false;
|
||||
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK))
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_PWM_RATES:
|
||||
value &= PX4IO_P_SETUP_RATES_VALID;
|
||||
r_page_setup[PX4IO_P_SETUP_PWM_RATES] = value;
|
||||
r_setup_pwm_rates = value;
|
||||
/* XXX re-configure timers */
|
||||
break;
|
||||
|
||||
|
|
@ -201,7 +232,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
value = 50;
|
||||
if (value > 400)
|
||||
value = 400;
|
||||
r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] = value;
|
||||
r_setup_pwm_lowrate = value;
|
||||
/* XXX re-configure timers */
|
||||
break;
|
||||
|
||||
|
|
@ -210,13 +241,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
value = 50;
|
||||
if (value > 400)
|
||||
value = 400;
|
||||
r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] = value;
|
||||
r_setup_pwm_highrate = value;
|
||||
/* XXX re-configure timers */
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_RELAYS:
|
||||
value &= PX4IO_P_SETUP_RELAYS_VALID;
|
||||
r_page_setup[PX4IO_P_SETUP_RELAYS] = value;
|
||||
r_setup_relays = value;
|
||||
POWER_RELAY1(value & (1 << 0) ? 1 : 0);
|
||||
POWER_RELAY2(value & (1 << 1) ? 1 : 0);
|
||||
POWER_ACC1(value & (1 << 2) ? 1 : 0);
|
||||
|
|
@ -228,6 +259,63 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
}
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_RC_CONFIG: {
|
||||
unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
|
||||
unsigned index = offset % PX4IO_P_RC_CONFIG_STRIDE;
|
||||
uint16_t *conf = &r_page_rc_input_config[offset * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
if (channel >= MAX_CONTROL_CHANNELS)
|
||||
return -1;
|
||||
|
||||
/* disable the channel until we have a chance to sanity-check it */
|
||||
conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
|
||||
switch (index) {
|
||||
|
||||
case PX4IO_P_RC_CONFIG_MIN:
|
||||
case PX4IO_P_RC_CONFIG_CENTER:
|
||||
case PX4IO_P_RC_CONFIG_MAX:
|
||||
case PX4IO_P_RC_CONFIG_DEADZONE:
|
||||
case PX4IO_P_RC_CONFIG_ASSIGNMENT:
|
||||
conf[index] = value;
|
||||
break;
|
||||
|
||||
case PX4IO_P_RC_CONFIG_OPTIONS:
|
||||
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
|
||||
|
||||
/* set all options except the enabled option */
|
||||
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
|
||||
/* should the channel be enabled? */
|
||||
/* this option is normally set last */
|
||||
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
||||
/* assert min..center..max ordering */
|
||||
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500)
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_MAX] < 2500)
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN])
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX])
|
||||
break;
|
||||
/* assert deadzone is sane */
|
||||
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500)
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]))
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]))
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS)
|
||||
break;
|
||||
|
||||
/* sanity checks pass, enable channel */
|
||||
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
|
|
@ -237,27 +325,27 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
int
|
||||
registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
|
||||
{
|
||||
#define SELECT_PAGE(_page_name) { *values = _page_name; *num_values = sizeof(_page_name) / sizeof(_page_name[0]); }
|
||||
|
||||
switch (page) {
|
||||
case PX4IO_PAGE_CONFIG:
|
||||
*values = r_page_config;
|
||||
*num_values = sizeof(r_page_config) / sizeof(r_page_config[0]);
|
||||
break;
|
||||
|
||||
/*
|
||||
* Handle pages that are updated dynamically at read time.
|
||||
*/
|
||||
case PX4IO_PAGE_STATUS:
|
||||
/* PX4IO_P_STATUS_FREEMEM */
|
||||
{
|
||||
struct mallinfo minfo = mallinfo();
|
||||
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
|
||||
}
|
||||
|
||||
/* XXX PX4IO_P_STATUS_CPULOAD */
|
||||
r_page_status[PX4IO_P_STATUS_FLAGS] =
|
||||
(system_state.armed ? PX4IO_P_STATUS_FLAGS_ARMED : 0) |
|
||||
(system_state.manual_override_ok ? PX4IO_P_STATUS_FLAGS_OVERRIDE : 0) |
|
||||
((system_state.rc_channels > 0) ? PX4IO_P_STATUS_FLAGS_RC_OK : 0))
|
||||
/* XXX specific receiver status */
|
||||
|
||||
/* XXX PX4IO_P_STATUS_ALARMS] */
|
||||
/* PX4IO_P_STATUS_FLAGS maintained externally */
|
||||
|
||||
/* PX4IO_P_STATUS_ALARMS maintained externally */
|
||||
|
||||
/* PX4IO_P_STATUS_VBATT */
|
||||
{
|
||||
/*
|
||||
* Coefficients here derived by measurement of the 5-16V
|
||||
|
|
@ -285,43 +373,42 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
unsigned counts = adc_measure(ADC_VBATT);
|
||||
r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10;
|
||||
}
|
||||
|
||||
/* XXX PX4IO_P_STATUS_TEMPERATURE */
|
||||
|
||||
*values = r_page_status;
|
||||
*num_values = sizeof(r_page_status) / sizeof(r_page_status[0]);
|
||||
SELECT_PAGE(r_page_status);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_ACTUATORS:
|
||||
*values = r_page_actuators;
|
||||
*num_values = sizeof(r_page_actuators) / sizeof(r_page_actuators[0]);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_SERVOS:
|
||||
*values = system_state.servos;
|
||||
*num_values = IO_SERVO_COUNT;
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_RAW_RC_INPUT:
|
||||
*values = r_page_raw_rc_input;
|
||||
*num_values = sizeof(r_page_raw_rc_input) / sizeof(r_page_raw_rc_input[0]);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_RC_INPUT:
|
||||
*values = system_state.rc_channel_data;
|
||||
*num_values = system_state.rc_channels;
|
||||
return -1;
|
||||
|
||||
case PX4IO_PAGE_RAW_ADC_INPUT:
|
||||
r_page_adc[0] = adc_measure(ADC_VBATT);
|
||||
r_page_adc[1] = adc_measure(ADC_IN5);
|
||||
*values = r_page_adc;
|
||||
*num_values = ADC_CHANNEL_COUNT;
|
||||
|
||||
SELECT_PAGE(r_page_adc);
|
||||
break;
|
||||
|
||||
/*
|
||||
* Pages that are just a straight read of the register state.
|
||||
*/
|
||||
#define COPY_PAGE(_page_name, _page) case _page_name: SELECT_PAGE(_page); break;
|
||||
|
||||
/* status pages */
|
||||
COPY_PAGE(PX4IO_PAGE_CONFIG, r_page_config);
|
||||
COPY_PAGE(PX4IO_PAGE_ACTUATORS, r_page_actuators);
|
||||
COPY_PAGE(PX4IO_PAGE_SERVOS, r_page_servos);
|
||||
COPY_PAGE(PX4IO_PAGE_RAW_RC_INPUT, r_page_raw_rc_input);
|
||||
COPY_PAGE(PX4IO_PAGE_RC_INPUT, r_page_rc_input);
|
||||
|
||||
/* readback of input pages */
|
||||
COPY_PAGE(PX4IO_PAGE_SETUP, r_page_setup);
|
||||
COPY_PAGE(PX4IO_PAGE_CONTROLS, r_page_controls);
|
||||
COPY_PAGE(PX4IO_PAGE_RC_CONFIG, r_page_rc_input_config);
|
||||
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
|
||||
#undef SELECT_PAGE
|
||||
|
||||
/* if the offset is beyond the end of the page, we have no data */
|
||||
if (*num_values <= offset)
|
||||
return -1;
|
||||
|
|
|
|||
|
|
@ -36,15 +36,8 @@
|
|||
*/
|
||||
|
||||
#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 <nuttx/clock.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
|
@ -116,30 +109,28 @@ safety_check_button(void *arg)
|
|||
* state machine, keep ARM_COUNTER_THRESHOLD the same
|
||||
* length in all cases of the if/else struct below.
|
||||
*/
|
||||
if (safety_button_pressed && !system_state.armed) {
|
||||
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
|
||||
|
||||
if (counter < ARM_COUNTER_THRESHOLD) {
|
||||
counter++;
|
||||
|
||||
} else if (counter == ARM_COUNTER_THRESHOLD) {
|
||||
/* change to armed state and notify the FMU */
|
||||
system_state.armed = true;
|
||||
/* switch to armed state */
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
|
||||
counter++;
|
||||
system_state.fmu_report_due = true;
|
||||
}
|
||||
|
||||
/* Disarm quickly */
|
||||
|
||||
} else if (safety_button_pressed && system_state.armed) {
|
||||
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
|
||||
|
||||
if (counter < ARM_COUNTER_THRESHOLD) {
|
||||
counter++;
|
||||
|
||||
} else if (counter == ARM_COUNTER_THRESHOLD) {
|
||||
/* change to disarmed state and notify the FMU */
|
||||
system_state.armed = false;
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
|
||||
counter++;
|
||||
system_state.fmu_report_due = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
@ -149,15 +140,15 @@ safety_check_button(void *arg)
|
|||
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
|
||||
uint16_t pattern = LED_PATTERN_SAFE;
|
||||
|
||||
if (system_state.armed) {
|
||||
if (system_state.arm_ok) {
|
||||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
|
||||
if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
|
||||
pattern = LED_PATTERN_IO_FMU_ARMED;
|
||||
|
||||
} else {
|
||||
pattern = LED_PATTERN_IO_ARMED;
|
||||
}
|
||||
|
||||
} else if (system_state.arm_ok) {
|
||||
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
|
||||
pattern = LED_PATTERN_FMU_ARMED;
|
||||
|
||||
} else if (system_state.vector_flight_mode_ok) {
|
||||
|
|
@ -188,7 +179,7 @@ failsafe_blink(void *arg)
|
|||
static bool failsafe = false;
|
||||
|
||||
/* blink the failsafe LED if we don't have FMU input */
|
||||
if (!system_state.mixer_fmu_available) {
|
||||
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
|
||||
failsafe = !failsafe;
|
||||
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -66,13 +66,13 @@ static unsigned partial_frame_count;
|
|||
|
||||
unsigned sbus_frame_drops;
|
||||
|
||||
static void sbus_decode(hrt_abstime frame_time);
|
||||
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values);
|
||||
|
||||
int
|
||||
sbus_init(const char *device)
|
||||
{
|
||||
if (sbus_fd < 0)
|
||||
sbus_fd = open(device, O_RDONLY);
|
||||
sbus_fd = open(device, O_RDONLY | O_NONBLOCK);
|
||||
|
||||
if (sbus_fd >= 0) {
|
||||
struct termios t;
|
||||
|
|
@ -97,7 +97,7 @@ sbus_init(const char *device)
|
|||
}
|
||||
|
||||
bool
|
||||
sbus_input(void)
|
||||
sbus_input(uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
ssize_t ret;
|
||||
hrt_abstime now;
|
||||
|
|
@ -134,7 +134,7 @@ sbus_input(void)
|
|||
|
||||
/* if the read failed for any reason, just give up here */
|
||||
if (ret < 1)
|
||||
goto out;
|
||||
return false;
|
||||
|
||||
last_rx_time = now;
|
||||
|
||||
|
|
@ -147,20 +147,14 @@ sbus_input(void)
|
|||
* If we don't have a full frame, return
|
||||
*/
|
||||
if (partial_frame_count < SBUS_FRAME_SIZE)
|
||||
goto out;
|
||||
return false;
|
||||
|
||||
/*
|
||||
* Great, it looks like we might have a frame. Go ahead and
|
||||
* decode it.
|
||||
*/
|
||||
sbus_decode(now);
|
||||
partial_frame_count = 0;
|
||||
|
||||
out:
|
||||
/*
|
||||
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
|
||||
*/
|
||||
return (now - last_frame_time) < 200000;
|
||||
return sbus_decode(now, values, num_values);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -199,13 +193,13 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
|
|||
/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
|
||||
};
|
||||
|
||||
static void
|
||||
sbus_decode(hrt_abstime frame_time)
|
||||
static bool
|
||||
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
/* check frame boundary markers to avoid out-of-sync cases */
|
||||
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
|
||||
sbus_frame_drops++;
|
||||
return;
|
||||
return false;
|
||||
}
|
||||
|
||||
/* if the failsafe or connection lost bit is set, we consider the frame invalid */
|
||||
|
|
@ -213,8 +207,8 @@ sbus_decode(hrt_abstime frame_time)
|
|||
(frame[23] & (1 << 3))) { /* failsafe */
|
||||
|
||||
/* actively announce signal loss */
|
||||
system_state.rc_channels = 0;
|
||||
return 1;
|
||||
*values = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
/* we have received something we think is a frame */
|
||||
|
|
@ -241,23 +235,19 @@ sbus_decode(hrt_abstime frame_time)
|
|||
}
|
||||
|
||||
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
|
||||
system_state.rc_channel_data[channel] = (value / 2) + 998;
|
||||
values[channel] = (value / 2) + 998;
|
||||
}
|
||||
|
||||
/* decode switch channels if data fields are wide enough */
|
||||
if (chancount > 17) {
|
||||
/* channel 17 (index 16) */
|
||||
system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998;
|
||||
values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
|
||||
/* channel 18 (index 17) */
|
||||
system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998;
|
||||
values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
|
||||
}
|
||||
|
||||
/* note the number of channels decoded */
|
||||
system_state.rc_channels = chancount;
|
||||
*num_values = chancount;
|
||||
|
||||
/* and note that we have received data from the R/C controller */
|
||||
system_state.rc_channels_timestamp = frame_time;
|
||||
|
||||
/* trigger an immediate report to the FMU */
|
||||
system_state.fmu_report_due = true;
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue