aboutsummaryrefslogtreecommitdiff
path: root/src/modules/px4iofirmware
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r--src/modules/px4iofirmware/controls.c51
-rw-r--r--src/modules/px4iofirmware/dsm.c2
-rw-r--r--src/modules/px4iofirmware/mixer.cpp39
-rw-r--r--src/modules/px4iofirmware/protocol.h61
-rw-r--r--src/modules/px4iofirmware/px4io.c33
-rw-r--r--src/modules/px4iofirmware/px4io.h8
-rw-r--r--src/modules/px4iofirmware/registers.c55
-rw-r--r--src/modules/px4iofirmware/safety.c16
-rw-r--r--src/modules/px4iofirmware/sbus.c67
9 files changed, 242 insertions, 90 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 5c621cfb2..541eed0e1 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -50,7 +50,7 @@
#define RC_CHANNEL_HIGH_THRESH 5000
#define RC_CHANNEL_LOW_THRESH -5000
-static bool ppm_input(uint16_t *values, uint16_t *num_values);
+static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
@@ -66,7 +66,7 @@ controls_init(void)
sbus_init("/dev/ttyS2");
/* default to a 1:1 input map, all enabled */
- for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+ for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
@@ -94,6 +94,9 @@ controls_tick() {
* other. Don't do that.
*/
+ /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
+ uint16_t rssi = 0;
+
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
@@ -104,14 +107,15 @@ controls_tick() {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
else
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+
+ rssi = 255;
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
- bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */);
+ bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
- r_raw_rc_count = 8;
}
perf_end(c_gather_sbus);
@@ -121,11 +125,20 @@ controls_tick() {
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
- bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
- if (ppm_updated)
+ bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
+ if (ppm_updated) {
+
+ /* XXX sample RSSI properly here */
+ rssi = 255;
+
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
+ }
perf_end(c_gather_ppm);
+ /* limit number of channels to allowable data size */
+ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
+ r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
+
/*
* In some cases we may have received a frame, but input has still
* been lost.
@@ -197,14 +210,16 @@ controls_tick() {
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
- ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
+ if (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 */
- scaled = -scaled;
+ /* 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 */
+ scaled = -scaled;
+
+ r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+ assigned_channels |= (1 << mapped);
- r_rc_values[mapped] = SIGNED_TO_REG(scaled);
- assigned_channels |= (1 << mapped);
+ }
}
}
@@ -221,7 +236,7 @@ controls_tick() {
* This might happen if a protocol-based receiver returns an update
* that contains no channels that we have mapped.
*/
- if (assigned_channels == 0) {
+ if (assigned_channels == 0 || rssi == 0) {
rc_input_lost = true;
} else {
/* set RC OK flag */
@@ -306,7 +321,7 @@ controls_tick() {
}
static bool
-ppm_input(uint16_t *values, uint16_t *num_values)
+ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
{
bool result = false;
@@ -321,8 +336,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* PPM data exists, copy it */
*num_values = ppm_decoded_channels;
- if (*num_values > PX4IO_CONTROL_CHANNELS)
- *num_values = PX4IO_CONTROL_CHANNELS;
+ if (*num_values > PX4IO_RC_INPUT_CHANNELS)
+ *num_values = PX4IO_RC_INPUT_CHANNELS;
for (unsigned i = 0; i < *num_values; i++)
values[i] = ppm_buffer[i];
@@ -330,6 +345,10 @@ ppm_input(uint16_t *values, uint16_t *num_values)
/* clear validity */
ppm_last_valid_decode = 0;
+ /* store PPM frame length */
+ if (num_values)
+ *frame_len = ppm_frame_length;
+
/* good if we got any channels */
result = (*num_values > 0);
}
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index fd3b72015..4d306d6d0 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -355,7 +355,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
continue;
/* ignore channels out of range */
- if (channel >= PX4IO_INPUT_CHANNELS)
+ if (channel >= PX4IO_RC_INPUT_CHANNELS)
continue;
/* update the decoded channel count */
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 05897b4ce..e55ef784a 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -77,7 +77,8 @@ enum mixer_source {
MIX_NONE,
MIX_FMU,
MIX_OVERRIDE,
- MIX_FAILSAFE
+ MIX_FAILSAFE,
+ MIX_OVERRIDE_FMU_OK
};
static mixer_source source;
@@ -135,10 +136,19 @@ mixer_tick(void)
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
- !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
+ !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
+ !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
+ } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+ !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
+
+ /* if allowed, mix from RC inputs directly up to available rc channels */
+ source = MIX_OVERRIDE_FMU_OK;
}
}
@@ -185,7 +195,7 @@ mixer_tick(void)
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
- r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
+ r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
}
@@ -201,6 +211,10 @@ mixer_tick(void)
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
+
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
+ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
+ }
}
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
@@ -236,24 +250,35 @@ mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control)
{
- if (control_group != 0)
+ if (control_group > 3)
return -1;
switch (source) {
case MIX_FMU:
- if (control_index < PX4IO_CONTROL_CHANNELS) {
- control = REG_TO_FLOAT(r_page_controls[control_index]);
+ if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) {
+ control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
break;
}
return -1;
case MIX_OVERRIDE:
- if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
+ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
break;
}
return -1;
+ case MIX_OVERRIDE_FMU_OK:
+ /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
+ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
+ control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
+ break;
+ } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
+ control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
+ break;
+ }
+ return -1;
+
case MIX_FAILSAFE:
case MIX_NONE:
control = 0.0f;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 5e5396782..e5bef6eb3 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -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
@@ -63,7 +63,7 @@
* readable pages to be densely packed. Page numbers do not need to be
* packed.
*
- * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise,
+ * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise,
* [2] denotes definitions specific to the PX4IOv2 board.
*/
@@ -76,6 +76,9 @@
#define PX4IO_PROTOCOL_VERSION 4
+/* maximum allowable sizes on this protocol version */
+#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
+
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */
@@ -87,6 +90,7 @@
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */
+#define PX4IO_P_CONFIG_CONTROL_GROUP_COUNT 8 /**< hardcoded # of control groups*/
/* dynamic status page */
#define PX4IO_PAGE_STATUS 1
@@ -124,6 +128,8 @@
#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 */
+#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
+#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -184,44 +190,59 @@ enum { /* DSM bind states */
dsm_bind_reinit_uart
};
/* 8 */
-#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
+#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
+
+#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
+
+#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
/* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
+#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
+
+#define PX4IO_P_CONTROLS_GROUP_VALID 64
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */
/* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD 52
+#define PX4IO_PAGE_MIXERLOAD 52
/* R/C channel config */
-#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 */
-#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */
-#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */
-#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */
+#define PX4IO_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 */
+#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */
+#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
-#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
+#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */
/* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM 54 /* 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 55 /* 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 */
+#define PX4IO_PAGE_TEST 127
+#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */
/* PWM minimum values for certain ESCs */
-#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM maximum values for certain ESCs */
-#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM disarmed values that are active, even when SAFETY_SAFE */
-#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index ff9eecd74..745bd5705 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -45,6 +45,7 @@
#include <string.h>
#include <poll.h>
#include <signal.h>
+#include <crc32.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
@@ -117,6 +118,29 @@ show_debug_messages(void)
}
}
+static void
+heartbeat_blink(void)
+{
+ static bool heartbeat = false;
+ LED_BLUE(heartbeat = !heartbeat);
+}
+
+
+static void
+calculate_fw_crc(void)
+{
+#define APP_SIZE_MAX 0xf000
+#define APP_LOAD_ADDRESS 0x08001000
+ // compute CRC of the current firmware
+ uint32_t sum = 0;
+ for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
+ uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
+ sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
+ }
+ r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF;
+ r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
+}
+
int
user_start(int argc, char *argv[])
{
@@ -129,6 +153,9 @@ user_start(int argc, char *argv[])
/* configure the high-resolution time/callout interface */
hrt_init();
+ /* calculate our fw CRC so FMU can decide if we need to update */
+ calculate_fw_crc();
+
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
@@ -201,6 +228,7 @@ user_start(int argc, char *argv[])
*/
uint64_t last_debug_time = 0;
+ uint64_t last_heartbeat_time = 0;
for (;;) {
/* track the rate at which the loop is running */
@@ -216,6 +244,11 @@ user_start(int argc, char *argv[])
controls_tick();
perf_end(controls_perf);
+ if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) {
+ last_heartbeat_time = hrt_absolute_time();
+ heartbeat_blink();
+ }
+
#if 0
/* check for debug activity */
show_debug_messages();
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 4fea0288c..dea04a663 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -53,7 +53,9 @@
*/
#define PX4IO_SERVO_COUNT 8
#define PX4IO_CONTROL_CHANNELS 8
-#define PX4IO_INPUT_CHANNELS 8 // XXX this should be 18 channels
+#define PX4IO_CONTROL_GROUPS 2
+#define PX4IO_RC_INPUT_CHANNELS 18
+#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */
/*
* Debug logging
@@ -169,6 +171,8 @@ extern pwm_limit_t pwm_limit;
#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
+#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel)
+
/*
* Mixer
*/
@@ -209,7 +213,7 @@ extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
-extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
+extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 86a40bc22..0358725db 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.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
@@ -45,6 +45,8 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
+#include <systemlib/systemlib.h>
+#include <stm32_pwr.h>
#include "px4io.h"
#include "protocol.h"
@@ -68,7 +70,7 @@ static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
- [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS,
+ [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS,
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
};
@@ -87,7 +89,9 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_IBATT] = 0,
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
- [PX4IO_P_STATUS_PRSSI] = 0
+ [PX4IO_P_STATUS_PRSSI] = 0,
+ [PX4IO_P_STATUS_NRSSI] = 0,
+ [PX4IO_P_STATUS_RC_DATA] = 0
};
/**
@@ -112,7 +116,7 @@ uint16_t r_page_servos[PX4IO_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 + PX4IO_CONTROL_CHANNELS)] = 0
+ [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
};
/**
@@ -122,7 +126,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 + PX4IO_CONTROL_CHANNELS)] = 0
+ [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
};
/**
@@ -154,6 +158,8 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
#endif
[PX4IO_P_SETUP_SET_DEBUG] = 0,
+ [PX4IO_P_SETUP_REBOOT_BL] = 0,
+ [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
@@ -172,7 +178,7 @@ volatile uint16_t r_page_setup[] =
*
* Control values from the FMU.
*/
-volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
+volatile uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
/*
* PAGE 102 does not have a buffer.
@@ -183,7 +189,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
*
* R/C channel input configuration.
*/
-uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_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)
@@ -235,7 +241,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
case PX4IO_PAGE_CONTROLS:
/* copy channel data */
- while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+ while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_controls[offset] = *values;
@@ -501,6 +507,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
break;
+ case PX4IO_P_SETUP_REBOOT_BL:
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
+ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+ // don't allow reboot while armed
+ break;
+ }
+
+ // check the magic value
+ if (value != PX4IO_REBOOT_BL_MAGIC)
+ break;
+
+ // note that we don't set BL_WAIT_MAGIC in
+ // BKP_DR1 as that is not necessary given the
+ // timing of the forceupdate command. The
+ // bootloader on px4io waits for enough time
+ // anyway, and this method works with older
+ // bootloader versions (tested with both
+ // revision 3 and revision 4).
+
+ up_systemreset();
+ break;
+
case PX4IO_P_SETUP_DSM:
dsm_bind(value & 0x0f, (value >> 4) & 7);
break;
@@ -525,7 +554,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 >= PX4IO_CONTROL_CHANNELS)
+ if (channel >= PX4IO_RC_INPUT_CHANNELS)
return -1;
/* disable the channel until we have a chance to sanity-check it */
@@ -555,6 +584,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* this option is normally set last */
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint8_t count = 0;
+ bool disabled = false;
/* assert min..center..max ordering */
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
@@ -573,7 +603,10 @@ 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] >= PX4IO_CONTROL_CHANNELS) {
+
+ if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
+ disabled = true;
+ } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
count++;
}
@@ -581,7 +614,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (count) {
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
- } else {
+ } else if (!disabled) {
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
}
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 95335f038..cdb54a80a 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -77,7 +77,6 @@ static unsigned blink_counter = 0;
static bool safety_button_pressed;
static void safety_check_button(void *arg);
-static void heartbeat_blink(void *arg);
static void failsafe_blink(void *arg);
void
@@ -86,9 +85,6 @@ safety_init(void)
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
- /* arrange for the heartbeat handler to be called at 4Hz */
- hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
-
/* arrange for the failsafe blinker to be called at 8Hz */
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
}
@@ -164,16 +160,6 @@ safety_check_button(void *arg)
}
static void
-heartbeat_blink(void *arg)
-{
- static bool heartbeat = false;
-
- /* XXX add flags here that need to be frobbed by various loops */
-
- LED_BLUE(heartbeat = !heartbeat);
-}
-
-static void
failsafe_blink(void *arg)
{
/* indicate that a serious initialisation error occured */
@@ -192,4 +178,4 @@ failsafe_blink(void *arg)
}
LED_AMBER(failsafe);
-} \ No newline at end of file
+}
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index c523df6ca..11ccd7356 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -54,6 +54,27 @@
#define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16
+#define SBUS_FLAGS_BYTE 23
+#define SBUS_FAILSAFE_BIT 3
+#define SBUS_FRAMELOST_BIT 2
+
+/*
+ Measured values with Futaba FX-30/R6108SB:
+ -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV)
+ -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV)
+ -+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks)
+*/
+
+/* define range mapping here, -+100% -> 1000..2000 */
+#define SBUS_RANGE_MIN 200.0f
+#define SBUS_RANGE_MAX 1800.0f
+
+#define SBUS_TARGET_MIN 1000.0f
+#define SBUS_TARGET_MAX 2000.0f
+
+/* pre-calculate the floating point stuff as far as possible at compile time */
+#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
+#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
static int sbus_fd = -1;
@@ -66,7 +87,7 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
int
sbus_init(const char *device)
@@ -97,7 +118,7 @@ sbus_init(const char *device)
}
bool
-sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
+sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
{
ssize_t ret;
hrt_abstime now;
@@ -154,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
* decode it.
*/
partial_frame_count = 0;
- return sbus_decode(now, values, num_values, max_channels);
+ return sbus_decode(now, values, num_values, rssi, max_channels);
}
/*
@@ -194,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
};
static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
@@ -202,15 +223,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
return false;
}
- /* if the failsafe or connection lost bit is set, we consider the frame invalid */
- if ((frame[23] & (1 << 2)) && /* signal lost */
- (frame[23] & (1 << 3))) { /* failsafe */
-
- /* actively announce signal loss */
- *values = 0;
- return false;
- }
-
/* we have received something we think is a frame */
last_frame_time = frame_time;
@@ -234,22 +246,41 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
}
}
- /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
- values[channel] = (value / 2) + 998;
+
+ /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
+ values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
}
/* decode switch channels if data fields are wide enough */
- if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) {
+ if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) {
chancount = 18;
/* channel 17 (index 16) */
- values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
+ values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
/* channel 18 (index 17) */
- values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
+ values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
}
/* note the number of channels decoded */
*num_values = chancount;
+ /* decode and handle failsafe and frame-lost flags */
+ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
+ /* report that we failed to read anything valid off the receiver */
+ *rssi = 0;
+ return false;
+ }
+ else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
+ /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented
+ *
+ * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
+ * condition as fail-safe greatly reduces the reliability and range of the radio link,
+ * e.g. by prematurely issueing return-to-launch!!! */
+
+ *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
+ }
+
+ *rssi = 255;
+
return true;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-01 20:36:30 +0000