diff options
Diffstat (limited to 'apps/drivers/px4fmu/fmu.cpp')
-rw-r--r-- | apps/drivers/px4fmu/fmu.cpp | 108 |
1 files changed, 75 insertions, 33 deletions
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index dac0b5e84..a672bd2fb 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -58,8 +58,10 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> #include <drivers/boards/px4fmu/px4fmu_internal.h> +#include <drivers/drv_hrt.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> #include <systemlib/mixer/mixer.h> #include <drivers/drv_mixer.h> @@ -112,9 +114,9 @@ private: void task_main() __attribute__((noreturn)); static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); int pwm_ioctl(file *filp, int cmd, unsigned long arg); @@ -180,6 +182,7 @@ PX4FMU::~PX4FMU() _task_should_exit = true; unsigned i = 10; + do { /* wait 50ms - it should wake every 100ms or so worst-case */ usleep(50000); @@ -215,6 +218,7 @@ PX4FMU::init() /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == OK) { log("default PWM output device"); _primary_pwm_device = true; @@ -248,7 +252,7 @@ PX4FMU::task_main_trampoline(int argc, char *argv[]) int PX4FMU::set_mode(Mode mode) { - /* + /* * Configure for PWM output. * * Note that regardless of the configured mode, the task is always @@ -282,6 +286,7 @@ PX4FMU::set_mode(Mode mode) default: return -EINVAL; } + _mode = mode; return OK; } @@ -341,6 +346,7 @@ PX4FMU::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); + /* reject faster than 500 Hz updates */ if (update_rate_in_ms < 2) { update_rate_in_ms = 2; @@ -351,6 +357,7 @@ PX4FMU::task_main() update_rate_in_ms = 20; _update_rate = 50; } + orb_set_interval(_t_actuators, update_rate_in_ms); up_pwm_servo_set_rate(_update_rate); _current_update_rate = _update_rate; @@ -376,7 +383,8 @@ PX4FMU::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); // XXX output actual limited values memcpy(&controls_effective, &_controls, sizeof(controls_effective)); @@ -386,8 +394,21 @@ PX4FMU::task_main() /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } /* output to the servo */ up_pwm_servo_set(i, outputs.output[i]); @@ -428,9 +449,9 @@ PX4FMU::task_main() int PX4FMU::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -448,15 +469,17 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); + if (ret != -ENOTTY) return ret; /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch(_mode) { + switch (_mode) { case MODE_2PWM: case MODE_4PWM: ret = pwm_ioctl(filp, cmd, arg); break; + default: debug("not in a PWM mode"); break; @@ -574,8 +597,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) { ret = -ENOMEM; + } else { debug("loading mixers from %s", path); @@ -606,7 +631,7 @@ void PX4FMU::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs, GPIO driver chip + * Setup default GPIO config - all pins as GPIOs, GPIO driver chip * to input mode. */ for (unsigned i = 0; i < _ngpio; i++) @@ -633,17 +658,20 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) /* configure selected GPIOs as required */ for (unsigned i = 0; i < _ngpio; i++) { - if (gpios & (1<<i)) { + if (gpios & (1 << i)) { switch (function) { case GPIO_SET_INPUT: stm32_configgpio(_gpio_tab[i].input); break; + case GPIO_SET_OUTPUT: stm32_configgpio(_gpio_tab[i].output); break; + case GPIO_SET_ALT_1: if (_gpio_tab[i].alt != 0) stm32_configgpio(_gpio_tab[i].alt); + break; } } @@ -660,7 +688,7 @@ PX4FMU::gpio_write(uint32_t gpios, int function) int value = (function == GPIO_SET) ? 1 : 0; for (unsigned i = 0; i < _ngpio; i++) - if (gpios & (1<<i)) + if (gpios & (1 << i)) stm32_gpiowrite(_gpio_tab[i].output, value); } @@ -684,7 +712,7 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) lock(); switch (cmd) { - + case GPIO_RESET: gpio_reset(); break; @@ -786,6 +814,7 @@ fmu_new_mode(PortMode new_mode, int update_rate) /* (re)set the PWM output mode */ g_fmu->set_mode(servo_mode); + if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0)) g_fmu->set_pwm_rate(update_rate); @@ -824,13 +853,18 @@ test(void) fd = open(PWM_OUTPUT_DEVICE_PATH, 0); - if (fd < 0) { - puts("open fail"); - exit(1); - } + if (fd < 0) + errx(1, "open fail"); + + if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); + + if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); - ioctl(fd, PWM_SERVO_ARM, 0); - ioctl(fd, PWM_SERVO_SET(0), 1000); + if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); close(fd); @@ -840,10 +874,8 @@ test(void) void fake(int argc, char *argv[]) { - if (argc < 5) { - puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)"); - exit(1); - } + if (argc < 5) + errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)"); actuator_controls_s ac; @@ -857,10 +889,18 @@ fake(int argc, char *argv[]) orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - if (handle < 0) { - puts("advertise failed"); - exit(1); - } + if (handle < 0) + errx(1, "advertise failed"); + + actuator_armed_s aa; + + aa.armed = true; + aa.lockdown = false; + + handle = orb_advertise(ORB_ID(actuator_armed), &aa); + + if (handle < 0) + errx(1, "advertise failed 2"); exit(0); } @@ -914,15 +954,17 @@ fmu_main(int argc, char *argv[]) if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) { if (argc > i + 1) { pwm_update_rate_in_hz = atoi(argv[i + 1]); + } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + errx(1, "missing argument for pwm update rate (-u)"); return 1; } + } else { - fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); + errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio"); } } - } + } /* was a new mode set? */ if (new_mode != PORT_MODE_UNSET) { @@ -939,5 +981,5 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command, try:\n"); fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); - return -EINVAL; + exit(1); } |