From eb76d116cc67c6354c29fa41e49b4cf9df1a472d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:30:42 +0200 Subject: Minor state machine improvements and fixes for IO safety / in-air restart handling --- src/modules/px4iofirmware/protocol.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src/modules/px4iofirmware/protocol.h') diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd..497e6af8e 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -77,7 +77,7 @@ /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ @@ -93,7 +93,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 */ @@ -105,6 +105,7 @@ #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_SAFETY_OFF (1 << 12) /* safety is off */ #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) /* VBatt is very close to regulator dropout */ -- cgit v1.2.3 From c3a8f177b6316a9cefd814e312742f47d3049739 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:58:17 +0200 Subject: Software version check fixes --- src/drivers/px4io/px4io.cpp | 16 +++++++++++++--- src/modules/px4iofirmware/protocol.h | 7 +++++-- src/modules/px4iofirmware/registers.c | 4 ++-- 3 files changed, 20 insertions(+), 7 deletions(-) (limited to 'src/modules/px4iofirmware/protocol.h') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f033382a6..925041e0c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -404,13 +404,23 @@ PX4IO::init() unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION); - if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION) { + if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC) { mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n", proto_version, - PX4IO_P_CONFIG_PROTOCOL_VERSION); + PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC); mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); - log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION); + log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC); + return 1; + } + + if (sw_version != PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! SOFTWARE VER MISMATCH: v%u vs v%u\n", + proto_version, + PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC); + + mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); + log("software version mismatch (v%u on IO vs v%u on FMU)", sw_version, PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC); return 1; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 497e6af8e..6c6c7b4e2 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -75,10 +75,13 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC 2 +#define PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC 2 + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers */ +#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a092fc93b..148514455 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -57,8 +57,8 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC, + [PX4IO_P_CONFIG_SOFTWARE_VERSION] = PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC, [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, -- cgit v1.2.3 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/drivers/px4io/px4io.cpp | 108 ++++++++++++++++++++++++++++++++++ src/modules/px4iofirmware/mixer.cpp | 20 ++++++- src/modules/px4iofirmware/protocol.h | 6 ++ src/modules/px4iofirmware/px4io.h | 2 + src/modules/px4iofirmware/registers.c | 58 ++++++++++++++++++ 5 files changed, 192 insertions(+), 2 deletions(-) (limited to 'src/modules/px4iofirmware/protocol.h') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d728b7b76..6a596a987 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -125,6 +125,16 @@ public: */ int set_failsafe_values(const uint16_t *vals, unsigned len); + /** + * Set the minimum PWM signals when armed + */ + int set_min_values(const uint16_t *vals, unsigned len); + + /** + * Set the maximum PWM signal when armed + */ + int set_max_values(const uint16_t *vals, unsigned len); + /** * Print the current status of IO */ @@ -711,6 +721,34 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len); } +int +PX4IO::set_min_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len); +} + +int +PX4IO::set_max_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); +} + + + int PX4IO::io_set_arming_state() { @@ -1792,6 +1830,76 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "min")) { + + if (argc < 3) { + errx(1, "min command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 900. */ + uint16_t min[8]; + + for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++) + { + /* set channel to commanline argument or to 900 for non-provided channels */ + if (argc > i + 2) { + min[i] = atoi(argv[i+2]); + if (min[i] < 900 || min[i] > 1200) { + errx(1, "value out of range of 900 < value < 1200. Aborting."); + } + } else { + /* a zero value will the default */ + min[i] = 900; + } + } + + int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0])); + + if (ret != OK) + errx(ret, "failed setting min values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + + if (!strcmp(argv[1], "max")) { + + 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 2100. */ + uint16_t max[8]; + + for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++) + { + /* set channel to commanline argument or to 2100 for non-provided channels */ + if (argc > i + 2) { + max[i] = atoi(argv[i+2]); + if (max[i] < 1800 || max[i] > 2100) { + errx(1, "value out of range of 1800 < value < 2100. Aborting."); + } + } else { + /* a zero value will the default */ + max[i] = 2100; + } + } + + int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0])); + + if (ret != OK) + errx(ret, "failed setting max values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526..e257e7cb8 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -59,6 +59,11 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 200000 +/* + * Time that the ESCs need to initialize + */ + #define ESC_INIT_TIME_US 500000 + /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 #define PITCH 1 @@ -69,6 +74,8 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; +static uint64_t time_armed; + /* selected control values and count for mixing */ enum mixer_source { MIX_NONE, @@ -176,8 +183,16 @@ 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; + /* scale to control range after init time */ + if (mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + 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); + + /* but use init range from 900 to 2100 right after arming */ + } else { + r_page_servos[i] = (outputs[i] * 600 + 1500); + } } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) @@ -206,6 +221,7 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + time_armed = hrt_absolute_time(); } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd..1c9715451 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,12 @@ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* 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 */ + /** * As-needed mixer data upload. * 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. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd3..1fcfb2906 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -184,6 +184,22 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; +/** + * PAGE 106 + * + * minimum PWM values when armed + * + */ +uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + +/** + * PAGE 107 + * + * maximum PWM values when armed + * + */ +uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -247,6 +263,42 @@ 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 < IO_SERVO_COUNT) && (num_values > 0)) { + + 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 < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values > 1200) + r_page_servo_control_max[offset] = 1200; + else if (*values < 900) + r_page_servo_control_max[offset] = 900; + else + r_page_servo_control_max[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -583,6 +635,12 @@ 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; default: return -1; -- 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/protocol.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