aboutsummaryrefslogtreecommitdiff
path: root/src/modules/px4iofirmware
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r--src/modules/px4iofirmware/controls.c243
-rw-r--r--src/modules/px4iofirmware/mixer.cpp15
-rw-r--r--src/modules/px4iofirmware/protocol.h20
-rw-r--r--src/modules/px4iofirmware/px4io.h10
-rw-r--r--src/modules/px4iofirmware/registers.c60
-rw-r--r--src/modules/px4iofirmware/sbus.c22
6 files changed, 240 insertions, 130 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 5e2c92bf4..941500f0d 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-2014 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
@@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm;
void
controls_init(void)
{
+ /* no channels */
+ r_raw_rc_count = 0;
+ system_state.rc_channels_timestamp_received = 0;
+ system_state.rc_channels_timestamp_valid = 0;
+
/* DSM input (USART1) */
dsm_init("/dev/ttyS0");
@@ -97,35 +102,62 @@ controls_tick() {
/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
uint16_t rssi = 0;
+#ifdef ADC_RSSI
+ if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
+ unsigned counts = adc_measure(ADC_RSSI);
+ if (counts != 0xffff) {
+ /* use 1:1 scaling on 3.3V ADC input */
+ unsigned mV = counts * 3300 / 4096;
+
+ /* scale to 0..253 */
+ rssi = mV / 13;
+ }
+ }
+#endif
+
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
- r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+ r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
- r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
- r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+ r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
- 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, &rssi, PX4IO_RC_INPUT_CHANNELS);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
+ bool sbus_failsafe, sbus_frame_drop;
+ bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
+
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
- }
- /* switch S.Bus output pin as needed */
- if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) {
- #ifdef ENABLE_SBUS_OUT
- ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS));
- #endif
+ rssi = 255;
+
+ if (sbus_frame_drop) {
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
+ rssi = 100;
+ } else {
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+ }
+
+ if (sbus_failsafe) {
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+ rssi = 0;
+ } else {
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+ }
+
}
perf_end(c_gather_sbus);
@@ -136,13 +168,12 @@ 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, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
+ bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
- /* XXX sample RSSI properly here */
- rssi = 255;
-
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_ppm);
@@ -150,6 +181,9 @@ controls_tick() {
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
+ /* store RSSI */
+ r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
+
/*
* In some cases we may have received a frame, but input has still
* been lost.
@@ -161,97 +195,100 @@ controls_tick() {
*/
if (dsm_updated || sbus_updated || ppm_updated) {
- /* update RC-received timestamp */
- system_state.rc_channels_timestamp = hrt_absolute_time();
-
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
- /* map raw inputs to mapped inputs */
- /* XXX mapping should be atomic relative to protocol */
- for (unsigned i = 0; i < r_raw_rc_count; i++) {
-
- /* map the input channel */
- uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
-
- if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
-
- uint16_t raw = r_raw_rc_values[i];
-
- int16_t scaled;
-
- /*
- * 1) Constrain to min/max values, as later processing depends on bounds.
- */
- if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
- raw = conf[PX4IO_P_RC_CONFIG_MIN];
- if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
- raw = conf[PX4IO_P_RC_CONFIG_MAX];
-
- /*
- * 2) Scale around the mid point differently for lower and upper range.
- *
- * This is necessary as they don't share the same endpoints and slope.
- *
- * First normalize to 0..1 range with correct sign (below or above center),
- * then scale to 20000 range (if center is an actual center, -10000..10000,
- * if parameters only support half range, scale to 10000 range, e.g. if
- * center == min 0..10000, if center == max -10000..0).
- *
- * As the min and max bounds were enforced in step 1), division by zero
- * cannot occur, as for the case of center == min or center == max the if
- * statement is mutually exclusive with the arithmetic NaN case.
- *
- * DO NOT REMOVE OR ALTER STEP 1!
- */
- if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
- scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
-
- } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
- scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
-
- } else {
- /* in the configured dead zone, output zero */
- scaled = 0;
- }
-
- /* invert channel if requested */
- if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
- scaled = -scaled;
+ /* update RC-received timestamp */
+ system_state.rc_channels_timestamp_received = hrt_absolute_time();
+
+ /* do not command anything in failsafe, kick in the RC loss counter */
+ if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+
+ /* update RC-received timestamp */
+ system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
+
+ /* map raw inputs to mapped inputs */
+ /* XXX mapping should be atomic relative to protocol */
+ for (unsigned i = 0; i < r_raw_rc_count; i++) {
+
+ /* map the input channel */
+ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+
+ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+
+ uint16_t raw = r_raw_rc_values[i];
+
+ int16_t scaled;
+
+ /*
+ * 1) Constrain to min/max values, as later processing depends on bounds.
+ */
+ if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+ raw = conf[PX4IO_P_RC_CONFIG_MIN];
+ if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+ raw = conf[PX4IO_P_RC_CONFIG_MAX];
+
+ /*
+ * 2) Scale around the mid point differently for lower and upper range.
+ *
+ * This is necessary as they don't share the same endpoints and slope.
+ *
+ * First normalize to 0..1 range with correct sign (below or above center),
+ * then scale to 20000 range (if center is an actual center, -10000..10000,
+ * if parameters only support half range, scale to 10000 range, e.g. if
+ * center == min 0..10000, if center == max -10000..0).
+ *
+ * As the min and max bounds were enforced in step 1), division by zero
+ * cannot occur, as for the case of center == min or center == max the if
+ * statement is mutually exclusive with the arithmetic NaN case.
+ *
+ * DO NOT REMOVE OR ALTER STEP 1!
+ */
+ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+ scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+
+ } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+ scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+
+ } else {
+ /* in the configured dead zone, output zero */
+ scaled = 0;
+ }
+
+ /* invert channel if requested */
+ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
+ scaled = -scaled;
- /* and update the scaled/mapped version */
- unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
- if (mapped < PX4IO_CONTROL_CHANNELS) {
+ /* and update the scaled/mapped version */
+ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+ 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);
+ }
}
}
- }
- /* set un-assigned controls to zero */
- for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
- if (!(assigned_channels & (1 << i)))
- r_rc_values[i] = 0;
- }
+ /* set un-assigned controls to zero */
+ for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+ if (!(assigned_channels & (1 << i)))
+ r_rc_values[i] = 0;
+ }
- /*
- * If we got an update with zero channels, treat it as
- * a loss of input.
- *
- * This might happen if a protocol-based receiver returns an update
- * that contains no channels that we have mapped.
- */
- if (assigned_channels == 0 || rssi == 0) {
- rc_input_lost = true;
- } else {
- /* set RC OK flag */
+ /* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+
+ /* if we have enough channels (5) to control the vehicle, the mapping is ok */
+ if (assigned_channels > 4) {
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+ } else {
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
+ }
}
/*
@@ -264,7 +301,7 @@ controls_tick() {
* If we haven't seen any new control data in 200ms, assume we
* have lost input.
*/
- if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
+ if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
rc_input_lost = true;
/* clear the input-kind flags here */
@@ -272,24 +309,32 @@ controls_tick() {
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
+
}
/*
* Handle losing RC input
*/
- if (rc_input_lost) {
+ /* this kicks in if the receiver is gone or the system went to failsafe */
+ if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
+ /* Mark all channels as invalid, as we just lost the RX */
+ r_rc_valid = 0;
+
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
+ }
- /* Mark the arrays as empty */
+ /* this kicks in if the receiver is completely gone */
+ if (rc_input_lost) {
+
+ /* Set channel count to zero */
r_raw_rc_count = 0;
- r_rc_valid = 0;
}
/*
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 2e79f0ac6..f39fcf7ec 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -223,14 +223,25 @@ mixer_tick(void)
}
}
- if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
+ /* set arming */
+ bool needs_to_arm = (should_arm || should_always_enable_pwm);
+
+ /* check any conditions that prevent arming */
+ if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
+ needs_to_arm = false;
+ }
+ if (!should_arm && !should_always_enable_pwm) {
+ needs_to_arm = false;
+ }
+
+ if (needs_to_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
isr_debug(5, "> PWM enabled");
- } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
+ } else if (!needs_to_arm && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index e5bef6eb3..d48c6c529 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -111,7 +111,6 @@
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -128,8 +127,6 @@
#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 */
@@ -140,7 +137,17 @@
/* array of raw RC input values, microseconds */
#define PX4IO_PAGE_RAW_RC_INPUT 4
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
-#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */
+#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */
+#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */
+#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */
+#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */
+#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */
+
+#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
+#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
+#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */
+#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */
+#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */
/* array of scaled RC input values, -10000..10000 */
#define PX4IO_PAGE_RC_INPUT 5
@@ -157,6 +164,10 @@
/* setup page */
#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
@@ -166,6 +177,7 @@
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */
+#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#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 */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 393e0560e..bb224f388 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-2014 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
@@ -96,8 +96,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
-#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE])
#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]
@@ -115,7 +116,8 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
*/
struct sys_state_s {
- volatile uint64_t rc_channels_timestamp;
+ volatile uint64_t rc_channels_timestamp_received;
+ volatile uint64_t rc_channels_timestamp_valid;
/**
* Last FMU receive time, in microseconds since system boot
@@ -215,7 +217,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 *rssi, uint16_t max_channels);
+extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, 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 2c437d2c0..1335f52e1 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -90,8 +90,6 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
[PX4IO_P_STATUS_PRSSI] = 0,
- [PX4IO_P_STATUS_NRSSI] = 0,
- [PX4IO_P_STATUS_RC_DATA] = 0
};
/**
@@ -116,6 +114,12 @@ 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_FLAGS] = 0,
+ [PX4IO_P_RAW_RC_NRSSI] = 0,
+ [PX4IO_P_RAW_RC_DATA] = 0,
+ [PX4IO_P_RAW_FRAME_COUNT] = 0,
+ [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0,
+ [PX4IO_P_RAW_RC_DATA] = 0,
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
};
@@ -144,7 +148,12 @@ uint16_t r_page_scratch[32];
*/
volatile uint16_t r_page_setup[] =
{
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+ /* default to RSSI ADC functionality */
+ [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI,
+#else
[PX4IO_P_SETUP_FEATURES] = 0,
+#endif
[PX4IO_P_SETUP_ARMING] = 0,
[PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
@@ -162,14 +171,22 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
};
-#define PX4IO_P_SETUP_FEATURES_VALID (0)
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
+ PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
+ PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
+ PX4IO_P_SETUP_FEATURES_PWM_RSSI)
+#else
+#define PX4IO_P_SETUP_FEATURES_VALID 0
+#endif
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
- PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
+ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
+ PX4IO_P_SETUP_ARMING_LOCKDOWN)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -438,9 +455,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_FEATURES:
value &= PX4IO_P_SETUP_FEATURES_VALID;
- r_setup_features = value;
- /* no implemented feature selection at this point */
+ /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
+
+ /* switch S.Bus output pin as needed */
+ #ifdef ENABLE_SBUS_OUT
+ ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
+
+ /* disable the conflicting options */
+ if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+ value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+ }
+ #endif
+
+ /* disable the conflicting options with ADC RSSI */
+ if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
+ value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+ PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+ PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+ }
+
+ /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
+ if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
+ value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+ PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+ PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+ }
+
+ /* apply changes */
+ r_setup_features = value;
break;
@@ -456,11 +499,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* lockup of the IO arming state.
*/
- // XXX do not reset IO's safety state by FMU for now
- // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
- // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
- // }
-
if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 495447740..f6ec542eb 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -87,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 *rssi, uint16_t max_channels);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
int
sbus_init(const char *device)
@@ -118,7 +118,7 @@ sbus_init(const char *device)
}
bool
-sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
+sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
{
ssize_t ret;
hrt_abstime now;
@@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_
* decode it.
*/
partial_frame_count = 0;
- return sbus_decode(now, values, num_values, rssi, max_channels);
+ return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
}
/*
@@ -215,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 *rssi, uint16_t max_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f)) {
@@ -289,20 +289,22 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
/* 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;
+ *sbus_failsafe = true;
+ *sbus_frame_drop = true;
}
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
+ /* set a special warning flag
*
* 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)
+ *sbus_failsafe = false;
+ *sbus_frame_drop = true;
+ } else {
+ *sbus_failsafe = false;
+ *sbus_frame_drop = false;
}
- *rssi = 255;
-
return true;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-01 07:03:37 +0000