From 5fddb89c3e3290df26822c59c11fbfd4f7981f16 Mon Sep 17 00:00:00 2001 From: NosDE Date: Mon, 16 Feb 2015 22:06:17 +0100 Subject: mkblctrl - rework and bugfix --- src/drivers/mkblctrl/mkblctrl.cpp | 275 +++++++++++++++++++------------------- 1 file changed, 138 insertions(+), 137 deletions(-) (limited to 'src/drivers/mkblctrl/mkblctrl.cpp') 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 * * Redistribution and use in source and binary forms, with or without @@ -42,8 +42,8 @@ */ #include - #include +#include #include #include @@ -67,11 +67,12 @@ #include #include #include +#include +#include #include #include #include -#include #include #include @@ -81,17 +82,18 @@ #include -#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) { @@ -438,17 +432,29 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) return retVal; } +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) @@ -935,6 +905,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, @@ -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); -- cgit v1.2.3