aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/commander/state_machine_helper.c2
-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.c19
-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.h72
-rw-r--r--src/modules/px4iofirmware/registers.c128
-rw-r--r--src/modules/px4iofirmware/serial.c352
-rw-r--r--src/modules/sensors/sensor_params.c9
-rw-r--r--src/modules/sensors/sensors.cpp159
-rw-r--r--src/modules/systemlib/systemlib.c18
-rw-r--r--src/modules/systemlib/systemlib.h6
16 files changed, 846 insertions, 157 deletions
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c
index ab728c7bb..9b6527c33 100644
--- a/src/modules/commander/state_machine_helper.c
+++ b/src/modules/commander/state_machine_helper.c
@@ -141,7 +141,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
usleep(500000);
- up_systemreset();
+ systemreset(false);
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
} else {
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 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..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..3c59a75a7 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) /* 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 +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 57cffcc23..b30d155bc 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
@@ -119,32 +120,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 +168,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 +202,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..3f241d29c 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,
};
@@ -147,7 +159,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 +178,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 +194,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 +245,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 +272,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 +363,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 +407,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 +452,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 +472,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 +516,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 +550,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 +561,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);
@@ -616,7 +688,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/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index bd431c9eb..8d3992963 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -68,7 +68,10 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
-PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667);
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
+
+PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
@@ -158,8 +161,12 @@ PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT,
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
+#else
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
+#endif
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 42268b971..c47f6bb7d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -50,6 +50,7 @@
#include <stdio.h>
#include <errno.h>
#include <math.h>
+#include <mathlib/mathlib.h>
#include <nuttx/analog/adc.h>
@@ -138,6 +139,77 @@
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
/**
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
+ */
+enum Rotation {
+ ROTATION_NONE = 0,
+ ROTATION_YAW_45 = 1,
+ ROTATION_YAW_90 = 2,
+ ROTATION_YAW_135 = 3,
+ ROTATION_YAW_180 = 4,
+ ROTATION_YAW_225 = 5,
+ ROTATION_YAW_270 = 6,
+ ROTATION_YAW_315 = 7,
+ ROTATION_ROLL_180 = 8,
+ ROTATION_ROLL_180_YAW_45 = 9,
+ ROTATION_ROLL_180_YAW_90 = 10,
+ ROTATION_ROLL_180_YAW_135 = 11,
+ ROTATION_PITCH_180 = 12,
+ ROTATION_ROLL_180_YAW_225 = 13,
+ ROTATION_ROLL_180_YAW_270 = 14,
+ ROTATION_ROLL_180_YAW_315 = 15,
+ ROTATION_ROLL_90 = 16,
+ ROTATION_ROLL_90_YAW_45 = 17,
+ ROTATION_ROLL_90_YAW_90 = 18,
+ ROTATION_ROLL_90_YAW_135 = 19,
+ ROTATION_ROLL_270 = 20,
+ ROTATION_ROLL_270_YAW_45 = 21,
+ ROTATION_ROLL_270_YAW_90 = 22,
+ ROTATION_ROLL_270_YAW_135 = 23,
+ ROTATION_PITCH_90 = 24,
+ ROTATION_PITCH_270 = 25,
+ ROTATION_MAX
+};
+
+typedef struct
+{
+ uint16_t roll;
+ uint16_t pitch;
+ uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] =
+{
+ { 0, 0, 0 },
+ { 0, 0, 45 },
+ { 0, 0, 90 },
+ { 0, 0, 135 },
+ { 0, 0, 180 },
+ { 0, 0, 225 },
+ { 0, 0, 270 },
+ { 0, 0, 315 },
+ {180, 0, 0 },
+ {180, 0, 45 },
+ {180, 0, 90 },
+ {180, 0, 135 },
+ { 0, 180, 0 },
+ {180, 0, 225 },
+ {180, 0, 270 },
+ {180, 0, 315 },
+ { 90, 0, 0 },
+ { 90, 0, 45 },
+ { 90, 0, 90 },
+ { 90, 0, 135 },
+ {270, 0, 0 },
+ {270, 0, 45 },
+ {270, 0, 90 },
+ {270, 0, 135 },
+ { 0, 90, 0 },
+ { 0, 270, 0 }
+};
+
+/**
* Sensor app start / stop handling function
*
* @ingroup apps
@@ -189,8 +261,8 @@ private:
int _mag_sub; /**< raw mag data subscription */
int _rc_sub; /**< raw rc channels data subscription */
int _baro_sub; /**< raw baro data subscription */
- int _airspeed_sub; /**< airspeed subscription */
- int _diff_pres_sub; /**< raw differential pressure subscription */
+ int _airspeed_sub; /**< airspeed subscription */
+ int _diff_pres_sub; /**< raw differential pressure subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
@@ -210,13 +282,15 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
+ math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
+ math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
+
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
float max[_rc_max_chan_count];
float rev[_rc_max_chan_count];
float dz[_rc_max_chan_count];
- // float ex[_rc_max_chan_count];
float scaling_factor[_rc_max_chan_count];
float gyro_offset[3];
@@ -227,6 +301,9 @@ private:
float accel_scale[3];
float diff_pres_offset_pa;
+ int board_rotation;
+ int external_mag_rotation;
+
int rc_type;
int rc_map_roll;
@@ -265,7 +342,6 @@ private:
param_t max[_rc_max_chan_count];
param_t rev[_rc_max_chan_count];
param_t dz[_rc_max_chan_count];
- // param_t ex[_rc_max_chan_count];
param_t rc_type;
param_t rc_demix;
@@ -306,6 +382,9 @@ private:
param_t battery_voltage_scaling;
+ param_t board_rotation;
+ param_t external_mag_rotation;
+
} _parameter_handles; /**< handles for interesting parameters */
@@ -315,6 +394,11 @@ private:
int parameters_update();
/**
+ * Get the rotation matrices
+ */
+ void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
+ /**
* Do accel-related initialisation.
*/
void accel_init();
@@ -450,7 +534,10 @@ Sensors::Sensors() :
_diff_pres_pub(-1),
/* performance counters */
- _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
+ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
+
+ _board_rotation(3,3),
+ _external_mag_rotation(3,3)
{
/* basic r/c parameters */
@@ -540,6 +627,10 @@ Sensors::Sensors() :
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
+ /* rotations */
+ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+ _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+
/* fetch initial parameter values */
parameters_update();
}
@@ -731,10 +822,34 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
+ param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+ param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
+
+ get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
+ get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+
return OK;
}
void
+Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+{
+ /* first set to zero */
+ rot_matrix->Matrix::zero(3,3);
+
+ float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
+ float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
+ float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
+
+ math::EulerAngles euler(roll, pitch, yaw);
+
+ math::Dcm R(euler);
+
+ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+ (*rot_matrix)(i,j) = R(i, j);
+}
+
+void
Sensors::accel_init()
{
int fd;
@@ -757,7 +872,7 @@ Sensors::accel_init()
/* set the driver to poll at 1000Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 1000);
- #else
+ #elif CONFIG_ARCH_BOARD_PX4FMU_V2
/* set the accel internal sampling rate up to at leat 800Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -765,6 +880,9 @@ Sensors::accel_init()
/* set the driver to poll at 800Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 800);
+ #else
+ #error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+
#endif
warnx("using system accel");
@@ -874,9 +992,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
- raw.accelerometer_m_s2[0] = accel_report.x;
- raw.accelerometer_m_s2[1] = accel_report.y;
- raw.accelerometer_m_s2[2] = accel_report.z;
+ math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+ vect = _board_rotation*vect;
+
+ raw.accelerometer_m_s2[0] = vect(0);
+ raw.accelerometer_m_s2[1] = vect(1);
+ raw.accelerometer_m_s2[2] = vect(2);
raw.accelerometer_raw[0] = accel_report.x_raw;
raw.accelerometer_raw[1] = accel_report.y_raw;
@@ -897,9 +1018,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
- raw.gyro_rad_s[0] = gyro_report.x;
- raw.gyro_rad_s[1] = gyro_report.y;
- raw.gyro_rad_s[2] = gyro_report.z;
+ math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+ vect = _board_rotation*vect;
+
+ raw.gyro_rad_s[0] = vect(0);
+ raw.gyro_rad_s[1] = vect(1);
+ raw.gyro_rad_s[2] = vect(2);
raw.gyro_raw[0] = gyro_report.x_raw;
raw.gyro_raw[1] = gyro_report.y_raw;
@@ -920,9 +1044,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
- raw.magnetometer_ga[0] = mag_report.x;
- raw.magnetometer_ga[1] = mag_report.y;
- raw.magnetometer_ga[2] = mag_report.z;
+ // XXX TODO add support for external mag orientation
+
+ math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+ vect = _board_rotation*vect;
+
+ raw.magnetometer_ga[0] = vect(0);
+ raw.magnetometer_ga[1] = vect(1);
+ raw.magnetometer_ga[2] = vect(2);
raw.magnetometer_raw[0] = mag_report.x_raw;
raw.magnetometer_raw[1] = mag_report.y_raw;
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 3283aad8a..57a751e1c 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -43,13 +43,29 @@
#include <fcntl.h>
#include <sched.h>
#include <signal.h>
-#include <sys/stat.h>
#include <unistd.h>
#include <float.h>
#include <string.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include <stm32_pwr.h>
+
#include "systemlib.h"
+void
+systemreset(bool to_bootloader)
+{
+ if (to_bootloader) {
+ stm32_pwr_enablebkp();
+
+ /* XXX wow, this is evil - write a magic number into backup register zero */
+ *(uint32_t *)0x40002850 = 0xb007b007;
+ }
+ up_systemreset();
+}
+
static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
void killall()
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 0194b5e52..3728f2067 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -42,11 +42,11 @@
#include <float.h>
#include <stdint.h>
-/** Reboots the board */
-extern void up_systemreset(void) noreturn_function;
-
__BEGIN_DECLS
+/** Reboots the board */
+__EXPORT void systemreset(bool to_bootloader) noreturn_function;
+
/** Sends SIGUSR1 to all processes */
__EXPORT void killall(void);
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-06 16:07:31 +0000