diff options
author | NosDE <marco@wtns.de> | 2015-02-16 22:06:17 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-18 16:55:05 +0100 |
commit | 5fddb89c3e3290df26822c59c11fbfd4f7981f16 (patch) | |
tree | ebcef8ec5c827ce24660e26978d1bdc920d4fc08 /src/drivers/mkblctrl | |
parent | bd2d411a09a5d9747ed5ea3ef75173f44b33d997 (diff) | |
download | px4-firmware-5fddb89c3e3290df26822c59c11fbfd4f7981f16.tar.gz px4-firmware-5fddb89c3e3290df26822c59c11fbfd4f7981f16.tar.bz2 px4-firmware-5fddb89c3e3290df26822c59c11fbfd4f7981f16.zip |
mkblctrl - rework and bugfix
Diffstat (limited to 'src/drivers/mkblctrl')
-rw-r--r-- | src/drivers/mkblctrl/mkblctrl.cpp | 275 | ||||
-rw-r--r-- | src/drivers/mkblctrl/mkblctrl_params.c | 52 | ||||
-rw-r--r-- | src/drivers/mkblctrl/module.mk | 3 |
3 files changed, 192 insertions, 138 deletions
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1331744ae..f3db4dd94 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. * Author: Marco Bauer <marco@wtns.de> * * Redistribution and use in source and binary forms, with or without @@ -42,8 +42,8 @@ */ #include <nuttx/config.h> - #include <drivers/device/i2c.h> +#include <systemlib/param/param.h> #include <sys/types.h> #include <stdint.h> @@ -67,11 +67,12 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> #include <drivers/drv_rc_input.h> +#include <drivers/drv_mixer.h> +#include <drivers/drv_tone_alarm.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> #include <systemlib/mixer/mixer.h> -#include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_0.h> @@ -81,17 +82,18 @@ #include <systemlib/err.h> -#define I2C_BUS_SPEED 400000 -#define UPDATE_RATE 400 +#define I2C_BUS_SPEED 100000 +#define UPDATE_RATE 200 #define MAX_MOTORS 8 #define BLCTRL_BASE_ADDR 0x29 #define BLCTRL_OLD 0 #define BLCTRL_NEW 1 #define BLCTRL_MIN_VALUE -0.920F #define MOTOR_STATE_PRESENT_MASK 0x80 -#define MOTOR_STATE_ERROR_MASK 0x7F +#define MOTOR_STATE_ERROR_MASK 0x7F #define MOTOR_SPINUP_COUNTER 30 -#define ESC_UORB_PUBLISH_DELAY 500000 +#define MOTOR_LOCATE_DELAY 10000000 +#define ESC_UORB_PUBLISH_DELAY 500000 struct MotorData_t { unsigned int Version; // the version of the BL (0 = old) @@ -108,6 +110,9 @@ struct MotorData_t { unsigned int RoundCount; }; + + + class MK : public device::I2C { public: @@ -128,7 +133,6 @@ public: virtual int init(unsigned motors); virtual ssize_t write(file *filp, const char *buffer, size_t len); - int set_update_rate(unsigned rate); int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); @@ -141,7 +145,6 @@ private: static const bool showDebug = false; int _update_rate; - int _current_update_rate; int _task; int _t_actuators; int _t_actuator_armed; @@ -179,20 +182,17 @@ private: int pwm_ioctl(file *filp, int cmd, unsigned long arg); - struct GPIOConfig { - uint32_t input; - uint32_t output; - uint32_t alt; - }; - - static const GPIOConfig _gpio_tab[]; - static const unsigned _ngpio; int mk_servo_arm(bool status); int mk_servo_set(unsigned int chan, short val); - int mk_servo_set_value(unsigned int chan, short val); int mk_servo_test(unsigned int chan); + int mk_servo_locate(); short scaling(float val, float inMin, float inMax, float outMin, float outMax); + + void play_beep(int count); + bool _indicate_esc; + param_t _param_indicate_esc; + }; @@ -218,8 +218,8 @@ MK *g_mk; } // namespace MK::MK(int bus, const char *_device_path) : - I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), - _update_rate(400), + I2C("mkblctrl", "/dev/mkblctrl0", bus, 0, I2C_BUS_SPEED), + _update_rate(UPDATE_RATE), _task(-1), _t_actuators(-1), _t_actuator_armed(-1), @@ -234,13 +234,15 @@ MK::MK(int bus, const char *_device_path) : _overrideSecurityChecks(false), _task_should_exit(false), _armed(false), - _mixers(nullptr) + _mixers(nullptr), + _indicate_esc(false) { strncpy(_device, _device_path, sizeof(_device)); /* enforce null termination */ _device[sizeof(_device) - 1] = '\0'; _debug_enabled = true; + } MK::~MK() @@ -274,6 +276,8 @@ MK::~MK() int MK::init(unsigned motors) { + _param_indicate_esc = param_find("MKBLCTRL_TEST"); + _num_outputs = motors; debugCounter = 0; int ret; @@ -321,16 +325,6 @@ MK::task_main_trampoline(int argc, char *argv[]) g_mk->task_main(); } -int -MK::set_update_rate(unsigned rate) -{ - if ((rate > 500) || (rate < 10)) - return -EINVAL; - - _update_rate = rate; - return OK; -} - void MK::set_px4mode(int px4mode) { @@ -439,16 +433,28 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) } void +MK::play_beep(int count) +{ + int buzzer = ::open(TONEALARM0_DEVICE_PATH, O_WRONLY); + + for (int i = 0; i < count; i++) { + ::ioctl(buzzer, TONE_SET_ALARM, TONE_SINGLE_BEEP_TUNE); + usleep(300000); + } + + ::close(buzzer); +} + +void MK::task_main() { + int32_t param_mkblctrl_test = 0; /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. */ - _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - - /* force a reset of the update rate */ - _current_update_rate = 0; + _t_actuators = orb_subscribe(ORB_ID(actuator_controls_0)); + orb_set_interval(_t_actuators, 20); /* default to 50Hz */ _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ @@ -467,37 +473,26 @@ MK::task_main() _t_esc_status = orb_advertise(ORB_ID(esc_status), &esc); - pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; + + orb_set_interval(_t_actuators, int(1000 / _update_rate)); + up_pwm_servo_set_rate(_update_rate); + log("starting"); /* loop until killed */ while (!_task_should_exit) { - /* 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; - _update_rate = 500; - } - - /* reject slower than 50 Hz updates */ - if (update_rate_in_ms > 20) { - 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; + param_get(_param_indicate_esc ,¶m_mkblctrl_test); + if (param_mkblctrl_test > 0) { + _indicate_esc = true; + } else { + _indicate_esc = false; } /* sleep waiting for data max 100ms */ @@ -513,66 +508,69 @@ MK::task_main() /* do we have a control update? */ if (fds[0].revents & POLLIN) { - /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + bool changed = false; + orb_check(_t_actuators, &changed); - /* can we mix? */ - if (_mixers != nullptr) { + if (changed) { - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); - outputs.timestamp = hrt_absolute_time(); + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - /* iterate actuators */ - for (unsigned int i = 0; i < _num_outputs; i++) { + /* can we mix? */ + if (_mixers != nullptr) { - /* 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 */ - /* nothing to do here */ - } 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. - */ - if (outputs.output[i] < -1.0f) { - outputs.output[i] = -1.0f; + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); + outputs.timestamp = hrt_absolute_time(); - } else if (outputs.output[i] > 1.0f) { - outputs.output[i] = 1.0f; + /* iterate actuators */ + for (unsigned int i = 0; i < _num_outputs; 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 */ + /* nothing to do here */ } else { - outputs.output[i] = -1.0f; + /* + * 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. + */ + if (outputs.output[i] < -1.0f) { + outputs.output[i] = -1.0f; + + } else if (outputs.output[i] > 1.0f) { + outputs.output[i] = 1.0f; + + } else { + outputs.output[i] = -1.0f; + } } - } - if (!_overrideSecurityChecks) { - /* don't go under BLCTRL_MIN_VALUE */ + if (!_overrideSecurityChecks) { + /* don't go under BLCTRL_MIN_VALUE */ + + if (outputs.output[i] < BLCTRL_MIN_VALUE) { + outputs.output[i] = BLCTRL_MIN_VALUE; + } - if (outputs.output[i] < BLCTRL_MIN_VALUE) { - outputs.output[i] = BLCTRL_MIN_VALUE; } - } + /* output to BLCtrl's */ + if (_motortest != true && _indicate_esc != true) { + Motor[i].SetPoint_PX4 = outputs.output[i]; + mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine + } - /* output to BLCtrl's */ - if (_motortest != true) { - //mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine - // 11 Bit - Motor[i].SetPoint_PX4 = outputs.output[i]; - mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine } } - } - } /* how about an arming update? */ @@ -624,6 +622,11 @@ MK::task_main() if (_motortest == true) { mk_servo_test(i); } + + // if esc locate is requested + if (_indicate_esc == true) { + mk_servo_locate(); + } } @@ -834,39 +837,6 @@ MK::mk_servo_set(unsigned int chan, short val) return 0; } -int -MK::mk_servo_set_value(unsigned int chan, short val) -{ - _retries = 0; - int ret; - short tmpVal = 0; - uint8_t msg[2] = { 0, 0 }; - - tmpVal = val; - - if (tmpVal > 1024) { - tmpVal = 1024; - - } else if (tmpVal < 0) { - tmpVal = 0; - } - - Motor[chan].SetPoint = (uint8_t)(tmpVal / 4); - - if (_armed == false) { - Motor[chan].SetPoint = 0; - Motor[chan].SetPointLowerBits = 0; - } - - msg[0] = Motor[chan].SetPoint; - - set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); - ret = transfer(&msg[0], 1, nullptr, 0); - - ret = OK; - return ret; -} - int MK::mk_servo_test(unsigned int chan) @@ -936,6 +906,39 @@ MK::mk_servo_test(unsigned int chan) int +MK::mk_servo_locate() +{ + int ret = 0; + static unsigned int chan = 0; + static uint64_t last_timestamp = 0; + _retries = 0; + uint8_t msg[2] = { 0, 0 }; + + + if (hrt_absolute_time() - last_timestamp > MOTOR_LOCATE_DELAY) { + last_timestamp = hrt_absolute_time(); + + set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + chan++; + + if (chan <= _num_outputs) { + fprintf(stderr, "[mkblctrl] ESC Locate - #%i:\tgreen\n", chan); + play_beep(chan); + } + + if (chan > _num_outputs) { + chan = 0; + } + } + + // do i2c transfer to selected esc + ret = transfer(&msg[0], 1, nullptr, 0); + + return ret; +} + + +int MK::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, @@ -1130,7 +1133,7 @@ enum FrameType { int -mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) +mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) { int shouldStop = 0; @@ -1160,8 +1163,6 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false)); - g_mk->set_update_rate(update_rate); - return OK; } @@ -1178,7 +1179,7 @@ mk_start(unsigned motors, const char *device_path) if (OK == g_mk->init(motors)) { warnx("[mkblctrl] scanning i2c3...\n"); - ret = g_mk->mk_check_for_blctrl(8, false, true); + ret = g_mk->mk_check_for_blctrl(8, false, false); if (ret > 0) { return OK; @@ -1196,7 +1197,7 @@ mk_start(unsigned motors, const char *device_path) if (OK == g_mk->init(motors)) { warnx("[mkblctrl] scanning i2c1...\n"); - ret = g_mk->mk_check_for_blctrl(8, false, true); + ret = g_mk->mk_check_for_blctrl(8, false, false); if (ret > 0) { return OK; @@ -1217,14 +1218,13 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); int mkblctrl_main(int argc, char *argv[]) { - int pwm_update_rate_in_hz = UPDATE_RATE; int motorcount = 8; int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default bool motortest = false; bool overrideSecurityChecks = false; bool showHelp = false; - bool newMode = false; + bool newMode = true; const char *devicepath = ""; /* @@ -1265,6 +1265,7 @@ mkblctrl_main(int argc, char *argv[]) /* look for the optional -h --help parameter */ if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { showHelp = true; + newMode = false; } /* look for the optional --override-security-checks parameter */ @@ -1311,7 +1312,7 @@ mkblctrl_main(int argc, char *argv[]) /* parameter set ? */ if (newMode) { /* switch parameter */ - return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + return mk_new_mode(motorcount, motortest, px4mode, frametype, overrideSecurityChecks); } exit(0); diff --git a/src/drivers/mkblctrl/mkblctrl_params.c b/src/drivers/mkblctrl/mkblctrl_params.c new file mode 100644 index 000000000..4296260f8 --- /dev/null +++ b/src/drivers/mkblctrl/mkblctrl_params.c @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Author: Marco Bauer <marco@wtns.de> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mkblctrl_params.c + * + * Parameters defined by the mkblctrl driver. + * + * @author Marco Bauer <marco@wtns.de> + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +/** + * Enables testmode (Identify) of MKBLCTRL Driver + * + * @group MKBLCTRL Testmode + */ +PARAM_DEFINE_INT32(MKBLCTRL_TEST, 0); + diff --git a/src/drivers/mkblctrl/module.mk b/src/drivers/mkblctrl/module.mk index 6daa14aa5..10468509e 100644 --- a/src/drivers/mkblctrl/module.mk +++ b/src/drivers/mkblctrl/module.mk @@ -37,7 +37,8 @@ MODULE_COMMAND = mkblctrl -SRCS = mkblctrl.cpp +SRCS = mkblctrl.cpp \ + mkblctrl_params.c INCLUDE_DIRS += $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common |