aboutsummaryrefslogtreecommitdiff
path: root/src/modules/px4iofirmware
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r--src/modules/px4iofirmware/controls.c2
-rw-r--r--src/modules/px4iofirmware/dsm.c4
-rw-r--r--src/modules/px4iofirmware/mixer.cpp134
-rw-r--r--src/modules/px4iofirmware/protocol.h15
-rw-r--r--src/modules/px4iofirmware/px4io.h3
-rw-r--r--src/modules/px4iofirmware/registers.c119
-rw-r--r--src/modules/px4iofirmware/safety.c10
7 files changed, 250 insertions, 37 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 74037d1c2..796c6cd9f 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -300,6 +300,8 @@ controls_tick() {
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
+ } else {
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
}
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 598bcee34..4925ced54 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -129,9 +129,9 @@ dsm_bind(uint16_t cmd, int pulses)
case dsm_bind_send_pulses:
for (int i = 0; i < pulses; i++) {
stm32_gpiowrite(usart1RxAsOutp, false);
- up_udelay(50);
+ up_udelay(25);
stm32_gpiowrite(usart1RxAsOutp, true);
- up_udelay(50);
+ up_udelay(25);
}
break;
case dsm_bind_reinit_uart:
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index d8c0e58ba..38727c68c 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -59,6 +59,12 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
+/*
+ * Time that the ESCs need to initialize
+ */
+ #define ESC_INIT_TIME_US 1000000
+ #define ESC_RAMP_TIME_US 2000000
+
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
@@ -68,6 +74,17 @@ extern "C" {
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
+static bool should_arm = false;
+static bool should_always_enable_pwm = false;
+static uint64_t esc_init_time;
+
+enum esc_state_e {
+ ESC_OFF,
+ ESC_INIT,
+ ESC_RAMP,
+ ESC_ON
+};
+static esc_state_e esc_state;
/* selected control values and count for mixing */
enum mixer_source {
@@ -98,7 +115,7 @@ mixer_tick(void)
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
- r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
} else {
@@ -112,12 +129,11 @@ mixer_tick(void)
* Decide which set of controls we're using.
*/
- /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+ /* do not mix if RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
- !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
- /* don't actually mix anything - we already have raw PWM values or
- not a valid mixer. */
+ /* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
} else {
@@ -167,6 +183,48 @@ mixer_tick(void)
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
+ uint16_t ramp_promille;
+
+ /* update esc init state, but only if we are truely armed and not just PWM enabled */
+ if (mixer_servos_armed && should_arm) {
+
+ switch (esc_state) {
+
+ /* after arming, some ESCs need an initalization period, count the time from here */
+ case ESC_OFF:
+ esc_init_time = hrt_absolute_time();
+ esc_state = ESC_INIT;
+ break;
+
+ /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
+ case ESC_INIT:
+ if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
+ esc_state = ESC_RAMP;
+ }
+ break;
+
+ /* then ramp until the min speed is reached */
+ case ESC_RAMP:
+ if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
+ esc_state = ESC_ON;
+ }
+ break;
+
+ case ESC_ON:
+ default:
+
+ break;
+ }
+ } else {
+ esc_state = ESC_OFF;
+ }
+
+ /* do the calculations during the ramp for all at once */
+ if(esc_state == ESC_RAMP) {
+ ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
+ }
+
+
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
@@ -176,9 +234,27 @@ mixer_tick(void)
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
- /* scale to servo output */
- r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
-
+ switch (esc_state) {
+ case ESC_INIT:
+ r_page_servos[i] = (outputs[i] * 600 + 1500);
+ break;
+
+ case ESC_RAMP:
+ r_page_servos[i] = (outputs[i]
+ * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+ + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
+ break;
+
+ case ESC_ON:
+ r_page_servos[i] = (outputs[i]
+ * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+ + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
+ break;
+
+ case ESC_OFF:
+ default:
+ break;
+ }
}
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
@@ -193,30 +269,46 @@ mixer_tick(void)
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
- bool should_arm = (
- /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
- /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
- /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
- /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
- /* FMU is available or FMU is not available but override is an option */
- ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+ should_arm = (
+ /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+ /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+ /* and FMU is armed */ && (
+ ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+ /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+ /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+ /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+ )
);
- if (should_arm && !mixer_servos_armed) {
+ should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+ && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+ && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+ if ((should_arm || should_always_enable_pwm) && !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 && mixer_servos_armed) {
+ } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
+ isr_debug(5, "> PWM disabled");
+
}
- if (mixer_servos_armed) {
+ if (mixer_servos_armed && should_arm) {
/* update the servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
+
+ } else if (mixer_servos_armed && should_always_enable_pwm) {
+ /* set the idle servo outputs. */
+ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
+ up_pwm_servo_set(i, r_page_servo_idle[i]);
}
}
@@ -265,9 +357,8 @@ static unsigned mixer_text_length = 0;
void
mixer_handle_text(const void *buffer, size_t length)
{
- /* do not allow a mixer change while fully armed */
- if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
- /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ /* do not allow a mixer change while outputs armed */
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return;
}
@@ -344,6 +435,7 @@ mixer_set_failsafe()
* Check if a custom failsafe value has been written,
* or if the mixer is not ok and bail out.
*/
+
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
return;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 3c59a75a7..02df76068 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -94,7 +94,7 @@
#define PX4IO_P_STATUS_CPULOAD 1
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
-#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
@@ -106,7 +106,8 @@
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
#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_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */
+#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 */
@@ -157,6 +158,7 @@
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */
#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_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
@@ -209,6 +211,15 @@ enum { /* DSM bind states */
#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 */
+
+/* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM idle values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
/**
* As-needed mixer data upload.
*
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 9eb092a63..d6cf22b21 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -78,6 +78,9 @@ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
+extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
+extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */
/*
* Register aliases.
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 3f241d29c..3ff9307cd 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -158,7 +158,9 @@ volatile uint16_t r_page_setup[] =
#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_IO_ARM_OK) | \
+ PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
+ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -196,6 +198,30 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI
*/
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
+/**
+ * PAGE 106
+ *
+ * minimum PWM values when armed
+ *
+ */
+uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+/**
+ * PAGE 107
+ *
+ * maximum PWM values when armed
+ *
+ */
+uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
+
+/**
+ * PAGE 108
+ *
+ * idle PWM values for difficult ESCs
+ *
+ */
+uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
@@ -259,6 +285,75 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
}
break;
+ case PX4IO_PAGE_CONTROL_MIN_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0)
+ /* set to default */
+ r_page_servo_control_min[offset] = 900;
+
+ else if (*values > 1200)
+ r_page_servo_control_min[offset] = 1200;
+ else if (*values < 900)
+ r_page_servo_control_min[offset] = 900;
+ else
+ r_page_servo_control_min[offset] = *values;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
+ case PX4IO_PAGE_CONTROL_MAX_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0)
+ /* set to default */
+ r_page_servo_control_max[offset] = 2100;
+
+ else if (*values > 2100)
+ r_page_servo_control_max[offset] = 2100;
+ else if (*values < 1800)
+ r_page_servo_control_max[offset] = 1800;
+ else
+ r_page_servo_control_max[offset] = *values;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
+ case PX4IO_PAGE_IDLE_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0)
+ /* set to default */
+ r_page_servo_idle[offset] = 0;
+
+ else if (*values < 900)
+ r_page_servo_idle[offset] = 900;
+ else if (*values > 2100)
+ r_page_servo_idle[offset] = 2100;
+ else
+ r_page_servo_idle[offset] = *values;
+
+ /* flag the failsafe values as custom */
+ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
@@ -331,9 +426,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* so that an in-air reset of FMU can not lead to a
* lockup of the IO arming state.
*/
- 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;
- }
+
+ // 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;
+ // }
r_setup_arming = value;
@@ -397,9 +494,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
- /* do not allow a RC config change while fully armed */
- if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
- /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ /* do not allow a RC config change while outputs armed */
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
break;
}
@@ -659,6 +755,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
+ case PX4IO_PAGE_CONTROL_MIN_PWM:
+ SELECT_PAGE(r_page_servo_control_min);
+ break;
+ case PX4IO_PAGE_CONTROL_MAX_PWM:
+ SELECT_PAGE(r_page_servo_control_max);
+ break;
+ case PX4IO_PAGE_IDLE_PWM:
+ SELECT_PAGE(r_page_servo_idle);
+ break;
default:
return -1;
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 4dbecc274..95335f038 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -110,7 +110,7 @@ safety_check_button(void *arg)
* state machine, keep ARM_COUNTER_THRESHOLD the same
* length in all cases of the if/else struct below.
*/
- if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+ if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +118,18 @@ safety_check_button(void *arg)
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* switch to armed state */
- r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
counter++;
}
- } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
- r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
counter++;
}
@@ -140,7 +140,7 @@ safety_check_button(void *arg)
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
- if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+ if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_IO_FMU_ARMED;
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-03 18:28:13 +0000