diff options
author | marco <marco@marcos-mbp.bauer.loc> | 2013-11-21 22:24:16 +0100 |
---|---|---|
committer | marco <marco@marcos-mbp.bauer.loc> | 2013-11-21 22:24:16 +0100 |
commit | cc8e85ce7e4e79a217edd40f64f6870b0ab72bb3 (patch) | |
tree | 800cd9c70456b14497e4b50552862d56b703e385 /src | |
parent | f82a202667c8b4e38d229e6fcac70ca40380aea2 (diff) | |
download | px4-firmware-cc8e85ce7e4e79a217edd40f64f6870b0ab72bb3.tar.gz px4-firmware-cc8e85ce7e4e79a217edd40f64f6870b0ab72bb3.tar.bz2 px4-firmware-cc8e85ce7e4e79a217edd40f64f6870b0ab72bb3.zip |
mkblctrl scans now i2c3 and i2c1 bir connected esc's
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/mkblctrl/mkblctrl.cpp | 80 |
1 files 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) { |