aboutsummaryrefslogtreecommitdiff
path: root/src/modules/px4iofirmware
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r--src/modules/px4iofirmware/controls.c205
-rw-r--r--src/modules/px4iofirmware/i2c.c5
-rw-r--r--src/modules/px4iofirmware/mixer.cpp28
-rw-r--r--src/modules/px4iofirmware/protocol.h40
-rw-r--r--src/modules/px4iofirmware/px4io.c5
-rw-r--r--src/modules/px4iofirmware/px4io.h3
-rw-r--r--src/modules/px4iofirmware/registers.c51
-rw-r--r--src/modules/px4iofirmware/sbus.c18
8 files changed, 203 insertions, 152 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index fffc6b347..f86ed3a6e 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -47,8 +47,8 @@
#include "sbus.h"
#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */
-#define RC_CHANNEL_HIGH_THRESH 5000
-#define RC_CHANNEL_LOW_THRESH -5000
+#define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */
+#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
@@ -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;
/*
@@ -208,94 +206,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);
}
/*
@@ -323,37 +332,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;
@@ -376,10 +390,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);
}
}
@@ -402,8 +416,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/i2c.c b/src/modules/px4iofirmware/i2c.c
index 79b6546b3..6d1d1fc2d 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -64,12 +64,15 @@
#define rCCR REG(STM32_I2C_CCR_OFFSET)
#define rTRISE REG(STM32_I2C_TRISE_OFFSET)
+void i2c_reset(void);
static int i2c_interrupt(int irq, void *context);
static void i2c_rx_setup(void);
static void i2c_tx_setup(void);
static void i2c_rx_complete(void);
static void i2c_tx_complete(void);
+#ifdef DEBUG
static void i2c_dump(void);
+#endif
static DMA_HANDLE rx_dma;
static DMA_HANDLE tx_dma;
@@ -331,6 +334,7 @@ i2c_tx_complete(void)
i2c_tx_setup();
}
+#ifdef DEBUG
static void
i2c_dump(void)
{
@@ -339,3 +343,4 @@ i2c_dump(void)
debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE);
debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2);
}
+#endif
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index f6a54c5ed..8f47a9efb 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -224,6 +224,7 @@ mixer_tick(void)
actuator_count = mixed;
in_mixer = false;
+ /* the pwm limit call takes care of out of band errors */
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
@@ -265,14 +266,12 @@ mixer_tick(void)
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
- /* update S.BUS1 outputs */
- if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
- sbus1_output(r_page_servos, actuator_count);
- }
+ /* set S.BUS1 or S.BUS2 outputs */
- /* update S.BUS2 outputs */
- if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
- sbus2_output(r_page_servos, actuator_count);
+ 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) {
@@ -280,15 +279,12 @@ mixer_tick(void)
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
- /* update S.BUS1 outputs */
- if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
- sbus1_output(r_page_servos, actuator_count);
- }
+ /* 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);
- /* update S.BUS2 outputs */
- if (r_page_setup[PX4IO_P_SETUP_FEATURES] & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
- sbus2_output(r_page_servos, actuator_count);
- }
+ if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
+ sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
}
}
@@ -298,7 +294,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..d5a6b1ec4 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
@@ -302,7 +312,7 @@ struct IOPacket {
#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)))
+#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))))
static const uint8_t crc8_tab[256] __attribute__((unused)) =
{
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index d4c25911e..cd134ccb4 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -37,6 +37,7 @@
*/
#include <nuttx/config.h>
+#include <nuttx/arch.h>
#include <stdio.h> // required for task_create
#include <stdbool.h>
@@ -303,14 +304,12 @@ user_start(int argc, char *argv[])
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
- struct mallinfo minfo = mallinfo();
-
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)minfo.mxordblk);
+ (unsigned)mallinfo().mxordblk);
last_debug_time = hrt_absolute_time();
}
}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 39df81eed..2ae081d51 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -56,7 +56,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 */
@@ -111,6 +111,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])
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 97d25bbfa..b37259997 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -119,7 +119,6 @@ uint16_t r_page_raw_rc_input[] =
[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
};
@@ -160,6 +159,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 +171,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 +465,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 +525,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 +572,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 +586,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;
}
@@ -640,7 +669,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
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) {
+ } else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
count++;
}
@@ -682,7 +711,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
{
#define SELECT_PAGE(_page_name) \
do { \
- *values = &_page_name[0]; \
+ *values = (uint16_t *)&_page_name[0]; \
*num_values = sizeof(_page_name) / sizeof(_page_name[0]); \
} while(0)
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 33ef515be..925e24f65 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -109,7 +109,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;
@@ -138,21 +138,15 @@ sbus_init(const char *device)
void
sbus1_output(uint16_t *values, uint16_t num_values)
{
- /*
- * S.BUS2 outputs are defined as:
- *
- */
- #warning SBUS1 output is not yet implemented
+ char a = 'A';
+ write(sbus_fd, &a, 1);
}
void
sbus2_output(uint16_t *values, uint16_t num_values)
{
- /*
- * S.BUS2 outputs are defined as:
- *
- */
- #warning SBUS2 output is not yet implemented
+ char b = 'B';
+ write(sbus_fd, &b, 1);
}
bool
@@ -280,6 +274,7 @@ sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *v
case 0x03:
{
uint16_t rx_voltage = (frame[1] << 8) | frame[2];
+ isr_debug(30, "rx_voltage %d", (int)rx_voltage);
n_consumed = 3;
}
break;
@@ -308,6 +303,7 @@ sbus_parse(hrt_abstime now, uint8_t *frame, unsigned *partial_count, uint16_t *v
case 0x13:
{
uint16_t gps_something = (frame[1] << 8) | frame[2];
+ isr_debug(30, "gps_something %d", (int)gps_something);
n_consumed = 24;
}
break;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-22 00:54:43 +0000