aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/gpio_led/gpio_led.c10
-rw-r--r--src/modules/px4iofirmware/controls.c16
-rw-r--r--src/modules/px4iofirmware/dsm.c5
-rw-r--r--src/modules/px4iofirmware/i2c.c15
-rw-r--r--src/modules/px4iofirmware/mixer.cpp16
-rw-r--r--src/modules/px4iofirmware/module.mk11
-rw-r--r--src/modules/px4iofirmware/protocol.h144
-rw-r--r--src/modules/px4iofirmware/px4io.c26
-rw-r--r--src/modules/px4iofirmware/px4io.h79
-rw-r--r--src/modules/px4iofirmware/registers.c136
-rw-r--r--src/modules/px4iofirmware/serial.c352
-rw-r--r--src/modules/systemlib/hx_stream.c185
-rw-r--r--src/modules/systemlib/hx_stream.h42
-rw-r--r--src/modules/systemlib/perf_counter.c41
-rw-r--r--src/modules/systemlib/perf_counter.h14
15 files changed, 891 insertions, 201 deletions
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 1aef739c7..7466dfdb9 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -109,19 +109,19 @@ int gpio_led_main(int argc, char *argv[])
} else if (!strcmp(argv[3], "a1")) {
use_io = true;
- pin = PX4IO_ACC1;
+ pin = PX4IO_P_SETUP_RELAYS_ACC1;
} else if (!strcmp(argv[3], "a2")) {
use_io = true;
- pin = PX4IO_ACC2;
+ pin = PX4IO_P_SETUP_RELAYS_ACC2;
} else if (!strcmp(argv[3], "r1")) {
use_io = true;
- pin = PX4IO_RELAY1;
+ pin = PX4IO_P_SETUP_RELAYS_POWER1;
} else if (!strcmp(argv[3], "r2")) {
use_io = true;
- pin = PX4IO_RELAY2;
+ pin = PX4IO_P_SETUP_RELAYS_POWER2;
} else {
errx(1, "unsupported pin: %s", argv[3]);
@@ -142,7 +142,7 @@ int gpio_led_main(int argc, char *argv[])
char pin_name[24];
if (use_io) {
- if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
+ if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) {
sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
} else {
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 43d96fb06..74037d1c2 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;
}
@@ -319,8 +319,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 ab6e3fec4..598bcee34 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -110,6 +110,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:
// power down DSM satellite
@@ -135,6 +139,7 @@ dsm_bind(uint16_t cmd, int pulses)
stm32_configgpio(GPIO_USART1_RX);
break;
}
+#endif
}
bool
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..d8c0e58ba 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -154,7 +154,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 +164,11 @@ mixer_tick(void)
} else if (source != MIX_NONE) {
- 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++) {
@@ -180,7 +180,7 @@ mixer_tick(void)
r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
}
- for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
@@ -215,7 +215,7 @@ mixer_tick(void)
if (mixer_servos_armed) {
/* 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]);
}
}
@@ -349,11 +349,11 @@ mixer_set_failsafe()
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 +364,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..dc5c40638 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 2
+
/* 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 */
@@ -108,16 +109,20 @@
#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* 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 +148,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 */
@@ -156,33 +161,34 @@
#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) /* [1] power relay 1 */
+#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* [1] power relay 2 */
+#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* [1] accessory power 1 */
+#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* [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 /* [1] battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* [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 +200,14 @@ 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 */
/**
* As-needed mixer data upload.
@@ -218,3 +228,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 83feeb9b6..bd78f2638 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -42,15 +42,21 @@
#include <stdbool.h>
#include <stdint.h>
-#include <drivers/boards/px4io/px4io_internal.h>
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+# include <drivers/boards/px4io/px4io_internal.h>
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+# include <drivers/boards/px4iov2/px4iov2_internal.h>
+#endif
#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
@@ -119,32 +125,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 +173,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 +207,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 */
-extern void isr_debug(uint8_t level, const char *fmt, ...);
-
-#ifdef CONFIG_STM32_I2C1
-void i2c_dump(void);
-void i2c_reset(void);
-#endif
+/** send a debug message to the console */
+extern void isr_debug(uint8_t level, const char *fmt, ...);
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index a922362b6..9606faa86 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
};
/**
@@ -147,7 +155,7 @@ volatile uint16_t r_page_setup[] =
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)
+#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
/**
@@ -166,7 +174,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 +190,9 @@ 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
+int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
@@ -233,7 +241,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;
@@ -260,11 +268,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
@@ -349,10 +359,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:
@@ -381,7 +403,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 */
@@ -426,7 +448,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 +468,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 +512,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 +546,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 +557,74 @@ 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:
+ *
+ * V counts
+ * 5 1001
+ * 6 1219
+ * 7 1436
+ * 8 1653
+ * 9 1870
+ * 10 2086
+ * 11 2303
+ * 12 2522
+ * 13 2738
+ * 14 2956
+ * 15 3172
+ * 16 3389
+ *
+ * slope = 0.0046067
+ * intercept = 0.3863
+ *
+ * Intercept corrected for best results @ 12V.
+ */
+ unsigned counts = adc_measure(ADC_VSERVO);
+ if (counts != 0xffff) {
+ unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+ unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_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);
@@ -616,7 +696,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/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;
+}
+
diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index 8d77f14a8..8e9c2bfcf 100644
--- a/src/modules/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
@@ -53,14 +53,26 @@
struct hx_stream {
- uint8_t buf[HX_STREAM_MAX_FRAME + 4];
- unsigned frame_bytes;
- bool escaped;
- bool txerror;
-
+ /* RX state */
+ uint8_t rx_buf[HX_STREAM_MAX_FRAME + 4];
+ unsigned rx_frame_bytes;
+ bool rx_escaped;
+ hx_stream_rx_callback rx_callback;
+ void *rx_callback_arg;
+
+ /* TX state */
int fd;
- hx_stream_rx_callback callback;
- void *callback_arg;
+ bool tx_error;
+ uint8_t *tx_buf;
+ unsigned tx_resid;
+ uint32_t tx_crc;
+ enum {
+ TX_IDLE = 0,
+ TX_SEND_START,
+ TX_SEND_DATA,
+ TX_SENT_ESCAPE,
+ TX_SEND_END
+ } tx_state;
perf_counter_t pc_tx_frames;
perf_counter_t pc_rx_frames;
@@ -81,21 +93,7 @@ static void
hx_tx_raw(hx_stream_t stream, uint8_t c)
{
if (write(stream->fd, &c, 1) != 1)
- stream->txerror = true;
-}
-
-static void
-hx_tx_byte(hx_stream_t stream, uint8_t c)
-{
- switch (c) {
- case FBO:
- case CEO:
- hx_tx_raw(stream, CEO);
- c ^= 0x20;
- break;
- }
-
- hx_tx_raw(stream, c);
+ stream->tx_error = true;
}
static int
@@ -105,11 +103,11 @@ hx_rx_frame(hx_stream_t stream)
uint8_t b[4];
uint32_t w;
} u;
- unsigned length = stream->frame_bytes;
+ unsigned length = stream->rx_frame_bytes;
/* reset the stream */
- stream->frame_bytes = 0;
- stream->escaped = false;
+ stream->rx_frame_bytes = 0;
+ stream->rx_escaped = false;
/* not a real frame - too short */
if (length < 4) {
@@ -122,11 +120,11 @@ hx_rx_frame(hx_stream_t stream)
length -= 4;
/* compute expected CRC */
- u.w = crc32(&stream->buf[0], length);
+ u.w = crc32(&stream->rx_buf[0], length);
/* compare computed and actual CRC */
for (unsigned i = 0; i < 4; i++) {
- if (u.b[i] != stream->buf[length + i]) {
+ if (u.b[i] != stream->rx_buf[length + i]) {
perf_count(stream->pc_rx_errors);
return 0;
}
@@ -134,7 +132,7 @@ hx_rx_frame(hx_stream_t stream)
/* frame is good */
perf_count(stream->pc_rx_frames);
- stream->callback(stream->callback_arg, &stream->buf[0], length);
+ stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
return 1;
}
@@ -150,8 +148,8 @@ hx_stream_init(int fd,
if (stream != NULL) {
memset(stream, 0, sizeof(struct hx_stream));
stream->fd = fd;
- stream->callback = callback;
- stream->callback_arg = arg;
+ stream->rx_callback = callback;
+ stream->rx_callback_arg = arg;
}
return stream;
@@ -179,49 +177,112 @@ hx_stream_set_counters(hx_stream_t stream,
stream->pc_rx_errors = rx_errors;
}
+void
+hx_stream_reset(hx_stream_t stream)
+{
+ stream->rx_frame_bytes = 0;
+ stream->rx_escaped = false;
+
+ stream->tx_buf = NULL;
+ stream->tx_resid = 0;
+ stream->tx_state = TX_IDLE;
+}
+
int
-hx_stream_send(hx_stream_t stream,
+hx_stream_start(hx_stream_t stream,
const void *data,
size_t count)
{
- union {
- uint8_t b[4];
- uint32_t w;
- } u;
- const uint8_t *p = (const uint8_t *)data;
- unsigned resid = count;
-
- if (resid > HX_STREAM_MAX_FRAME)
+ if (count > HX_STREAM_MAX_FRAME)
return -EINVAL;
- /* start the frame */
- hx_tx_raw(stream, FBO);
+ stream->tx_buf = data;
+ stream->tx_resid = count;
+ stream->tx_state = TX_SEND_START;
+ stream->tx_crc = crc32(data, count);
+ return OK;
+}
+
+int
+hx_stream_send_next(hx_stream_t stream)
+{
+ int c;
+
+ /* sort out what we're going to send */
+ switch (stream->tx_state) {
- /* transmit the data */
- while (resid--)
- hx_tx_byte(stream, *p++);
+ case TX_SEND_START:
+ stream->tx_state = TX_SEND_DATA;
+ return FBO;
- /* compute the CRC */
- u.w = crc32(data, count);
+ case TX_SEND_DATA:
+ c = *stream->tx_buf;
- /* send the CRC */
- p = &u.b[0];
- resid = 4;
+ switch (c) {
+ case FBO:
+ case CEO:
+ stream->tx_state = TX_SENT_ESCAPE;
+ return CEO;
+ }
+ break;
+
+ case TX_SENT_ESCAPE:
+ c = *stream->tx_buf ^ 0x20;
+ stream->tx_state = TX_SEND_DATA;
+ break;
+
+ case TX_SEND_END:
+ stream->tx_state = TX_IDLE;
+ return FBO;
+
+ case TX_IDLE:
+ default:
+ return -1;
+ }
+
+ /* if we are here, we have consumed a byte from the buffer */
+ stream->tx_resid--;
+ stream->tx_buf++;
+
+ /* buffer exhausted */
+ if (stream->tx_resid == 0) {
+ uint8_t *pcrc = (uint8_t *)&stream->tx_crc;
+
+ /* was the buffer the frame CRC? */
+ if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) {
+ stream->tx_state = TX_SEND_END;
+ } else {
+ /* no, it was the payload - switch to sending the CRC */
+ stream->tx_buf = pcrc;
+ stream->tx_resid = sizeof(stream->tx_crc);
+ }
+ }
+ return c;
+}
+
+int
+hx_stream_send(hx_stream_t stream,
+ const void *data,
+ size_t count)
+{
+ int result;
- while (resid--)
- hx_tx_byte(stream, *p++);
+ result = hx_stream_start(stream, data, count);
+ if (result != OK)
+ return result;
- /* and the trailing frame separator */
- hx_tx_raw(stream, FBO);
+ int c;
+ while ((c = hx_stream_send_next(stream)) >= 0)
+ hx_tx_raw(stream, c);
/* check for transmit error */
- if (stream->txerror) {
- stream->txerror = false;
+ if (stream->tx_error) {
+ stream->tx_error = false;
return -EIO;
}
perf_count(stream->pc_tx_frames);
- return 0;
+ return OK;
}
void
@@ -234,17 +295,17 @@ hx_stream_rx(hx_stream_t stream, uint8_t c)
}
/* escaped? */
- if (stream->escaped) {
- stream->escaped = false;
+ if (stream->rx_escaped) {
+ stream->rx_escaped = false;
c ^= 0x20;
} else if (c == CEO) {
- /* now escaped, ignore the byte */
- stream->escaped = true;
+ /* now rx_escaped, ignore the byte */
+ stream->rx_escaped = true;
return;
}
/* save for later */
- if (stream->frame_bytes < sizeof(stream->buf))
- stream->buf[stream->frame_bytes++] = c;
+ if (stream->rx_frame_bytes < sizeof(stream->rx_buf))
+ stream->rx_buf[stream->rx_frame_bytes++] = c;
}
diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h
index 128689953..1f3927222 100644
--- a/src/modules/systemlib/hx_stream.h
+++ b/src/modules/systemlib/hx_stream.h
@@ -58,7 +58,8 @@ __BEGIN_DECLS
* Allocate a new hx_stream object.
*
* @param fd The file handle over which the protocol will
- * communicate.
+ * communicate, or -1 if the protocol will use
+ * hx_stream_start/hx_stream_send_next.
* @param callback Called when a frame is received.
* @param callback_arg Passed to the callback.
* @return A handle to the stream, or NULL if memory could
@@ -80,6 +81,7 @@ __EXPORT extern void hx_stream_free(hx_stream_t stream);
*
* Any counter may be set to NULL to disable counting that datum.
*
+ * @param stream A handle returned from hx_stream_init.
* @param tx_frames Counter for transmitted frames.
* @param rx_frames Counter for received frames.
* @param rx_errors Counter for short and corrupt received frames.
@@ -90,6 +92,44 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream,
perf_counter_t rx_errors);
/**
+ * Reset a stream.
+ *
+ * Forces the local stream state to idle.
+ *
+ * @param stream A handle returned from hx_stream_init.
+ */
+__EXPORT extern void hx_stream_reset(hx_stream_t stream);
+
+/**
+ * Prepare to send a frame.
+ *
+ * Use this in conjunction with hx_stream_send_next to
+ * set the frame to be transmitted.
+ *
+ * Use hx_stream_send() to write to the stream fd directly.
+ *
+ * @param stream A handle returned from hx_stream_init.
+ * @param data Pointer to the data to send.
+ * @param count The number of bytes to send.
+ * @return Zero on success, -errno on error.
+ */
+__EXPORT extern int hx_stream_start(hx_stream_t stream,
+ const void *data,
+ size_t count);
+
+/**
+ * Get the next byte to send for a stream.
+ *
+ * This requires that the stream be prepared for sending by
+ * calling hx_stream_start first.
+ *
+ * @param stream A handle returned from hx_stream_init.
+ * @return The byte to send, or -1 if there is
+ * nothing left to send.
+ */
+__EXPORT extern int hx_stream_send_next(hx_stream_t stream);
+
+/**
* Send a frame.
*
* This function will block until all frame bytes are sent if
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index 879f4715a..3c1e10287 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -201,23 +201,50 @@ perf_end(perf_counter_t handle)
switch (handle->type) {
case PC_ELAPSED: {
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
- hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
- pce->event_count++;
- pce->time_total += elapsed;
+ if (pce->time_start != 0) {
+ hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
- if ((pce->time_least > elapsed) || (pce->time_least == 0))
- pce->time_least = elapsed;
+ pce->event_count++;
+ pce->time_total += elapsed;
- if (pce->time_most < elapsed)
- pce->time_most = elapsed;
+ if ((pce->time_least > elapsed) || (pce->time_least == 0))
+ pce->time_least = elapsed;
+
+ if (pce->time_most < elapsed)
+ pce->time_most = elapsed;
+
+ pce->time_start = 0;
+ }
+ }
+ break;
+
+ default:
+ break;
+ }
+}
+
+void
+perf_cancel(perf_counter_t handle)
+{
+ if (handle == NULL)
+ return;
+
+ switch (handle->type) {
+ case PC_ELAPSED: {
+ struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+ pce->time_start = 0;
}
+ break;
default:
break;
}
}
+
+
void
perf_reset(perf_counter_t handle)
{
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 5c2cb15b2..4cd8b67a1 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -92,13 +92,25 @@ __EXPORT extern void perf_begin(perf_counter_t handle);
* End a performance event.
*
* This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
*
* @param handle The handle returned from perf_alloc.
*/
__EXPORT extern void perf_end(perf_counter_t handle);
/**
- * Reset a performance event.
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle The handle returned from perf_alloc.
+ */
+__EXPORT extern void perf_cancel(perf_counter_t handle);
+
+/**
+ * Reset a performance counter.
*
* This call resets performance counter to initial state
*
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-12 22:37:23 +0000