diff options
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 52 |
1 files changed, 45 insertions, 7 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'"); } |