From 21cc19cef6a6ad9d88ac20cf2223635fe8ec4388 Mon Sep 17 00:00:00 2001 From: marco Date: Mon, 18 Nov 2013 21:32:41 +0100 Subject: mkblctrl set a default device / -d (device) parameter for alternate device added / -t testmode enhanced --- .../init.d/rc.custom_dji_f330_mkblctrl | 4 +- src/drivers/mkblctrl/mkblctrl.cpp | 73 +++++++++++++++------- 2 files changed, 52 insertions(+), 25 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index 8e0914d63..40b2ee68b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -97,9 +97,9 @@ fi # if [ $MKBLCTRL_FRAME == x ] then - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix else - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix fi # diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index b93f38cf6..4e26d9c50 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. + * Author: Marco Bauer * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +36,8 @@ * @file mkblctrl.cpp * * Driver/configurator for the Mikrokopter BL-Ctrl. - * Marco Bauer + * + * @author Marco Bauer * */ @@ -89,8 +91,8 @@ #define BLCTRL_MIN_VALUE -0.920F #define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_ERROR_MASK 0x7F -#define MOTOR_SPINUP_COUNTER 2500 -#define ESC_UORB_PUBLISH_DELAY 200 +#define MOTOR_SPINUP_COUNTER 30 +#define ESC_UORB_PUBLISH_DELAY 200 class MK : public device::I2C { @@ -112,7 +114,7 @@ public: FRAME_X, }; - MK(int bus); + MK(int bus, const char *_device_path); ~MK(); virtual int ioctl(file *filp, int cmd, unsigned long arg); @@ -141,6 +143,7 @@ private: unsigned int _motor; int _px4mode; int _frametype; + char _device[20]; ///< device orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; @@ -244,7 +247,7 @@ MK *g_mk; } // namespace -MK::MK(int bus) : +MK::MK(int bus, const char *_device_path) : I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), _mode(MODE_NONE), _update_rate(50), @@ -265,6 +268,10 @@ MK::MK(int bus) : _armed(false), _mixers(nullptr) { + strncpy(_device, _device_path, sizeof(_device)); + /* enforce null termination */ + _device[sizeof(_device) - 1] = '\0'; + _debug_enabled = true; } @@ -291,7 +298,7 @@ MK::~MK() /* clean up the alternate device node */ if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); + unregister_driver(_device); g_mk = nullptr; } @@ -313,13 +320,15 @@ MK::init(unsigned motors) usleep(500000); - /* 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 (sizeof(_device) > 0) { + ret = register_driver(_device, &fops, 0666, (void *)this); - if (ret == OK) { - log("default PWM output device"); - _primary_pwm_device = true; - } + if (ret == OK) { + log("creating alternate output device"); + _primary_pwm_device = true; + } + + } /* reset GPIOs */ gpio_reset(); @@ -633,10 +642,7 @@ MK::task_main() } /* output to BLCtrl's */ - if (_motortest == true) { - mk_servo_test(i); - - } else { + 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]; @@ -692,9 +698,16 @@ MK::task_main() esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; esc.esc[i].esc_state = (uint16_t) Motor[i].State; esc.esc[i].esc_errorcount = (uint16_t) 0; + + // if motortest is requested - do it... + if (_motortest == true) { + mk_servo_test(i); + } + } orb_publish(ORB_ID(esc_status), _t_esc_status, &esc); + } } @@ -1390,13 +1403,13 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, } int -mk_start(unsigned bus, unsigned motors) +mk_start(unsigned bus, unsigned motors, char *device_path) { int ret = OK; if (g_mk == nullptr) { - g_mk = new MK(bus); + g_mk = new MK(bus, device_path); if (g_mk == nullptr) { ret = -ENOMEM; @@ -1432,6 +1445,7 @@ mkblctrl_main(int argc, char *argv[]) bool overrideSecurityChecks = false; bool showHelp = false; bool newMode = false; + char *devicepath = ""; /* * optional parameters @@ -1491,25 +1505,38 @@ mkblctrl_main(int argc, char *argv[]) newMode = true; } + /* look for the optional device parameter */ + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { + if (argc > i + 1) { + devicepath = argv[i + 1]; + newMode = true; + + } else { + errx(1, "missing the devicename (-d)"); + return 1; + } + } + } if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); - fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n"); + fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-d devicename] [-t] [--override-security-checks] [-h / --help]\n\n"); fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n"); - fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); + fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); + fprintf(stderr, "\t -t \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); exit(1); } if (g_mk == nullptr) { - if (mk_start(bus, motorcount) != OK) { + if (mk_start(bus, motorcount, devicepath) != OK) { errx(1, "failed to start the MK-BLCtrl driver"); } else { - newMode = true; + //////newMode = true; } } @@ -1522,5 +1549,5 @@ mkblctrl_main(int argc, char *argv[]) /* test, etc. here g*/ - exit(1); + exit(0); } -- cgit v1.2.3 From f82a202667c8b4e38d229e6fcac70ca40380aea2 Mon Sep 17 00:00:00 2001 From: marco Date: Tue, 19 Nov 2013 17:35:04 +0100 Subject: actuator effective removed - unused --- src/drivers/mkblctrl/mkblctrl.cpp | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 4e26d9c50..9442ae906 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -75,7 +75,6 @@ #include #include -#include #include #include #include @@ -146,7 +145,6 @@ private: char _device[20]; ///< device orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; orb_advert_t _t_esc_status; unsigned int _num_outputs; @@ -255,7 +253,6 @@ MK::MK(int bus, const char *_device_path) : _t_actuators(-1), _t_actuator_armed(-1), _t_outputs(0), - _t_actuators_effective(0), _t_esc_status(0), _num_outputs(0), _motortest(false), @@ -534,13 +531,6 @@ MK::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - /* advertise the blctrl status */ esc_status_s esc; memset(&esc, 0, sizeof(esc)); @@ -604,9 +594,6 @@ MK::task_main() 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)); - /* iterate actuators */ for (unsigned int i = 0; i < _num_outputs; i++) { @@ -712,9 +699,8 @@ MK::task_main() } - //::close(_t_esc_status); + ::close(_t_esc_status); ::close(_t_actuators); - ::close(_t_actuators_effective); ::close(_t_actuator_armed); -- cgit v1.2.3 From cc8e85ce7e4e79a217edd40f64f6870b0ab72bb3 Mon Sep 17 00:00:00 2001 From: marco Date: Thu, 21 Nov 2013 22:24:16 +0100 Subject: mkblctrl scans now i2c3 and i2c1 bir connected esc's --- src/drivers/mkblctrl/mkblctrl.cpp | 80 +++++++++++++++++++++++++++++++++------ 1 file changed, 69 insertions(+), 11 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 9442ae906..bd058e5d0 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -127,7 +127,7 @@ public: int set_overrideSecurityChecks(bool overrideSecurityChecks); int set_px4mode(int px4mode); int set_frametype(int frametype); - unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput); + unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: static const unsigned _max_actuators = MAX_MOTORS; @@ -726,8 +726,12 @@ MK::mk_servo_arm(bool status) unsigned int -MK::mk_check_for_blctrl(unsigned int count, bool showOutput) +MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) { + if(initI2C) { + I2C::init(); + } + _retries = 50; uint8_t foundMotorCount = 0; @@ -1366,7 +1370,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, /* count used motors */ do { - if (g_mk->mk_check_for_blctrl(8, false) != 0) { + if (g_mk->mk_check_for_blctrl(8, false, false) != 0) { shouldStop = 4; } else { @@ -1376,7 +1380,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, sleep(1); } while (shouldStop < 3); - g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true)); + g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false)); /* (re)set the PWM output mode */ g_mk->set_mode(servo_mode); @@ -1414,6 +1418,52 @@ mk_start(unsigned bus, unsigned motors, char *device_path) } +int +mk_check_for_i2c_esc_bus(char *device_path, int motors) +{ + int ret; + + if (g_mk == nullptr) { + + g_mk = new MK(3, device_path); + + if (g_mk == nullptr) { + return -1; + + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + + if (ret > 0) { + return 3; + } + + } + + + g_mk = new MK(1, device_path); + + if (g_mk == nullptr) { + return -1; + + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + + if (ret > 0) { + return 1; + } + + } + } + + return -1; +} + + + } // namespace extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); @@ -1424,7 +1474,7 @@ mkblctrl_main(int argc, char *argv[]) PortMode port_mode = PORT_FULL_PWM; int pwm_update_rate_in_hz = UPDATE_RATE; int motorcount = 8; - int bus = 1; + int bus = -1; int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default bool motortest = false; @@ -1517,15 +1567,23 @@ mkblctrl_main(int argc, char *argv[]) } - if (g_mk == nullptr) { - if (mk_start(bus, motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); + if (bus != -1) { + bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); + } - } else { - //////newMode = true; + if (bus != -1) { + if (g_mk == nullptr) { + if (mk_start(bus, motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + + } else { + //////newMode = true; + } } - } + } else { + errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); + } /* parameter set ? */ if (newMode) { -- cgit v1.2.3 From d2e32f2fc56d723b566e8e935f86379cec4fee39 Mon Sep 17 00:00:00 2001 From: marco Date: Fri, 22 Nov 2013 21:05:40 +0100 Subject: mkblctrl - hotfix for i2c scan --- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index bd058e5d0..0a41bfef4 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1567,7 +1567,7 @@ mkblctrl_main(int argc, char *argv[]) } - if (bus != -1) { + if (bus == -1) { bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); } -- cgit v1.2.3 From 4e713a70835ee041f10d2f34745c9de81973c984 Mon Sep 17 00:00:00 2001 From: marco Date: Tue, 26 Nov 2013 19:01:43 +0100 Subject: motortest mode enhanced --- src/drivers/mkblctrl/mkblctrl.cpp | 79 +++++++++++++++++++++++---------------- 1 file changed, 46 insertions(+), 33 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 0a41bfef4..30d6069b3 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -91,7 +91,7 @@ #define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_ERROR_MASK 0x7F #define MOTOR_SPINUP_COUNTER 30 -#define ESC_UORB_PUBLISH_DELAY 200 +#define ESC_UORB_PUBLISH_DELAY 500000 class MK : public device::I2C { @@ -661,7 +661,7 @@ MK::task_main() * Only update esc topic every half second. */ - if (hrt_absolute_time() - esc.timestamp > 500000) { + if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) { esc.counter++; esc.timestamp = hrt_absolute_time(); esc.esc_count = (uint8_t) _num_outputs; @@ -955,6 +955,7 @@ MK::mk_servo_test(unsigned int chan) if (_motor >= _num_outputs) { _motor = -1; _motortest = false; + fprintf(stderr, "[mkblctrl] Motortest finished...\n"); } } @@ -1557,41 +1558,53 @@ mkblctrl_main(int argc, char *argv[]) if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); - fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-d devicename] [-t] [--override-security-checks] [-h / --help]\n\n"); - fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); - fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n"); + fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n"); + fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); + fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n"); fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); - fprintf(stderr, "\t -t \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); + fprintf(stderr, "\n"); + fprintf(stderr, "Motortest:\n"); + fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n"); + fprintf(stderr, "mkblctrl -t\n"); + fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n"); exit(1); } - if (bus == -1) { - bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); - } - - if (bus != -1) { - if (g_mk == nullptr) { - if (mk_start(bus, motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); - - } else { - //////newMode = true; - } - } - } else { - errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); - - } - - /* parameter set ? */ - if (newMode) { - /* switch parameter */ - return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); - } - - /* test, etc. here g*/ - - exit(0); + if (!motortest) { + if (g_mk == nullptr) { + if (bus == -1) { + bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); + } + + if (bus != -1) { + if (mk_start(bus, motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + } + } else { + errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); + } + + /* parameter set ? */ + if (newMode) { + /* switch parameter */ + return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + } + + exit(0); + } else { + errx(1, "MK-BLCtrl driver already running"); + } + + } else { + if (g_mk == nullptr) { + errx(1, "MK-BLCtrl driver not running. You have to start it first."); + + } else { + g_mk->set_motor_test(motortest); + exit(0); + + } + } } -- cgit v1.2.3