aboutsummaryrefslogtreecommitdiff
path: root/src/modules/px4iofirmware
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r--src/modules/px4iofirmware/controls.c16
-rw-r--r--src/modules/px4iofirmware/dsm.c19
-rw-r--r--src/modules/px4iofirmware/i2c.c15
-rw-r--r--src/modules/px4iofirmware/mixer.cpp153
-rw-r--r--src/modules/px4iofirmware/module.mk11
-rw-r--r--src/modules/px4iofirmware/protocol.h160
-rw-r--r--src/modules/px4iofirmware/px4io.c26
-rw-r--r--src/modules/px4iofirmware/px4io.h75
-rw-r--r--src/modules/px4iofirmware/registers.c257
-rw-r--r--src/modules/px4iofirmware/safety.c10
-rw-r--r--src/modules/px4iofirmware/serial.c352
11 files changed, 928 insertions, 166 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index fbd82a4c6..796c6cd9f 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -59,14 +59,14 @@ static perf_counter_t c_gather_ppm;
void
controls_init(void)
{
- /* DSM input */
+ /* DSM input (USART1) */
dsm_init("/dev/ttyS0");
- /* S.bus input */
+ /* S.bus input (USART3) */
sbus_init("/dev/ttyS2");
/* default to a 1:1 input map, all enabled */
- for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+ for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
@@ -124,7 +124,7 @@ controls_tick() {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
perf_end(c_gather_ppm);
- ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
+ ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS);
/*
* In some cases we may have received a frame, but input has still
@@ -197,7 +197,7 @@ controls_tick() {
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
- ASSERT(mapped < MAX_CONTROL_CHANNELS);
+ ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
@@ -209,7 +209,7 @@ controls_tick() {
}
/* set un-assigned controls to zero */
- for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+ for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
}
@@ -321,8 +321,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* PPM data exists, copy it */
*num_values = ppm_decoded_channels;
- if (*num_values > MAX_CONTROL_CHANNELS)
- *num_values = MAX_CONTROL_CHANNELS;
+ if (*num_values > PX4IO_CONTROL_CHANNELS)
+ *num_values = PX4IO_CONTROL_CHANNELS;
for (unsigned i = 0; i < *num_values; i++)
values[i] = ppm_buffer[i];
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 745cdfa40..206e27db5 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -53,13 +53,13 @@
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
-static int dsm_fd = -1; /**<File handle to the DSM UART*/
-static hrt_abstime dsm_last_rx_time; /**<Timestamp when we last received*/
-static hrt_abstime dsm_last_frame_time; /**<Timestamp for start of last dsm frame*/
-static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**<DSM dsm frame receive buffer*/
-static unsigned dsm_partial_frame_count; /**<Count of bytes received for current dsm frame*/
-static unsigned dsm_channel_shift; /**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/
-static unsigned dsm_frame_drops; /**<Count of incomplete DSM frames*/
+static int dsm_fd = -1; /**< File handle to the DSM UART */
+static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */
+static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
+static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */
+static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */
+static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
+static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */
/**
* Attempt to decode a single channel raw channel datum
@@ -249,6 +249,10 @@ dsm_bind(uint16_t cmd, int pulses)
if (dsm_fd < 0)
return;
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+ // XXX implement
+ #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
+#else
switch (cmd) {
case dsm_bind_power_down:
@@ -288,6 +292,7 @@ dsm_bind(uint16_t cmd, int pulses)
break;
}
+#endif
}
/**
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 4485daa5b..10aeb5c9f 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012,2013 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
@@ -69,6 +69,7 @@ static void i2c_rx_setup(void);
static void i2c_tx_setup(void);
static void i2c_rx_complete(void);
static void i2c_tx_complete(void);
+static void i2c_dump(void);
static DMA_HANDLE rx_dma;
static DMA_HANDLE tx_dma;
@@ -92,7 +93,7 @@ enum {
} direction;
void
-i2c_init(void)
+interface_init(void)
{
debug("i2c init");
@@ -148,12 +149,18 @@ i2c_init(void)
#endif
}
+void
+interface_tick()
+{
+}
+
/*
reset the I2C bus
used to recover from lockups
*/
-void i2c_reset(void)
+void
+i2c_reset(void)
{
rCR1 |= I2C_CR1_SWRST;
rCR1 = 0;
@@ -330,7 +337,7 @@ i2c_tx_complete(void)
i2c_tx_setup();
}
-void
+static void
i2c_dump(void)
{
debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2);
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index a2193b526..deed25836 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -59,6 +59,12 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
+/*
+ * Time that the ESCs need to initialize
+ */
+ #define ESC_INIT_TIME_US 1000000
+ #define ESC_RAMP_TIME_US 2000000
+
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
@@ -68,6 +74,17 @@ extern "C" {
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
+static bool should_arm = false;
+static bool should_always_enable_pwm = false;
+static uint64_t esc_init_time;
+
+enum esc_state_e {
+ ESC_OFF,
+ ESC_INIT,
+ ESC_RAMP,
+ ESC_ON
+};
+static esc_state_e esc_state;
/* selected control values and count for mixing */
enum mixer_source {
@@ -98,7 +115,7 @@ mixer_tick(void)
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
- r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
} else {
@@ -112,12 +129,11 @@ mixer_tick(void)
* Decide which set of controls we're using.
*/
- /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+ /* do not mix if RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
- !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
- /* don't actually mix anything - we already have raw PWM values or
- not a valid mixer. */
+ /* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
} else {
@@ -132,7 +148,8 @@ mixer_tick(void)
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
- (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+ !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
@@ -154,7 +171,7 @@ mixer_tick(void)
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
- for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
@@ -164,11 +181,53 @@ mixer_tick(void)
} else if (source != MIX_NONE) {
- float outputs[IO_SERVO_COUNT];
+ float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
+ uint16_t ramp_promille;
+
+ /* update esc init state, but only if we are truely armed and not just PWM enabled */
+ if (mixer_servos_armed && should_arm) {
+
+ switch (esc_state) {
+
+ /* after arming, some ESCs need an initalization period, count the time from here */
+ case ESC_OFF:
+ esc_init_time = hrt_absolute_time();
+ esc_state = ESC_INIT;
+ break;
+
+ /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
+ case ESC_INIT:
+ if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
+ esc_state = ESC_RAMP;
+ }
+ break;
+
+ /* then ramp until the min speed is reached */
+ case ESC_RAMP:
+ if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
+ esc_state = ESC_ON;
+ }
+ break;
+
+ case ESC_ON:
+ default:
+
+ break;
+ }
+ } else {
+ esc_state = ESC_OFF;
+ }
+
+ /* do the calculations during the ramp for all at once */
+ if(esc_state == ESC_RAMP) {
+ ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
+ }
+
+
/* mix */
- mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
@@ -176,11 +235,29 @@ mixer_tick(void)
/* 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] * 600.0f) + 1500;
-
+ switch (esc_state) {
+ case ESC_INIT:
+ r_page_servos[i] = (outputs[i] * 600 + 1500);
+ break;
+
+ case ESC_RAMP:
+ r_page_servos[i] = (outputs[i]
+ * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+ + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
+ break;
+
+ case ESC_ON:
+ r_page_servos[i] = (outputs[i]
+ * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+ + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
+ break;
+
+ case ESC_OFF:
+ default:
+ break;
+ }
}
- for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
@@ -193,30 +270,46 @@ mixer_tick(void)
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
- bool should_arm = (
- /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
- /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
- /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
- /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
- /* FMU is available or FMU is not available but override is an option */
- ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+ should_arm = (
+ /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+ /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+ /* and FMU is armed */ && (
+ ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+ /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+ /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+ /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+ )
);
- if (should_arm && !mixer_servos_armed) {
+ should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+ && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+ && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+ if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
+ isr_debug(5, "> PWM enabled");
- } else if (!should_arm && mixer_servos_armed) {
+ } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
+ isr_debug(5, "> PWM disabled");
+
}
- if (mixer_servos_armed) {
+ if (mixer_servos_armed && should_arm) {
/* update the servo outputs. */
- for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
+
+ } else if (mixer_servos_armed && should_always_enable_pwm) {
+ /* set the idle servo outputs. */
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
+ up_pwm_servo_set(i, r_page_servo_idle[i]);
}
}
@@ -265,9 +358,8 @@ static unsigned mixer_text_length = 0;
void
mixer_handle_text(const void *buffer, size_t length)
{
- /* do not allow a mixer change while fully armed */
- if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
- /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ /* do not allow a mixer change while outputs armed */
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return;
}
@@ -344,16 +436,17 @@ mixer_set_failsafe()
* Check if a custom failsafe value has been written,
* or if the mixer is not ok and bail out.
*/
+
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
return;
/* set failsafe defaults to the values for all inputs = 0 */
- float outputs[IO_SERVO_COUNT];
+ float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
- mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
@@ -364,7 +457,7 @@ mixer_set_failsafe()
}
/* disable the rest of the outputs */
- for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servo_failsafe[i] = 0;
}
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 6379366f4..59f470a94 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -3,17 +3,22 @@
SRCS = adc.c \
controls.c \
dsm.c \
- i2c.c \
px4io.c \
registers.c \
safety.c \
sbus.c \
../systemlib/up_cxxinitialize.c \
- ../systemlib/hx_stream.c \
../systemlib/perf_counter.c \
mixer.cpp \
../systemlib/mixer/mixer.cpp \
../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \
../systemlib/mixer/mixer_simple.cpp \
- \ No newline at end of file
+
+ifeq ($(BOARD),px4io-v1)
+SRCS += i2c.c
+endif
+ifeq ($(BOARD),px4io-v2)
+SRCS += serial.c \
+ ../systemlib/hx_stream.c
+endif
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 88d8cc87c..f5fa0ba75 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -36,7 +36,7 @@
/**
* @file protocol.h
*
- * PX4IO I2C interface protocol.
+ * PX4IO interface protocol.
*
* Communication is performed via writes to and reads from 16-bit virtual
* registers organised into pages of 255 registers each.
@@ -45,7 +45,7 @@
* respectively. Subsequent reads and writes increment the offset within
* the page.
*
- * Most pages are readable or writable but not both.
+ * Some pages are read- or write-only.
*
* Note that some pages may permit offset values greater than 255, which
* can only be achieved by long writes. The offset does not wrap.
@@ -62,12 +62,11 @@
* Note that the implementation of readable pages prefers registers within
* readable pages to be densely packed. Page numbers do not need to be
* packed.
+ *
+ * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise,
+ * [2] denotes definitions specific to the PX4IOv2 board.
*/
-#define PX4IO_CONTROL_CHANNELS 8
-#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))
@@ -75,10 +74,12 @@
#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))
+#define PX4IO_PROTOCOL_VERSION 4
+
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
-#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
-#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */
+#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
@@ -93,7 +94,7 @@
#define PX4IO_P_STATUS_CPULOAD 1
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
-#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
@@ -105,19 +106,24 @@
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */
#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_VBATT_LOW (1 << 0) /* [1] 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_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */
#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) /* timed out waiting for RC input */
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */
+#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
-#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */
-#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */
+#define PX4IO_P_STATUS_VBATT 4 /* [1] battery voltage in mV */
+#define PX4IO_P_STATUS_IBATT 5 /* [1] battery current (raw ADC) */
+#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
+#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
+#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -143,7 +149,7 @@
#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
/* setup page */
-#define PX4IO_PAGE_SETUP 100
+#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
@@ -152,37 +158,40 @@
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
+#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
+#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
-#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
-/* px4io relay bit definitions */
-#define PX4IO_RELAY1 (1<<0)
-#define PX4IO_RELAY2 (1<<1)
-#define PX4IO_ACC1 (1<<2)
-#define PX4IO_ACC2 (1<<3)
+#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
+#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */
+#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */
+#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */
+#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */
-#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
-#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */
-enum { /* DSM bind states */
+#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */
+#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */
+enum { /* DSM bind states */
dsm_bind_power_down = 0,
dsm_bind_power_up,
dsm_bind_set_rx_out,
dsm_bind_send_pulses,
dsm_bind_reinit_uart
};
+ /* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */
/* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD 102
+#define PX4IO_PAGE_MIXERLOAD 52
/* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
+#define PX4IO_PAGE_RC_CONFIG 53 /* 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 */
@@ -194,10 +203,23 @@ enum { /* DSM bind states */
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
/* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* Debug and test page - not used in normal operation */
+#define PX4IO_PAGE_TEST 127
+#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */
+
+/* PWM minimum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM idle values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
@@ -218,3 +240,81 @@ struct px4io_mixdata {
};
#pragma pack(pop)
+/**
+ * Serial protocol encapsulation.
+ */
+
+#define PKT_MAX_REGS 32 // by agreement w/FMU
+
+#pragma pack(push, 1)
+struct IOPacket {
+ uint8_t count_code;
+ uint8_t crc;
+ uint8_t page;
+ uint8_t offset;
+ uint16_t regs[PKT_MAX_REGS];
+};
+#pragma pack(pop)
+
+#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */
+#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */
+#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */
+#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */
+#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */
+
+#define PKT_CODE_MASK 0xc0
+#define PKT_COUNT_MASK 0x3f
+
+#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK)
+#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK)
+#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
+
+static const uint8_t crc8_tab[256] __attribute__((unused)) =
+{
+ 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
+ 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
+ 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
+ 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
+ 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
+ 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
+ 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
+ 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
+ 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
+ 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
+ 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
+ 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
+ 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
+ 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
+ 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
+ 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
+ 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
+ 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
+ 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
+ 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
+ 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
+ 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
+ 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
+ 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
+ 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
+ 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
+ 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
+ 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
+ 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
+ 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
+ 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
+ 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
+};
+
+static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused));
+static uint8_t
+crc_packet(struct IOPacket *pkt)
+{
+ uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]);
+ uint8_t *p = (uint8_t *)pkt;
+ uint8_t c = 0;
+
+ while (p < end)
+ c = crc8_tab[c ^ *(p++)];
+
+ return c;
+}
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index bc8dfc116..e70b3fe88 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -64,11 +64,6 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call;
-#ifdef CONFIG_STM32_I2C1
-/* store i2c reset count XXX this should be a register, together with other error counters */
-volatile uint32_t i2c_loop_resets = 0;
-#endif
-
/*
* a set of debug buffers to allow us to send debug information from ISRs
*/
@@ -147,8 +142,10 @@ user_start(int argc, char *argv[])
LED_BLUE(false);
LED_SAFETY(false);
- /* turn on servo power */
+ /* turn on servo power (if supported) */
+#ifdef POWER_SERVO
POWER_SERVO(true);
+#endif
/* start the safety switch handler */
safety_init();
@@ -159,10 +156,11 @@ user_start(int argc, char *argv[])
/* initialise the control inputs */
controls_init();
-#ifdef CONFIG_STM32_I2C1
- /* start the i2c handler */
- i2c_init();
-#endif
+ /* start the FMU interface */
+ interface_init();
+
+ /* add a performance counter for the interface */
+ perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
@@ -205,6 +203,11 @@ user_start(int argc, char *argv[])
/* track the rate at which the loop is running */
perf_count(loop_perf);
+ /* kick the interface */
+ perf_begin(interface_perf);
+ interface_tick();
+ perf_end(interface_perf);
+
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
@@ -223,12 +226,11 @@ user_start(int argc, char *argv[])
struct mallinfo minfo = mallinfo();
- isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u",
+ isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
- (unsigned)i2c_loop_resets,
(unsigned)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 57cffcc23..dea67043e 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -42,15 +42,16 @@
#include <stdbool.h>
#include <stdint.h>
-#include <drivers/boards/px4io/px4io_internal.h>
+#include <board_config.h>
#include "protocol.h"
/*
* Constants and limits.
*/
-#define MAX_CONTROL_CHANNELS 12
-#define IO_SERVO_COUNT 8
+#define PX4IO_SERVO_COUNT 8
+#define PX4IO_CONTROL_CHANNELS 8
+#define PX4IO_INPUT_CHANNELS 12
/*
* Debug logging
@@ -77,6 +78,9 @@ 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 */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
+extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
+extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */
/*
* Register aliases.
@@ -119,32 +123,43 @@ extern struct sys_state_s system_state;
/*
* GPIO handling.
*/
-#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
-#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
-#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
+#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
+#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
+#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+
+# define PX4IO_RELAY_CHANNELS 4
+# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
+# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+# define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
+# define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT 2
+# define ADC_VBATT 4
+# define ADC_IN5 5
-#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
-#ifdef GPIO_ACC1_PWR_EN
- #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
-#endif
-#ifdef GPIO_ACC2_PWR_EN
- #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
-#endif
-#ifdef GPIO_RELAY1_EN
- #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
#endif
-#ifdef GPIO_RELAY2_EN
- #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+
+# define PX4IO_RELAY_CHANNELS 0
+# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+
+# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT 2
+# define ADC_VSERVO 4
+# define ADC_RSSI 5
+
#endif
-#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
-#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT))
#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
-#define ADC_VBATT 4
-#define ADC_IN5 5
-#define ADC_CHANNEL_COUNT 2
-
/*
* Mixer
*/
@@ -156,17 +171,16 @@ extern void mixer_handle_text(const void *buffer, size_t length);
*/
extern void safety_init(void);
-#ifdef CONFIG_STM32_I2C1
/**
* FMU communications
*/
-extern void i2c_init(void);
-#endif
+extern void interface_init(void);
+extern void interface_tick(void);
/**
* Register space
*/
-extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+extern int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
/**
@@ -191,10 +205,5 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
-/* send a debug message to the console */
+/** send a debug message to the console */
extern void isr_debug(uint8_t level, const char *fmt, ...);
-
-#ifdef CONFIG_STM32_I2C1
-void i2c_dump(void);
-void i2c_reset(void);
-#endif
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index a922362b6..9c95fd1c5 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -44,6 +44,7 @@
#include <string.h>
#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
#include "px4io.h"
#include "protocol.h"
@@ -57,14 +58,18 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt
* Static configuration parameters.
*/
static const uint16_t r_page_config[] = {
- [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */
- [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */
+ [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION,
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+ [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2,
+#else
+ [PX4IO_P_CONFIG_HARDWARE_VERSION] = 1,
+#endif
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
- [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT,
- [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS,
- [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT,
+ [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
+ [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS,
+ [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
};
@@ -79,7 +84,10 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_FLAGS] = 0,
[PX4IO_P_STATUS_ALARMS] = 0,
[PX4IO_P_STATUS_VBATT] = 0,
- [PX4IO_P_STATUS_IBATT] = 0
+ [PX4IO_P_STATUS_IBATT] = 0,
+ [PX4IO_P_STATUS_VSERVO] = 0,
+ [PX4IO_P_STATUS_VRSSI] = 0,
+ [PX4IO_P_STATUS_PRSSI] = 0
};
/**
@@ -87,14 +95,14 @@ uint16_t r_page_status[] = {
*
* Post-mixed actuator values.
*/
-uint16_t r_page_actuators[IO_SERVO_COUNT];
+uint16_t r_page_actuators[PX4IO_SERVO_COUNT];
/**
* PAGE 3
*
* Servo PWM values
*/
-uint16_t r_page_servos[IO_SERVO_COUNT];
+uint16_t r_page_servos[PX4IO_SERVO_COUNT];
/**
* PAGE 4
@@ -104,7 +112,7 @@ uint16_t r_page_servos[IO_SERVO_COUNT];
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
+ [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
};
/**
@@ -114,7 +122,7 @@ uint16_t r_page_raw_rc_input[] =
*/
uint16_t r_page_rc_input[] = {
[PX4IO_P_RC_VALID] = 0,
- [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+ [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
};
/**
@@ -138,7 +146,11 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
[PX4IO_P_SETUP_RELAYS] = 0,
+#ifdef ADC_VSERVO
+ [PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
+#else
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
+#endif
[PX4IO_P_SETUP_SET_DEBUG] = 0,
};
@@ -146,8 +158,11 @@ volatile uint16_t r_page_setup[] =
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
- PX4IO_P_SETUP_ARMING_IO_ARM_OK)
-#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
+ PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
+ PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
+ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
+ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
+#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
/**
@@ -166,7 +181,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
*
* R/C channel input configuration.
*/
-uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
/* valid options */
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
@@ -182,9 +197,33 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
*
* Disable pulses as default.
*/
-uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
+uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
-void
+/**
+ * PAGE 106
+ *
+ * minimum PWM values when armed
+ *
+ */
+uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+/**
+ * PAGE 107
+ *
+ * maximum PWM values when armed
+ *
+ */
+uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
+
+/**
+ * PAGE 108
+ *
+ * idle PWM values for difficult ESCs
+ *
+ */
+uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
@@ -233,7 +272,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
case PX4IO_PAGE_FAILSAFE_PWM:
/* copy channel data */
- while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servo_failsafe[offset] = *values;
@@ -247,6 +286,75 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
}
break;
+ case PX4IO_PAGE_CONTROL_MIN_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0)
+ /* set to default */
+ r_page_servo_control_min[offset] = 900;
+
+ else if (*values > 1200)
+ r_page_servo_control_min[offset] = 1200;
+ else if (*values < 900)
+ r_page_servo_control_min[offset] = 900;
+ else
+ r_page_servo_control_min[offset] = *values;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
+ case PX4IO_PAGE_CONTROL_MAX_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0)
+ /* set to default */
+ r_page_servo_control_max[offset] = 2100;
+
+ else if (*values > 2100)
+ r_page_servo_control_max[offset] = 2100;
+ else if (*values < 1800)
+ r_page_servo_control_max[offset] = 1800;
+ else
+ r_page_servo_control_max[offset] = *values;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
+ case PX4IO_PAGE_IDLE_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0)
+ /* set to default */
+ r_page_servo_idle[offset] = 0;
+
+ else if (*values < 900)
+ r_page_servo_idle[offset] = 900;
+ else if (*values > 2100)
+ r_page_servo_idle[offset] = 2100;
+ else
+ r_page_servo_idle[offset] = *values;
+
+ /* flag the failsafe values as custom */
+ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
@@ -260,11 +368,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* iterate individual registers, set each in turn */
while (num_values--) {
if (registers_set_one(page, offset, *values))
- break;
+ return -1;
offset++;
values++;
}
+ break;
}
+ return 0;
}
static int
@@ -317,8 +427,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* so that an in-air reset of FMU can not lead to a
* lockup of the IO arming state.
*/
- if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
- r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+
+ // XXX do not reset IO's safety state by FMU for now
+ // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+ // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+ // }
+
+ if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}
r_setup_arming = value;
@@ -349,10 +465,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_RELAYS:
value &= PX4IO_P_SETUP_RELAYS_VALID;
r_setup_relays = value;
- POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0);
- POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0);
- POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0);
- POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0);
+#ifdef POWER_RELAY1
+ POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
+#endif
+#ifdef POWER_RELAY2
+ POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
+#endif
+#ifdef POWER_ACC1
+ POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
+#endif
+#ifdef POWER_ACC2
+ POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
+#endif
+ break;
+
+ case PX4IO_P_SETUP_VBATT_SCALE:
+ r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
break;
case PX4IO_P_SETUP_SET_DEBUG:
@@ -371,9 +499,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
- /* do not allow a RC config change while fully armed */
- if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
- /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ /**
+ * do not allow a RC config change while outputs armed
+ */
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
+ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
break;
}
@@ -381,7 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
- if (channel >= MAX_CONTROL_CHANNELS)
+ if (channel >= PX4IO_CONTROL_CHANNELS)
return -1;
/* disable the channel until we have a chance to sanity-check it */
@@ -401,6 +532,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
+ /* clear any existing RC disabled flag */
+ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
+
/* set all options except the enabled option */
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
@@ -426,7 +560,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
count++;
}
- if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+ if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
count++;
}
@@ -446,6 +580,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* case PX4IO_RC_PAGE_CONFIG */
}
+ case PX4IO_PAGE_TEST:
+ switch (offset) {
+ case PX4IO_P_TEST_LED:
+ LED_AMBER(value & 1);
+ break;
+ }
+ break;
+
default:
return -1;
}
@@ -482,6 +624,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
/* PX4IO_P_STATUS_ALARMS maintained externally */
+#ifdef ADC_VBATT
/* PX4IO_P_STATUS_VBATT */
{
/*
@@ -515,7 +658,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
}
}
-
+#endif
+#ifdef ADC_IBATT
/* PX4IO_P_STATUS_IBATT */
{
/*
@@ -525,26 +669,62 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
FMU sort it out, with user selectable
configuration for their sensor
*/
- unsigned counts = adc_measure(ADC_IN5);
+ unsigned counts = adc_measure(ADC_IBATT);
if (counts != 0xffff) {
r_page_status[PX4IO_P_STATUS_IBATT] = counts;
}
}
+#endif
+#ifdef ADC_VSERVO
+ /* PX4IO_P_STATUS_VSERVO */
+ {
+ /*
+ * Coefficients here derived by measurement of the 5-16V
+ * range on one unit:
+ *
+ * XXX pending measurements
+ *
+ * slope = xxx
+ * intercept = xxx
+ *
+ * Intercept corrected for best results @ 5.0V.
+ */
+ unsigned counts = adc_measure(ADC_VSERVO);
+ if (counts != 0xffff) {
+ unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+ unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000;
+
+ r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
+ }
+ }
+#endif
+ /* XXX PX4IO_P_STATUS_VRSSI */
+ /* XXX PX4IO_P_STATUS_PRSSI */
SELECT_PAGE(r_page_status);
break;
case PX4IO_PAGE_RAW_ADC_INPUT:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
+#ifdef ADC_VBATT
r_page_scratch[0] = adc_measure(ADC_VBATT);
- r_page_scratch[1] = adc_measure(ADC_IN5);
-
+#endif
+#ifdef ADC_IBATT
+ r_page_scratch[1] = adc_measure(ADC_IBATT);
+#endif
+
+#ifdef ADC_VSERVO
+ r_page_scratch[0] = adc_measure(ADC_VSERVO);
+#endif
+#ifdef ADC_RSSI
+ r_page_scratch[1] = adc_measure(ADC_RSSI);
+#endif
SELECT_PAGE(r_page_scratch);
break;
case PX4IO_PAGE_PWM_INFO:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
- for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
SELECT_PAGE(r_page_scratch);
@@ -587,6 +767,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
+ case PX4IO_PAGE_CONTROL_MIN_PWM:
+ SELECT_PAGE(r_page_servo_control_min);
+ break;
+ case PX4IO_PAGE_CONTROL_MAX_PWM:
+ SELECT_PAGE(r_page_servo_control_max);
+ break;
+ case PX4IO_PAGE_IDLE_PWM:
+ SELECT_PAGE(r_page_servo_idle);
+ break;
default:
return -1;
@@ -616,7 +805,7 @@ static void
pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
{
for (unsigned pass = 0; pass < 2; pass++) {
- for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
+ for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) {
/* get the channel mask for this rate group */
uint32_t mask = up_pwm_servo_get_rate_group(group);
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 4dbecc274..95335f038 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -110,7 +110,7 @@ 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 && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+ if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +118,18 @@ safety_check_button(void *arg)
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* switch to armed state */
- r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
counter++;
}
- } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
- r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
counter++;
}
@@ -140,7 +140,7 @@ safety_check_button(void *arg)
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
- if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+ if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_IO_FMU_ARMED;
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
new file mode 100644
index 000000000..94d7407df
--- /dev/null
+++ b/src/modules/px4iofirmware/serial.c
@@ -0,0 +1,352 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012,2013 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 serial.c
+ *
+ * Serial communication for the PX4IO module.
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+
+/* XXX might be able to prune these */
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+#include <stm32.h>
+#include <systemlib/perf_counter.h>
+
+//#define DEBUG
+#include "px4io.h"
+
+static perf_counter_t pc_txns;
+static perf_counter_t pc_errors;
+static perf_counter_t pc_ore;
+static perf_counter_t pc_fe;
+static perf_counter_t pc_ne;
+static perf_counter_t pc_idle;
+static perf_counter_t pc_badidle;
+static perf_counter_t pc_regerr;
+static perf_counter_t pc_crcerr;
+
+static void rx_handle_packet(void);
+static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
+static DMA_HANDLE tx_dma;
+static DMA_HANDLE rx_dma;
+
+static int serial_interrupt(int irq, void *context);
+static void dma_reset(void);
+
+/* if we spend this many ticks idle, reset the DMA */
+static unsigned idle_ticks;
+
+static struct IOPacket dma_packet;
+
+/* serial register accessors */
+#define REG(_x) (*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x))
+#define rSR REG(STM32_USART_SR_OFFSET)
+#define rDR REG(STM32_USART_DR_OFFSET)
+#define rBRR REG(STM32_USART_BRR_OFFSET)
+#define rCR1 REG(STM32_USART_CR1_OFFSET)
+#define rCR2 REG(STM32_USART_CR2_OFFSET)
+#define rCR3 REG(STM32_USART_CR3_OFFSET)
+#define rGTPR REG(STM32_USART_GTPR_OFFSET)
+
+void
+interface_init(void)
+{
+ pc_txns = perf_alloc(PC_ELAPSED, "txns");
+ pc_errors = perf_alloc(PC_COUNT, "errors");
+ pc_ore = perf_alloc(PC_COUNT, "overrun");
+ pc_fe = perf_alloc(PC_COUNT, "framing");
+ pc_ne = perf_alloc(PC_COUNT, "noise");
+ pc_idle = perf_alloc(PC_COUNT, "idle");
+ pc_badidle = perf_alloc(PC_COUNT, "badidle");
+ pc_regerr = perf_alloc(PC_COUNT, "regerr");
+ pc_crcerr = perf_alloc(PC_COUNT, "crcerr");
+
+ /* allocate DMA */
+ tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA);
+ rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA);
+
+ /* configure pins for serial use */
+ stm32_configgpio(PX4FMU_SERIAL_TX_GPIO);
+ stm32_configgpio(PX4FMU_SERIAL_RX_GPIO);
+
+ /* reset and configure the UART */
+ rCR1 = 0;
+ rCR2 = 0;
+ rCR3 = 0;
+
+ /* clear status/errors */
+ (void)rSR;
+ (void)rDR;
+
+ /* configure line speed */
+ uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2);
+ uint32_t mantissa = usartdiv32 >> 5;
+ uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
+ rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
+
+ /* connect our interrupt */
+ irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt);
+ up_enable_irq(PX4FMU_SERIAL_VECTOR);
+
+ /* enable UART and error/idle interrupts */
+ rCR3 = USART_CR3_EIE;
+ rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
+
+#if 0 /* keep this for signal integrity testing */
+ for (;;) {
+ while (!(rSR & USART_SR_TXE))
+ ;
+ rDR = 0xfa;
+ while (!(rSR & USART_SR_TXE))
+ ;
+ rDR = 0xa0;
+ }
+#endif
+
+ /* configure RX DMA and return to listening state */
+ dma_reset();
+
+ debug("serial init");
+}
+
+void
+interface_tick()
+{
+ /* XXX look for stuck/damaged DMA and reset? */
+ if (idle_ticks++ > 100) {
+ dma_reset();
+ idle_ticks = 0;
+ }
+}
+
+static void
+rx_handle_packet(void)
+{
+ /* check packet CRC */
+ uint8_t crc = dma_packet.crc;
+ dma_packet.crc = 0;
+ if (crc != crc_packet(&dma_packet)) {
+ perf_count(pc_crcerr);
+
+ /* send a CRC error reply */
+ dma_packet.count_code = PKT_CODE_CORRUPT;
+ dma_packet.page = 0xff;
+ dma_packet.offset = 0xff;
+
+ return;
+ }
+
+ if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {
+
+ /* it's a blind write - pass it on */
+ if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
+ perf_count(pc_regerr);
+ dma_packet.count_code = PKT_CODE_ERROR;
+ } else {
+ dma_packet.count_code = PKT_CODE_SUCCESS;
+ }
+ return;
+ }
+
+ if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
+
+ /* it's a read - get register pointer for reply */
+ unsigned count;
+ uint16_t *registers;
+
+ if (registers_get(dma_packet.page, dma_packet.offset, &registers, &count) < 0) {
+ perf_count(pc_regerr);
+ dma_packet.count_code = PKT_CODE_ERROR;
+ } else {
+ /* constrain reply to requested size */
+ if (count > PKT_MAX_REGS)
+ count = PKT_MAX_REGS;
+ if (count > PKT_COUNT(dma_packet))
+ count = PKT_COUNT(dma_packet);
+
+ /* copy reply registers into DMA buffer */
+ memcpy((void *)&dma_packet.regs[0], registers, count * 2);
+ dma_packet.count_code = count | PKT_CODE_SUCCESS;
+ }
+ return;
+ }
+
+ /* send a bad-packet error reply */
+ dma_packet.count_code = PKT_CODE_CORRUPT;
+ dma_packet.page = 0xff;
+ dma_packet.offset = 0xfe;
+}
+
+static void
+rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+ /*
+ * We are here because DMA completed, or UART reception stopped and
+ * we think we have a packet in the buffer.
+ */
+ perf_begin(pc_txns);
+
+ /* disable UART DMA */
+ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
+ /* reset the idle counter */
+ idle_ticks = 0;
+
+ /* handle the received packet */
+ rx_handle_packet();
+
+ /* re-set DMA for reception first, so we are ready to receive before we start sending */
+ dma_reset();
+
+ /* send the reply to the just-processed request */
+ dma_packet.crc = 0;
+ dma_packet.crc = crc_packet(&dma_packet);
+ stm32_dmasetup(
+ tx_dma,
+ (uint32_t)&rDR,
+ (uint32_t)&dma_packet,
+ PKT_SIZE(dma_packet),
+ DMA_CCR_DIR |
+ DMA_CCR_MINC |
+ DMA_CCR_PSIZE_8BITS |
+ DMA_CCR_MSIZE_8BITS);
+ stm32_dmastart(tx_dma, NULL, NULL, false);
+ rCR3 |= USART_CR3_DMAT;
+
+ perf_end(pc_txns);
+}
+
+static int
+serial_interrupt(int irq, void *context)
+{
+ static bool abort_on_idle = false;
+
+ uint32_t sr = rSR; /* get UART status register */
+ (void)rDR; /* required to clear any of the interrupt status that brought us here */
+
+ if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
+ USART_SR_NE | /* noise error - we have lost a byte due to noise */
+ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
+
+ perf_count(pc_errors);
+ if (sr & USART_SR_ORE)
+ perf_count(pc_ore);
+ if (sr & USART_SR_NE)
+ perf_count(pc_ne);
+ if (sr & USART_SR_FE)
+ perf_count(pc_fe);
+
+ /* send a line break - this will abort transmission/reception on the other end */
+ rCR1 |= USART_CR1_SBK;
+
+ /* when the line goes idle, abort rather than look at the packet */
+ abort_on_idle = true;
+ }
+
+ if (sr & USART_SR_IDLE) {
+
+ /*
+ * If we saw an error, don't bother looking at the packet - it should have
+ * been aborted by the sender and will definitely be bad. Get the DMA reconfigured
+ * ready for their retry.
+ */
+ if (abort_on_idle) {
+
+ abort_on_idle = false;
+ dma_reset();
+ return 0;
+ }
+
+ /*
+ * The sender has stopped sending - this is probably the end of a packet.
+ * Check the received length against the length in the header to see if
+ * we have something that looks like a packet.
+ */
+ unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma);
+ if ((length < 1) || (length < PKT_SIZE(dma_packet))) {
+
+ /* it was too short - possibly truncated */
+ perf_count(pc_badidle);
+ return 0;
+ }
+
+ /*
+ * Looks like we received a packet. Stop the DMA and go process the
+ * packet.
+ */
+ perf_count(pc_idle);
+ stm32_dmastop(rx_dma);
+ rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL);
+ }
+
+ return 0;
+}
+
+static void
+dma_reset(void)
+{
+ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+ (void)rSR;
+ (void)rDR;
+ (void)rDR;
+
+ /* kill any pending DMA */
+ stm32_dmastop(tx_dma);
+ stm32_dmastop(rx_dma);
+
+ /* reset the RX side */
+ stm32_dmasetup(
+ rx_dma,
+ (uint32_t)&rDR,
+ (uint32_t)&dma_packet,
+ sizeof(dma_packet),
+ DMA_CCR_MINC |
+ DMA_CCR_PSIZE_8BITS |
+ DMA_CCR_MSIZE_8BITS);
+
+ /* start receive DMA ready for the next packet */
+ stm32_dmastart(rx_dma, rx_dma_callback, NULL, false);
+ rCR3 |= USART_CR3_DMAR;
+}
+
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-24 00:06:12 +0000