diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-05-28 17:46:24 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-05-28 17:46:24 +0200 |
commit | 2876bc72f979b4596fbe97cfae71c0d04d4753b2 (patch) | |
tree | 022c6b0d23bcbfe649f1f69812ab0938e0fb969c /src | |
parent | f1a8f6e75b98768daa5bc5290ccf60156d8185da (diff) | |
download | px4-firmware-2876bc72f979b4596fbe97cfae71c0d04d4753b2.tar.gz px4-firmware-2876bc72f979b4596fbe97cfae71c0d04d4753b2.tar.bz2 px4-firmware-2876bc72f979b4596fbe97cfae71c0d04d4753b2.zip |
Slightly reworked IO internal failsafe, added command to activate it (px4io failsafe), does not parse commandline arguments yet
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 52 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 29 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 4 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 3 |
4 files changed, 73 insertions, 15 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8b2fae12b..f25d471aa 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -106,7 +106,7 @@ public: * @param rate The rate in Hz actuator outpus are sent to IO. * Min 10 Hz, max 400 Hz */ - int set_update_rate(int rate); + int set_update_rate(int rate); /** * Set the battery current scaling and bias @@ -114,7 +114,15 @@ public: * @param amp_per_volt * @param amp_bias */ - void set_battery_current_scaling(float amp_per_volt, float amp_bias); + void set_battery_current_scaling(float amp_per_volt, float amp_bias); + + /** + * Push failsafe values to IO. + * + * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) + * @param len Number of channels, could up to 8 + */ + int set_failsafe_values(const uint16_t *vals, unsigned len); /** * Print the current status of IO @@ -326,11 +334,11 @@ PX4IO::PX4IO() : _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _primary_pwm_device(false), _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0), - _primary_pwm_device(false) + _battery_last_timestamp(0) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -690,6 +698,21 @@ PX4IO::io_set_control_state() } int +PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + unsigned max = (len < _max_actuators) ? len : _max_actuators; + + /* set failsafe values */ + for (unsigned i = 0; i < max; i++) + regs[i] = FLOAT_TO_REG(vals[i]); + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, regs, max); +} + +int PX4IO::io_set_arming_state() { actuator_armed_s armed; ///< system armed state @@ -1250,7 +1273,7 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), @@ -1262,7 +1285,8 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), - ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); printf("alarms 0x%04x%s%s%s%s%s%s%s\n", alarms, @@ -1718,6 +1742,20 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "failsafe")) { + + /* XXX parse arguments here */ + + if (g_dev != nullptr) { + /* XXX testing values */ + uint16_t failsafe[4] = {1500, 1500, 1200, 1200}; + g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { @@ -1845,5 +1883,5 @@ px4io_main(int argc, char *argv[]) monitor(); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1234e2eea..448d2355f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -102,13 +102,16 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } + /* default to failsafe mixing */ source = MIX_FAILSAFE; /* * Decide which set of controls we're using. */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + + /* do not mix if mixer is invalid or 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)) { /* don't actually mix anything - we already have raw PWM values or not a valid mixer. */ @@ -117,6 +120,7 @@ mixer_tick(void) } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ @@ -133,14 +137,28 @@ mixer_tick(void) } /* + * Set failsafe status flag depending on mixing source + */ + if (source == MIX_FAILSAFE) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; + } else { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); + } + + /* * Run the mixers. */ if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; + /* safe actuators for FMU feedback */ + r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; + } + + } else if (source != MIX_NONE) { float outputs[IO_SERVO_COUNT]; @@ -156,7 +174,7 @@ mixer_tick(void) r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); /* scale to servo output */ - r_page_servos[i] = (outputs[i] * 500.0f) + 1500; + r_page_servos[i] = (outputs[i] * 600.0f) + 1500; } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) @@ -175,7 +193,7 @@ mixer_tick(void) 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 */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && + /* 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) )) @@ -225,7 +243,6 @@ mixer_callback(uintptr_t handle, case MIX_FAILSAFE: case MIX_NONE: - /* XXX we could allow for configuration of per-output failsafe values */ return -1; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e575bd841..674f9dddd 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -85,7 +85,7 @@ #define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ #define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ -#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ /* dynamic status page */ #define PX4IO_PAGE_STATUS 1 @@ -104,6 +104,7 @@ #define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ #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_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 */ @@ -148,6 +149,7 @@ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ #define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ #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_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9698a0f2f..3abf56a18 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -41,6 +41,7 @@ #include <stdbool.h> #include <stdlib.h> +#include <string.h> #include <drivers/drv_hrt.h> @@ -179,7 +180,7 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE * * Failsafe servo PWM values */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; +uint16_t r_page_servo_failsafe[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) |