From 400842a0d875ea480a5ad9b538e6a85122a2d46c Mon Sep 17 00:00:00 2001 From: luft27 Date: Mon, 3 Aug 2015 15:24:13 +0300 Subject: [PATCH 1/2] fixed: fmu does not publish actuator_outputs --- src/drivers/px4fmu/fmu.cpp | 124 +++++++++++++++++++++++-------------- 1 file changed, 78 insertions(+), 46 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bf0d5e8d6f..b4ad660539 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -90,6 +90,7 @@ */ #define CONTROL_INPUT_DROP_LIMIT_MS 20 +#define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev { @@ -136,7 +137,6 @@ private: int _armed_sub; int _param_sub; orb_advert_t _outputs_pub; - actuator_armed_s _armed; unsigned _num_outputs; int _class_instance; @@ -151,11 +151,11 @@ private: int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; - int _actuator_output_topic_instance; pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; - pwm_limit_t _pwm_limit; + static pwm_limit_t _pwm_limit; + static actuator_armed_s _armed; uint16_t _failsafe_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; @@ -164,6 +164,8 @@ private: unsigned _num_failsafe_set; unsigned _num_disarmed_set; + static bool arm_nothrottle() { return (_armed.ready_to_arm && !_armed.armed); } + static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -175,6 +177,7 @@ private: int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); void update_pwm_rev_mask(); + void publish_pwm_outputs(uint16_t *values, size_t numvalues); struct GPIOConfig { uint32_t input; @@ -240,7 +243,9 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { #endif }; -const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +pwm_limit_t PX4FMU::_pwm_limit; +actuator_armed_s PX4FMU::_armed = {}; namespace { @@ -260,7 +265,6 @@ PX4FMU::PX4FMU() : _armed_sub(-1), _param_sub(-1), _outputs_pub(nullptr), - _armed{}, _num_outputs(0), _class_instance(0), _task_should_exit(false), @@ -270,9 +274,7 @@ PX4FMU::PX4FMU() : _groups_required(0), _groups_subscribed(0), _control_subs{-1}, - _actuator_output_topic_instance(-1), _poll_fds_num(0), - _pwm_limit{}, _failsafe_pwm{0}, _disarmed_pwm{0}, _reverse_pwm_mask(0), @@ -570,6 +572,25 @@ PX4FMU::update_pwm_rev_mask() } } +void +PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues) { + actuator_outputs_s outputs; + outputs.noutputs = numvalues; + outputs.timestamp = hrt_absolute_time(); + + for (size_t i = 0; i < _max_actuators; ++i) { + outputs.output[i] = i < numvalues ? (float)values[i] : 0; + } + + if (_outputs_pub == nullptr) { + int instance = -1; + _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT); + } else { + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); + } +} + + void PX4FMU::task_main() { @@ -579,10 +600,6 @@ PX4FMU::task_main() _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _param_sub = orb_subscribe(ORB_ID(parameter_update)); - /* advertise the mixed control outputs */ - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); - #ifdef HRT_PPM_CHANNEL // rc input, published to ORB struct rc_input_values rc_in; @@ -668,7 +685,7 @@ PX4FMU::task_main() /* can we mix? */ if (_mixers != nullptr) { - unsigned num_outputs; + size_t num_outputs; switch (_mode) { case MODE_2PWM: @@ -692,40 +709,27 @@ PX4FMU::task_main() } /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN and INF */ - if ((i >= outputs.noutputs) || - !isfinite(outputs.output[i])) { - /* - * 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] = -1.0f; + float outputs[_max_actuators]; + num_outputs = _mixers->mix(outputs, num_outputs, NULL); + + /* disable unused ports by setting their output to NaN */ + for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) { + if (i >= num_outputs) { + outputs[i] = NAN_VALUE; } } - uint16_t pwm_limited[num_outputs]; + uint16_t pwm_limited[_max_actuators]; - /* the PWM limit call takes care of out of band errors and constrains */ - pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + /* the PWM limit call takes care of out of band errors, NaN and constrains */ + pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); /* output to the servos */ - for (unsigned i = 0; i < num_outputs; i++) { + for (size_t i = 0; i < num_outputs; i++) { up_pwm_servo_set(i, pwm_limited[i]); } - /* publish mixed control outputs */ - if (_outputs_pub != nullptr) { - _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT); - } else { - - orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); - } + publish_pwm_outputs(pwm_limited, num_outputs); } } @@ -737,13 +741,14 @@ PX4FMU::task_main() orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = _armed.armed && !_armed.lockdown; + bool set_armed = (_armed.armed || _armed.ready_to_arm) && !_armed.lockdown; - if (_servo_armed != set_armed) + if (_servo_armed != set_armed) { _servo_armed = set_armed; + } /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + bool pwm_on = (set_armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; @@ -827,6 +832,34 @@ PX4FMU::control_callback(uintptr_t handle, const actuator_controls_s *controls = (actuator_controls_s *)handle; input = controls[control_group].control[control_index]; + + /* limit control input */ + if (input > 1.0f) { + input = 1.0f; + } else if (input < -1.0f) { + input = -1.0f; + } + + /* motor spinup phase - lock throttle to zero */ + if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* limit the throttle output to zero during motor spinup, + * as the motors cannot follow any demand yet + */ + input = 0.0f; + } + } + + /* throttle not arming - mark throttle input as invalid */ + if (arm_nothrottle()) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* set the throttle to an invalid value */ + input = NAN_VALUE; + } + } + return 0; } @@ -835,14 +868,12 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - // XXX disabled, confusing users - //debug("ioctl 0x%04x 0x%08x", cmd, arg); - /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); - if (ret != -ENOTTY) + if (ret != -ENOTTY) { return ret; + } /* if we are in valid PWM mode, try it as a PWM ioctl as well */ switch (_mode) { @@ -861,8 +892,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) } /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) + if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); + } return ret; } From bcc7b3d110f3a8f5a5eddab97caa67148e1a0722 Mon Sep 17 00:00:00 2001 From: luft27 Date: Mon, 3 Aug 2015 15:25:20 +0300 Subject: [PATCH 2/2] add mavlink stream for outputing AUX PWM --- ROMFS/px4fmu_common/init.d/rc.usb | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 4c2119aa2f..50cb8a5b1e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,7 +3,7 @@ # USB MAVLink start # -mavlink start -r 80000 -d /dev/ttyACM0 -x +mavlink start -r 800000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 @@ -11,14 +11,15 @@ mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 mavlink stream -d /dev/ttyACM0 -s DISTANCE_SENSOR -r 10 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 -mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100 mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 -mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 +mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS -r 5 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 +mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_1 -r 20 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5 -mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 20 +mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100 mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20 # Exit shell to make it available to MAVLink