diff options
author | marco <marco@Marcos-MacBook-Pro.local> | 2013-04-29 18:32:30 +0200 |
---|---|---|
committer | marco <marco@Marcos-MacBook-Pro.local> | 2013-04-29 18:32:30 +0200 |
commit | ca5dcc11a7754d38e3b5f63bcb708e106fb2a3cd (patch) | |
tree | 9a39b9e2e9f0dff14b037d4fbe41bee31b65b6c2 | |
parent | d9f9ecb084e862bd72528d118c570533deb6eccd (diff) | |
download | px4-firmware-ca5dcc11a7754d38e3b5f63bcb708e106fb2a3cd.tar.gz px4-firmware-ca5dcc11a7754d38e3b5f63bcb708e106fb2a3cd.tar.bz2 px4-firmware-ca5dcc11a7754d38e3b5f63bcb708e106fb2a3cd.zip |
BLCtrl 2.0 testing - currently only 8 Bit resolution - motor detection and px4 mode as default
-rw-r--r-- | apps/drivers/mkblctrl/mkblctrl.cpp | 83 |
1 files changed, 55 insertions, 28 deletions
diff --git a/apps/drivers/mkblctrl/mkblctrl.cpp b/apps/drivers/mkblctrl/mkblctrl.cpp index 664271bee..bcdb80b5d 100644 --- a/apps/drivers/mkblctrl/mkblctrl.cpp +++ b/apps/drivers/mkblctrl/mkblctrl.cpp @@ -121,6 +121,7 @@ public: int set_motor_test(bool motortest); int set_px4mode(int px4mode); int set_frametype(int frametype); + unsigned int mk_check_for_blctrl(unsigned int count, unsigned int showOutput); private: static const unsigned _max_actuators = MAX_MOTORS; @@ -176,7 +177,6 @@ private: int gpio_ioctl(file *filp, int cmd, unsigned long arg); int mk_servo_arm(bool status); - int mk_check_for_blctrl(unsigned int count); int mk_servo_set(unsigned int chan, float val); int mk_servo_set_test(unsigned int chan, float val); int mk_servo_test(unsigned int chan); @@ -350,11 +350,11 @@ MK::set_mode(Mode mode) switch (mode) { case MODE_2PWM: if(_num_outputs == 4) { - debug("MODE_QUAD"); + //debug("MODE_QUAD"); } else if(_num_outputs == 6) { - debug("MODE_HEXA"); + //debug("MODE_HEXA"); } else if(_num_outputs == 8) { - debug("MODE_OCTO"); + //debug("MODE_OCTO"); } //up_pwm_servo_init(0x3); up_pwm_servo_deinit(); @@ -363,11 +363,11 @@ MK::set_mode(Mode mode) case MODE_4PWM: if(_num_outputs == 4) { - debug("MODE_QUADRO"); + //debug("MODE_QUADRO"); } else if(_num_outputs == 6) { - debug("MODE_HEXA"); + //debug("MODE_HEXA"); } else if(_num_outputs == 8) { - debug("MODE_OCTO"); + //debug("MODE_OCTO"); } //up_pwm_servo_init(0xf); up_pwm_servo_deinit(); @@ -447,8 +447,13 @@ MK::set_motor_count(unsigned count) memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4)); } - /* check for connected blctrls */ - mk_check_for_blctrl(_num_outputs); + if(_num_outputs == 4) { + fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n"); + } else if(_num_outputs == 6) { + fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n"); + } else if(_num_outputs == 8) { + fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n"); + } return OK; } @@ -502,7 +507,6 @@ MK::task_main() memset(&rc_in, 0, sizeof(rc_in)); rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; - log("starting"); long update_rate_in_us = 0; @@ -666,10 +670,10 @@ MK::mk_servo_arm(bool status) } -int -MK::mk_check_for_blctrl(unsigned int count) +unsigned int +MK::mk_check_for_blctrl(unsigned int count, unsigned int showOutput) { - _retries = 10; + _retries = 50; uint8_t foundMotorCount = 0; for(unsigned i=0; i<MAX_MOTORS; i++) { @@ -708,16 +712,21 @@ MK::mk_check_for_blctrl(unsigned int count) } } - fprintf(stderr, "[mkblctrl] MotorsRequiered: %i\tMotorsFound: %i\n", count,foundMotorCount); - for(unsigned i=0; i< count; i++) { - fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i,Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); + if(showOutput == 1) { + fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount); + for(unsigned i=0; i< count; i++) { + fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i,Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); + } } + return foundMotorCount; + /* if(foundMotorCount == count) { return true; } else { return false; } + */ } @@ -1230,6 +1239,7 @@ int mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype) { uint32_t gpio_bits; + int shouldStop = 0; MK::Mode servo_mode; /* reset to all-inputs */ @@ -1282,14 +1292,28 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, /* set frametype (geometry) */ g_mk->set_frametype(frametype); + /* motortest if enabled */ + g_mk->set_motor_test(motortest); + + /* (re)set count of used motors */ - g_mk->set_motor_count(motorcount); + ////g_mk->set_motor_count(motorcount); + /* count used motors */ + + do { + if(g_mk->mk_check_for_blctrl(8, 0) != 0) { + shouldStop = 3; + } else { + shouldStop++; + } + sleep(1); + } while ( shouldStop != 3); + + g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, 1)); /* (re)set the PWM output mode */ g_mk->set_mode(servo_mode); - /* motortest if enabled */ - g_mk->set_motor_test(motortest); if ((servo_mode != MK::MODE_NONE) && (update_rate != 0)) g_mk->set_pwm_rate(update_rate); @@ -1335,15 +1359,19 @@ mkblctrl_main(int argc, char *argv[]) int motorcount = 0; int bus = 1; bool motortest = false; - int px4mode = MAPPING_MK; + int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default + new_mode = PORT_FULL_PWM; + motorcount = 8; + /* * Mode switches. * * XXX use getopt? */ for (int i = 1; i < argc; i++) { /* argv[0] is "mk" */ + /* if (!strcmp(argv[i], "mode_quad")) { new_mode = PORT_FULL_PWM; motorcount = 4; @@ -1354,8 +1382,10 @@ mkblctrl_main(int argc, char *argv[]) new_mode = PORT_FULL_PWM; motorcount = 8; } + */ /* look for the optional update rate for the supported modes */ + /* if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { if (argc > i + 1) { pwm_update_rate_in_hz = atoi(argv[i + 1]); @@ -1365,6 +1395,7 @@ mkblctrl_main(int argc, char *argv[]) } } + */ /* look for the optional i2c bus parameter */ if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { if (argc > i + 1) { @@ -1376,9 +1407,10 @@ mkblctrl_main(int argc, char *argv[]) } /* look for the optional frame parameter */ - if (strcmp(argv[i], "-f") == 0 || strcmp(argv[i], "--frame") == 0) { + if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) { if (argc > i + 1) { if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) { + px4mode = MAPPING_MK; if(strcmp(argv[i + 1], "+") == 0) { frametype = FRAME_PLUS; } else { @@ -1388,16 +1420,11 @@ mkblctrl_main(int argc, char *argv[]) errx(1, "only + or x for frametype supported !"); } } else { - errx(1, "missing argument for frametype (-f)"); + errx(1, "missing argument for mkmode (-mkmode)"); return 1; } } - /* look for the optional geometry parameter */ - if (strcmp(argv[i], "-px4mode") == 0) { - px4mode = MAPPING_PX4; - } - /* look for the optional test parameter */ if (strcmp(argv[i], "-t") == 0) { motortest = true; @@ -1407,7 +1434,7 @@ mkblctrl_main(int argc, char *argv[]) if(new_mode == PORT_MODE_UNSET) { fprintf(stderr, "mkblctrl: unrecognised command, try:\n"); - fprintf(stderr, " mode_quad, mode_hexa, mode_octo [-b i2c_bus_number] [-u update_rate_in_hz] [-t motortest] [-px4mode] [-f frame{+/x}]\n"); + fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest]\n"); exit(1); } |