aboutsummaryrefslogtreecommitdiff
path: root/src/modules/px4iofirmware
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r--src/modules/px4iofirmware/controls.c18
-rw-r--r--src/modules/px4iofirmware/protocol.h1
-rw-r--r--src/modules/px4iofirmware/px4io.c63
-rw-r--r--src/modules/px4iofirmware/px4io.h2
-rw-r--r--src/modules/px4iofirmware/registers.c3
-rw-r--r--src/modules/px4iofirmware/sbus.c40
6 files changed, 120 insertions, 7 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index ad60ee03e..58c9429b6 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -41,6 +41,7 @@
#include <stdbool.h>
#include <drivers/drv_hrt.h>
+#include <drivers/drv_rc_input.h>
#include <systemlib/perf_counter.h>
#include <systemlib/ppm_decode.h>
#include <rc/st24.h>
@@ -70,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
uint8_t *bytes;
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
if (*dsm_updated) {
- r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
@@ -91,6 +91,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
+ st24_rssi = RC_INPUT_RSSI_MAX;
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
}
@@ -170,6 +171,12 @@ controls_tick() {
perf_begin(c_gather_dsm);
bool dsm_updated, st24_updated;
(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
+ if (dsm_updated) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+ }
+ if (st24_updated) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
+ }
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
@@ -417,6 +424,15 @@ controls_tick() {
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
override = true;
+ /*
+ if the FMU is dead then enable override if we have a
+ mixer and OVERRIDE_IMMEDIATE is set
+ */
+ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+ (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
+ override = true;
+
if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 9b2e047cb..c7e9ae3eb 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -183,6 +183,7 @@
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
+#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
#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.c b/src/modules/px4iofirmware/px4io.c
index 30f32b38e..14ee9cb40 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in;
#define NUM_MSG 2
static char msg[NUM_MSG][40];
+static void heartbeat_blink(void);
+static void ring_blink(void);
+
/*
* add a debug message to be printed on the console
*/
@@ -124,8 +127,64 @@ heartbeat_blink(void)
{
static bool heartbeat = false;
LED_BLUE(heartbeat = !heartbeat);
+}
+
+static void
+ring_blink(void)
+{
#ifdef GPIO_LED4
- LED_RING(heartbeat);
+
+ if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+ /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+ LED_RING(1);
+ return;
+ }
+
+ // XXX this led code does have
+ // intentionally a few magic numbers.
+ const unsigned max_brightness = 118;
+
+ static unsigned counter = 0;
+ static unsigned brightness = max_brightness;
+ static unsigned brightness_counter = 0;
+ static unsigned on_counter = 0;
+
+ if (brightness_counter < max_brightness) {
+
+ bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1);
+
+ // XXX once led is PWM driven,
+ // remove the ! in the line below
+ // to return to the proper breathe
+ // animation / pattern (currently inverted)
+ LED_RING(!on);
+ brightness_counter++;
+
+ if (on) {
+ on_counter++;
+ }
+
+ } else {
+
+ if (counter >= 62) {
+ counter = 0;
+ }
+
+ int n;
+
+ if (counter < 32) {
+ n = counter;
+
+ } else {
+ n = 62 - counter;
+ }
+
+ brightness = (n * n) / 8;
+ brightness_counter = 0;
+ on_counter = 0;
+ counter++;
+ }
+
#endif
}
@@ -300,6 +359,8 @@ user_start(int argc, char *argv[])
heartbeat_blink();
}
+ ring_blink();
+
check_reboot();
/* check for debug activity (default: none) */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 8186e4c78..93a33490f 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -140,7 +140,7 @@ extern pwm_limit_t pwm_limit;
#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
-#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s))
+#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, (_s))
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 49c2a9f56..fbfdd35db 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -191,7 +191,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
- PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
+ PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
+ PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 6ead38d61..d76ec55f0 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -57,6 +57,7 @@
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
+#define SBUS1_FRAME_DELAY 14000
/*
Measured values with Futaba FX-30/R6108SB:
@@ -80,6 +81,7 @@ static int sbus_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
+static hrt_abstime last_txframe_time = 0;
static uint8_t frame[SBUS_FRAME_SIZE];
@@ -122,10 +124,42 @@ sbus_init(const char *device)
void
sbus1_output(uint16_t *values, uint16_t num_values)
{
- char a = 'A';
- write(sbus_fd, &a, 1);
-}
+ uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
+ uint8_t offset = 0;
+ uint16_t value;
+ hrt_abstime now;
+
+ now = hrt_absolute_time();
+
+ if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
+ last_txframe_time = now;
+ uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f };
+
+ /* 16 is sbus number of servos/channels minus 2 single bit channels.
+ * currently ignoring single bit channels. */
+
+ for (unsigned i = 0; (i < num_values) && (i < 16); ++i) {
+ value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
+
+ /*protect from out of bounds values and limit to 11 bits*/
+ if (value > 0x07ff ) {
+ value = 0x07ff;
+ }
+
+ while (offset >= 8) {
+ ++byteindex;
+ offset -= 8;
+ }
+ oframe[byteindex] |= (value << (offset)) & 0xff;
+ oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff;
+ oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff;
+ offset += 11;
+ }
+
+ write(sbus_fd, oframe, SBUS_FRAME_SIZE);
+ }
+}
void
sbus2_output(uint16_t *values, uint16_t num_values)
{
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-01 06:52:01 +0000