fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 13:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

BLCtrl 2.0 testing - currently only 8 Bit resolution - motor detection and px4 mode as default

Browse Source
This commit is contained in:
marco
2013-04-29 18:32:30 +02:00
parent d9f9ecb084
commit ca5dcc11a7
1 changed files with 55 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/mkblctrl/mkblctrl.cpp
+55 -28
View File
@@ -121,6 +121,7 @@ public:
int set_motor_test(bool motortest); int set_motor_test(bool motortest);
int set_px4mode(int px4mode); int set_px4mode(int px4mode);
int set_frametype(int frametype); int set_frametype(int frametype);
unsigned int mk_check_for_blctrl(unsigned int count, unsigned int showOutput);
private: private:
static const unsigned _max_actuators = MAX_MOTORS; static const unsigned _max_actuators = MAX_MOTORS;
@@ -176,7 +177,6 @@ private:
int gpio_ioctl(file *filp, int cmd, unsigned long arg); int gpio_ioctl(file *filp, int cmd, unsigned long arg);
int mk_servo_arm(bool status); 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(unsigned int chan, float val);
int mk_servo_set_test(unsigned int chan, float val); int mk_servo_set_test(unsigned int chan, float val);
int mk_servo_test(unsigned int chan); int mk_servo_test(unsigned int chan);
@@ -350,11 +350,11 @@ MK::set_mode(Mode mode)
switch (mode) { switch (mode) {
case MODE_2PWM: case MODE_2PWM:
if(_num_outputs == 4) { if(_num_outputs == 4) {
debug("MODE_QUAD"); //debug("MODE_QUAD");
} else if(_num_outputs == 6) { } else if(_num_outputs == 6) {
debug("MODE_HEXA"); //debug("MODE_HEXA");
} else if(_num_outputs == 8) { } else if(_num_outputs == 8) {
debug("MODE_OCTO"); //debug("MODE_OCTO");
} }
//up_pwm_servo_init(0x3); //up_pwm_servo_init(0x3);
up_pwm_servo_deinit(); up_pwm_servo_deinit();
@@ -363,11 +363,11 @@ MK::set_mode(Mode mode)
case MODE_4PWM: case MODE_4PWM:
if(_num_outputs == 4) { if(_num_outputs == 4) {
debug("MODE_QUADRO"); //debug("MODE_QUADRO");
} else if(_num_outputs == 6) { } else if(_num_outputs == 6) {
debug("MODE_HEXA"); //debug("MODE_HEXA");
} else if(_num_outputs == 8) { } else if(_num_outputs == 8) {
debug("MODE_OCTO"); //debug("MODE_OCTO");
} }
//up_pwm_servo_init(0xf); //up_pwm_servo_init(0xf);
up_pwm_servo_deinit(); up_pwm_servo_deinit();
@@ -447,8 +447,13 @@ MK::set_motor_count(unsigned count)
memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4)); memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4));
} }
/* check for connected blctrls */ if(_num_outputs == 4) {
mk_check_for_blctrl(_num_outputs); 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; return OK;
} }
@@ -502,7 +507,6 @@ MK::task_main()
memset(&rc_in, 0, sizeof(rc_in)); memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
log("starting"); log("starting");
long update_rate_in_us = 0; long update_rate_in_us = 0;
@@ -666,10 +670,10 @@ MK::mk_servo_arm(bool status)
} }
int unsigned int
MK::mk_check_for_blctrl(unsigned int count) MK::mk_check_for_blctrl(unsigned int count, unsigned int showOutput)
{ {
_retries = 10; _retries = 50;
uint8_t foundMotorCount = 0; uint8_t foundMotorCount = 0;
for(unsigned i=0; i<MAX_MOTORS; i++) { 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); if(showOutput == 1) {
for(unsigned i=0; i< count; i++) { fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount);
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); 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) { if(foundMotorCount == count) {
return true; return true;
} else { } else {
return false; return false;
} }
*/
} }
@@ -1230,6 +1239,7 @@ int
mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype) mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype)
{ {
uint32_t gpio_bits; uint32_t gpio_bits;
int shouldStop = 0;
MK::Mode servo_mode; MK::Mode servo_mode;
/* reset to all-inputs */ /* reset to all-inputs */
@@ -1282,14 +1292,28 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
/* set frametype (geometry) */ /* set frametype (geometry) */
g_mk->set_frametype(frametype); g_mk->set_frametype(frametype);
/* motortest if enabled */
g_mk->set_motor_test(motortest);
/* (re)set count of used motors */ /* (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 */ /* (re)set the PWM output mode */
g_mk->set_mode(servo_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)) if ((servo_mode != MK::MODE_NONE) && (update_rate != 0))
g_mk->set_pwm_rate(update_rate); g_mk->set_pwm_rate(update_rate);
@@ -1335,15 +1359,19 @@ mkblctrl_main(int argc, char *argv[])
int motorcount = 0; int motorcount = 0;
int bus = 1; int bus = 1;
bool motortest = false; bool motortest = false;
int px4mode = MAPPING_MK; int px4mode = MAPPING_PX4;
int frametype = FRAME_PLUS; // + plus is default int frametype = FRAME_PLUS; // + plus is default
new_mode = PORT_FULL_PWM;
motorcount = 8;
/* /*
* Mode switches. * Mode switches.
* *
* XXX use getopt? * XXX use getopt?
*/ */
for (int i = 1; i < argc; i++) { /* argv[0] is "mk" */ for (int i = 1; i < argc; i++) { /* argv[0] is "mk" */
/*
if (!strcmp(argv[i], "mode_quad")) { if (!strcmp(argv[i], "mode_quad")) {
new_mode = PORT_FULL_PWM; new_mode = PORT_FULL_PWM;
motorcount = 4; motorcount = 4;
@@ -1354,8 +1382,10 @@ mkblctrl_main(int argc, char *argv[])
new_mode = PORT_FULL_PWM; new_mode = PORT_FULL_PWM;
motorcount = 8; motorcount = 8;
} }
*/
/* look for the optional update rate for the supported modes */ /* look for the optional update rate for the supported modes */
/*
if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) {
if (argc > i + 1) { if (argc > i + 1) {
pwm_update_rate_in_hz = atoi(argv[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 */ /* look for the optional i2c bus parameter */
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
if (argc > i + 1) { if (argc > i + 1) {
@@ -1376,9 +1407,10 @@ mkblctrl_main(int argc, char *argv[])
} }
/* look for the optional frame parameter */ /* 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 (argc > i + 1) {
if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) { 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) { if(strcmp(argv[i + 1], "+") == 0) {
frametype = FRAME_PLUS; frametype = FRAME_PLUS;
} else { } else {
@@ -1388,16 +1420,11 @@ mkblctrl_main(int argc, char *argv[])
errx(1, "only + or x for frametype supported !"); errx(1, "only + or x for frametype supported !");
} }
} else { } else {
errx(1, "missing argument for frametype (-f)"); errx(1, "missing argument for mkmode (-mkmode)");
return 1; return 1;
} }
} }
/* look for the optional geometry parameter */
if (strcmp(argv[i], "-px4mode") == 0) {
px4mode = MAPPING_PX4;
}
/* look for the optional test parameter */ /* look for the optional test parameter */
if (strcmp(argv[i], "-t") == 0) { if (strcmp(argv[i], "-t") == 0) {
motortest = true; motortest = true;
@@ -1407,7 +1434,7 @@ mkblctrl_main(int argc, char *argv[])
if(new_mode == PORT_MODE_UNSET) { if(new_mode == PORT_MODE_UNSET) {
fprintf(stderr, "mkblctrl: unrecognised command, try:\n"); 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); exit(1);
} }