From cc452834c0dabd2689f5f102ce1cbbe714f056dd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Jun 2013 00:30:10 +0200 Subject: First try to prevent motors from stopping when armed --- src/modules/px4iofirmware/px4io.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules/px4iofirmware/px4io.h') diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf..3e4027c9b 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -77,6 +77,8 @@ 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 */ /* * Register aliases. -- cgit v1.2.3 From 23858a6726f151cc6d67ecda0d42c7374839d80f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 22:59:40 +0200 Subject: Added functionality to enable PWM output for stupid ESCs even when safety is not off, arming button functionality remains as is --- src/drivers/px4io/px4io.cpp | 71 +++++++++++++++++-- src/modules/px4iofirmware/mixer.cpp | 125 ++++++++++++++++++++++++---------- src/modules/px4iofirmware/protocol.h | 6 +- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 39 ++++++++++- 5 files changed, 197 insertions(+), 45 deletions(-) (limited to 'src/modules/px4iofirmware/px4io.h') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bce193bca..0ea90beb4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -136,6 +136,11 @@ public: */ int set_max_values(const uint16_t *vals, unsigned len); + /** + * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE + */ + int set_idle_values(const uint16_t *vals, unsigned len); + /** * Print the current status of IO */ @@ -567,7 +572,8 @@ PX4IO::init() io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); /* publish RC config to IO */ ret = io_set_rc_config(); @@ -793,6 +799,20 @@ PX4IO::set_max_values(const uint16_t *vals, unsigned len) return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); } +int +PX4IO::set_idle_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + printf("Sending IDLE values\n"); + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len); +} int @@ -1441,12 +1461,14 @@ PX4IO::print_status() /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); + ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), @@ -1473,8 +1495,10 @@ PX4IO::print_status() } printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - printf("\n"); + printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("idle values"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u\n", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); } int @@ -1973,6 +1997,41 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "idle")) { + + if (argc < 3) { + errx(1, "max command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 0. */ + uint16_t idle[8]; + + for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) + { + /* set channel to commanline argument or to 0 for non-provided channels */ + if (argc > i + 2) { + idle[i] = atoi(argv[i+2]); + if (idle[i] < 900 || idle[i] > 2100) { + errx(1, "value out of range of 900 < value < 2100. Aborting."); + } + } else { + /* a zero value will the default */ + idle[i] = 0; + } + } + + int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0])); + + if (ret != OK) + errx(ret, "failed setting idle values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { @@ -2100,5 +2159,5 @@ px4io_main(int argc, char *argv[]) monitor(); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'min, 'max', 'idle' or 'update'"); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6752a8128..1f118ea2f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -63,6 +63,7 @@ extern "C" { * 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 @@ -73,10 +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; -static bool esc_init_active = false; -static bool esc_init_done = false; + +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 { @@ -175,18 +183,48 @@ mixer_tick(void) float outputs[IO_SERVO_COUNT]; unsigned mixed; - /* after arming, some ESCs need an initalization period, count the time from here */ - if (mixer_servos_armed && !esc_init_done && !esc_init_active) { - esc_init_time = hrt_absolute_time(); - esc_init_active = true; + 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; } - /* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */ - if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) { - esc_init_active = false; - esc_init_done = true; + /* 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], IO_SERVO_COUNT); @@ -196,21 +234,27 @@ mixer_tick(void) /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - /* XXX maybe this check for an armed FMU could be achieved a little less messy */ - if (source == MIX_FMU && !(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) { - r_page_servos[i] = r_page_servo_failsafe[i]; - } - /* during ESC initialization, use low PWM */ - else if (esc_init_active) { - r_page_servos[i] = (outputs[i] * 600 + 1500); - - /* afterwards use min and max values */ - } else { - 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); + 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 < IO_SERVO_COUNT; i++) r_page_servos[i] = 0; @@ -225,36 +269,43 @@ mixer_tick(void) * XXX correct behaviour for failsafe may require an additional case * here. */ - bool should_arm = ( + should_arm = ( /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - /* and either FMU is armed */ ( ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* and there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))) || - - /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) + /* 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 direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || + /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) ) ); + 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 && !mixer_servos_armed) { + 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, "> 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, "> disarmed"); + isr_debug(5, "> PWM disabled"); - esc_init_active = false; } + } - if (mixer_servos_armed) { + if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ for (unsigned i = 0; i < IO_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 < IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servo_idle[i]); } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b43054197..0e477a200 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -155,6 +155,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 */ @@ -190,9 +191,12 @@ /* PWM minimum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ - /* PWM maximum values for certain ESCs */ +/* 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 3e4027c9b..042e7fe66 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -79,6 +79,7 @@ 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 379cf09e3..bd13f3b7d 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -147,7 +147,8 @@ volatile uint16_t r_page_setup[] = 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_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE #define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -201,6 +202,14 @@ uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, */ uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; +/** + * PAGE 108 + * + * idle PWM values for difficult ESCs + * + */ +uint16_t r_page_servo_idle[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -308,6 +317,31 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_IDLE_PWM: + + /* copy channel data */ + while ((offset < IO_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)); @@ -651,6 +685,9 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val 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; -- cgit v1.2.3