aboutsummaryrefslogtreecommitdiff
path: root/src/modules/px4iofirmware
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r--src/modules/px4iofirmware/controls.c201
-rw-r--r--src/modules/px4iofirmware/mixer.cpp17
-rw-r--r--src/modules/px4iofirmware/protocol.h38
-rw-r--r--src/modules/px4iofirmware/px4io.h5
-rw-r--r--src/modules/px4iofirmware/registers.c46
-rw-r--r--src/modules/px4iofirmware/sbus.c15
6 files changed, 203 insertions, 119 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 941500f0d..62e6b12cb 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -134,8 +134,6 @@ controls_tick() {
perf_begin(c_gather_sbus);
- 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);
@@ -201,94 +199,105 @@ controls_tick() {
/* 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;
+ /* 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;
+ }
- /* and update the scaled/mapped version */
- unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
- if (mapped < PX4IO_CONTROL_CHANNELS) {
+ /* invert channel if requested */
+ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
+ 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;
+ /* and update the scaled/mapped version */
+ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+ if (mapped < PX4IO_CONTROL_CHANNELS) {
- r_rc_values[mapped] = SIGNED_TO_REG(scaled);
- assigned_channels |= (1 << mapped);
+ /* 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;
+ }
+ if (mapped == 3 && r_setup_rc_thr_failsafe) {
+ /* throttle failsafe detection */
+ if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
+ ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+ } else {
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+ }
}
+
+ 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;
}
+ }
- /* set RC OK flag, as we got an update */
- r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+ /* set RC OK flag, as we got an update */
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_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);
- }
+ /* 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);
}
/*
@@ -316,37 +325,42 @@ controls_tick() {
* Handle losing RC input
*/
- /* 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)) {
+ /* if we are in failsafe, clear the override flag */
+ if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
+ }
+
+ /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
+ if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
+ /* flag raw RC as lost */
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
+
/* Mark all channels as invalid, as we just lost the RX */
r_rc_valid = 0;
+ /* Set raw channel count to zero */
+ r_raw_rc_count = 0;
+
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
}
- /* this kicks in if the receiver is completely gone */
- if (rc_input_lost) {
-
- /* Set channel count to zero */
- r_raw_rc_count = 0;
- }
-
/*
* Check for manual override.
*
* The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
- * must have R/C input.
+ * must have R/C input (NO FAILSAFE!).
* Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
- (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+ !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
bool override = false;
@@ -369,10 +383,10 @@ controls_tick() {
mixer_tick();
} else {
- r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
} else {
- r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
}
@@ -395,8 +409,9 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
if (*num_values > PX4IO_RC_INPUT_CHANNELS)
*num_values = PX4IO_RC_INPUT_CHANNELS;
- for (unsigned i = 0; i < *num_values; i++)
+ for (unsigned i = 0; i < *num_values; i++) {
values[i] = ppm_buffer[i];
+ }
/* clear validity */
ppm_last_valid_decode = 0;
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 6a4429461..ebf4f3e8e 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -254,10 +254,25 @@ mixer_tick(void)
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
+ /* set S.BUS1 or S.BUS2 outputs */
+
+ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
+ sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
+ } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
+ sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+ }
+
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
+
+ /* set S.BUS1 or S.BUS2 outputs */
+ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT)
+ sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+
+ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
+ sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
}
}
@@ -267,7 +282,7 @@ mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control)
{
- if (control_group > 3)
+ if (control_group >= PX4IO_CONTROL_GROUPS)
return -1;
switch (source) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index d48c6c529..7471faec7 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -142,6 +142,7 @@
#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_FLAGS_RC_OK (1 << 4) /* RC reception 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) */
@@ -164,10 +165,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_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 */
@@ -189,6 +190,8 @@
#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 */
+#else
+#define PX4IO_P_SETUP_RELAYS_PAD 5
#endif
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */
@@ -201,26 +204,33 @@ enum { /* DSM bind states */
dsm_bind_send_pulses,
dsm_bind_reinit_uart
};
- /* 8 */
-#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
+ /* 8 */
+#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_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 */
+#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
+ /* storage space of 12 occupied by CRC */
+#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into
+ 'armed' (PWM enabled) state - this is a non-data write and
+ hence index 12 can safely be used. */
+#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
+
+#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
/* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
+#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 */
+#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
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index bb224f388..ca175bfbc 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -53,7 +53,7 @@
*/
#define PX4IO_SERVO_COUNT 8
#define PX4IO_CONTROL_CHANNELS 8
-#define PX4IO_CONTROL_GROUPS 2
+#define PX4IO_CONTROL_GROUPS 4
#define PX4IO_RC_INPUT_CHANNELS 18
#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */
@@ -108,6 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS]
#endif
+#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
#define r_control_values (&r_page_controls[0])
@@ -218,6 +219,8 @@ 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, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
+extern bool sbus1_output(uint16_t *values, uint16_t num_values);
+extern bool sbus2_output(uint16_t *values, uint16_t num_values);
/** 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 97d25bbfa..db1836f4a 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -160,6 +160,9 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
[PX4IO_P_SETUP_RELAYS] = 0,
+#else
+ /* this is unused, but we will pad it for readability (the compiler pads it automatically) */
+ [PX4IO_P_SETUP_RELAYS_PAD] = 0,
#endif
#ifdef ADC_VSERVO
[PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
@@ -169,6 +172,7 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_SET_DEBUG] = 0,
[PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
+ [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
};
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
@@ -462,9 +466,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
#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);
+ /* disable the conflicting options with SBUS 1 */
+ if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
+ value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+ PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+ PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+ }
+
+ /* disable the conflicting options with SBUS 2 */
+ if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+ value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+ PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+ PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
}
#endif
@@ -513,18 +526,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break;
case PX4IO_P_SETUP_PWM_DEFAULTRATE:
- if (value < 50)
+ if (value < 50) {
value = 50;
- if (value > 400)
+ }
+ if (value > 400) {
value = 400;
+ }
pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
break;
case PX4IO_P_SETUP_PWM_ALTRATE:
- if (value < 50)
+ if (value < 50) {
value = 50;
- if (value > 400)
+ }
+ if (value > 400) {
value = 400;
+ }
pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
break;
@@ -556,8 +573,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
}
// check the magic value
- if (value != PX4IO_REBOOT_BL_MAGIC)
+ if (value != PX4IO_REBOOT_BL_MAGIC) {
break;
+ }
// we schedule a reboot rather than rebooting
// immediately to allow the IO board to ACK
@@ -569,6 +587,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
dsm_bind(value & 0x0f, (value >> 4) & 0xF);
break;
+ case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
+ if (value == PX4IO_FORCE_SAFETY_MAGIC) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+ }
+ break;
+
+ case PX4IO_P_SETUP_RC_THR_FAILSAFE_US:
+ if (value > 650 && value < 2350) {
+ r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value;
+ }
+ break;
+
default:
return -1;
}
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index f6ec542eb..0e7dc621c 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -93,7 +93,7 @@ int
sbus_init(const char *device)
{
if (sbus_fd < 0)
- sbus_fd = open(device, O_RDONLY | O_NONBLOCK);
+ sbus_fd = open(device, O_RDWR | O_NONBLOCK);
if (sbus_fd >= 0) {
struct termios t;
@@ -113,11 +113,22 @@ sbus_init(const char *device)
} else {
debug("S.Bus: open failed");
}
-
return sbus_fd;
}
bool
+sbus1_output(uint16_t *values, uint16_t num_values)
+{
+ write(sbus_fd, 'A', 1);
+}
+
+bool
+sbus2_output(uint16_t *values, uint16_t num_values)
+{
+ write(sbus_fd, 'B', 1);
+}
+
+bool
sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
{
ssize_t ret;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-07 13:29:38 +0000