aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/mkblctrl/mkblctrl.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/mkblctrl/mkblctrl.cpp')
-rw-r--r--src/drivers/mkblctrl/mkblctrl.cpp208
1 files changed, 146 insertions, 62 deletions
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index b93f38cf6..30d6069b3 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 <marco@wtns.de>
*
* 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 <marco@wtns.de>
*
*/
@@ -73,7 +75,6 @@
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/esc_status.h>
@@ -89,8 +90,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 500000
class MK : public device::I2C
{
@@ -112,7 +113,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);
@@ -126,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;
@@ -141,9 +142,9 @@ private:
unsigned int _motor;
int _px4mode;
int _frametype;
+ 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;
@@ -244,7 +245,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),
@@ -252,7 +253,6 @@ MK::MK(int bus) :
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
- _t_actuators_effective(0),
_t_esc_status(0),
_num_outputs(0),
_motortest(false),
@@ -265,6 +265,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 +295,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 +317,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();
@@ -525,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));
@@ -595,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++) {
@@ -633,10 +629,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];
@@ -668,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;
@@ -692,16 +685,22 @@ 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);
+
}
}
- //::close(_t_esc_status);
+ ::close(_t_esc_status);
::close(_t_actuators);
- ::close(_t_actuators_effective);
::close(_t_actuator_armed);
@@ -727,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;
@@ -952,6 +955,7 @@ MK::mk_servo_test(unsigned int chan)
if (_motor >= _num_outputs) {
_motor = -1;
_motortest = false;
+ fprintf(stderr, "[mkblctrl] Motortest finished...\n");
}
}
@@ -1367,7 +1371,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 {
@@ -1377,7 +1381,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);
@@ -1390,13 +1394,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;
@@ -1415,6 +1419,52 @@ mk_start(unsigned bus, unsigned motors)
}
+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[]);
@@ -1425,13 +1475,14 @@ 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;
bool overrideSecurityChecks = false;
bool showHelp = false;
bool newMode = false;
+ char *devicepath = "";
/*
* optional parameters
@@ -1491,36 +1542,69 @@ 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, "\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, " [-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 --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 (g_mk == nullptr) {
- if (mk_start(bus, motorcount) != OK) {
- errx(1, "failed to start the MK-BLCtrl driver");
-
- } else {
- newMode = true;
- }
- }
-
-
- /* 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(1);
+ 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);
+
+ }
+ }
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-19 00:49:53 +0000