forked from Archive/PX4-Autopilot
Merge branch 'master' into paul_estimator
This commit is contained in:
Lorenz Meier
commit
a0e691fa49
|
|
@ -38,6 +38,8 @@ MODULES += drivers/ets_airspeed
|
|||
MODULES += drivers/meas_airspeed
|
||||
MODULES += drivers/frsky_telemetry
|
||||
MODULES += modules/sensors
|
||||
MODULES += drivers/mkblctrl
|
||||
|
||||
|
||||
# Needs to be burned to the ground and re-written; for now,
|
||||
# just don't build it.
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
* Author: Marco Bauer <marco@wtns.de>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
|
|
@ -65,7 +65,6 @@
|
|||
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <drivers/drv_gpio.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
|
||||
|
|
@ -93,16 +92,12 @@
|
|||
#define MOTOR_SPINUP_COUNTER 30
|
||||
#define ESC_UORB_PUBLISH_DELAY 500000
|
||||
|
||||
|
||||
|
||||
|
||||
class MK : public device::I2C
|
||||
{
|
||||
public:
|
||||
enum Mode {
|
||||
MODE_NONE,
|
||||
MODE_2PWM,
|
||||
MODE_4PWM,
|
||||
MODE_6PWM,
|
||||
};
|
||||
|
||||
enum MappingMode {
|
||||
MAPPING_MK = 0,
|
||||
MAPPING_PX4,
|
||||
|
|
@ -120,8 +115,7 @@ public:
|
|||
virtual int init(unsigned motors);
|
||||
virtual ssize_t write(file *filp, const char *buffer, size_t len);
|
||||
|
||||
int set_mode(Mode mode);
|
||||
int set_pwm_rate(unsigned rate);
|
||||
int set_update_rate(unsigned rate);
|
||||
int set_motor_count(unsigned count);
|
||||
int set_motor_test(bool motortest);
|
||||
int set_overrideSecurityChecks(bool overrideSecurityChecks);
|
||||
|
|
@ -133,7 +127,6 @@ private:
|
|||
static const unsigned _max_actuators = MAX_MOTORS;
|
||||
static const bool showDebug = false;
|
||||
|
||||
Mode _mode;
|
||||
int _update_rate;
|
||||
int _current_update_rate;
|
||||
int _task;
|
||||
|
|
@ -180,33 +173,15 @@ private:
|
|||
static const GPIOConfig _gpio_tab[];
|
||||
static const unsigned _ngpio;
|
||||
|
||||
void gpio_reset(void);
|
||||
void gpio_set_function(uint32_t gpios, int function);
|
||||
void gpio_write(uint32_t gpios, int function);
|
||||
uint32_t gpio_read(void);
|
||||
int gpio_ioctl(file *filp, int cmd, unsigned long arg);
|
||||
int mk_servo_arm(bool status);
|
||||
|
||||
int mk_servo_set(unsigned int chan, short val);
|
||||
int mk_servo_set_value(unsigned int chan, short val);
|
||||
int mk_servo_test(unsigned int chan);
|
||||
short scaling(float val, float inMin, float inMax, float outMin, float outMax);
|
||||
|
||||
|
||||
};
|
||||
|
||||
const MK::GPIOConfig MK::_gpio_tab[] = {
|
||||
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
|
||||
{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
|
||||
{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
|
||||
{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
|
||||
{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
|
||||
{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
|
||||
{GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
|
||||
{GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
|
||||
};
|
||||
|
||||
const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]);
|
||||
|
||||
|
||||
const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad + configuration
|
||||
const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 }; // Addresstranslator for Hexa + configuration
|
||||
|
|
@ -247,8 +222,7 @@ MK *g_mk;
|
|||
|
||||
MK::MK(int bus, const char *_device_path) :
|
||||
I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
|
||||
_mode(MODE_NONE),
|
||||
_update_rate(50),
|
||||
_update_rate(400),
|
||||
_task(-1),
|
||||
_t_actuators(-1),
|
||||
_t_actuator_armed(-1),
|
||||
|
|
@ -317,26 +291,23 @@ MK::init(unsigned motors)
|
|||
|
||||
usleep(500000);
|
||||
|
||||
if (sizeof(_device) > 0) {
|
||||
ret = register_driver(_device, &fops, 0666, (void *)this);
|
||||
if (sizeof(_device) > 0) {
|
||||
ret = register_driver(_device, &fops, 0666, (void *)this);
|
||||
|
||||
if (ret == OK) {
|
||||
if (ret == OK) {
|
||||
log("creating alternate output device");
|
||||
_primary_pwm_device = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* reset GPIOs */
|
||||
gpio_reset();
|
||||
}
|
||||
|
||||
/* start the IO interface task */
|
||||
_task = task_spawn_cmd("mkblctrl",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 20,
|
||||
2048,
|
||||
(main_t)&MK::task_main_trampoline,
|
||||
nullptr);
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 20,
|
||||
2048,
|
||||
(main_t)&MK::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
|
||||
if (_task < 0) {
|
||||
|
|
@ -354,43 +325,7 @@ MK::task_main_trampoline(int argc, char *argv[])
|
|||
}
|
||||
|
||||
int
|
||||
MK::set_mode(Mode mode)
|
||||
{
|
||||
/*
|
||||
* Configure for PWM output.
|
||||
*
|
||||
* Note that regardless of the configured mode, the task is always
|
||||
* listening and mixing; the mode just selects which of the channels
|
||||
* are presented on the output pins.
|
||||
*/
|
||||
switch (mode) {
|
||||
case MODE_2PWM:
|
||||
up_pwm_servo_deinit();
|
||||
_update_rate = UPDATE_RATE; /* default output rate */
|
||||
break;
|
||||
|
||||
case MODE_4PWM:
|
||||
up_pwm_servo_deinit();
|
||||
_update_rate = UPDATE_RATE; /* default output rate */
|
||||
break;
|
||||
|
||||
case MODE_NONE:
|
||||
debug("MODE_NONE");
|
||||
/* disable servo outputs and set a very low update rate */
|
||||
up_pwm_servo_deinit();
|
||||
_update_rate = UPDATE_RATE;
|
||||
break;
|
||||
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
_mode = mode;
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
MK::set_pwm_rate(unsigned rate)
|
||||
MK::set_update_rate(unsigned rate)
|
||||
{
|
||||
if ((rate > 500) || (rate < 10))
|
||||
return -EINVAL;
|
||||
|
|
@ -621,11 +556,13 @@ MK::task_main()
|
|||
}
|
||||
}
|
||||
|
||||
if(!_overrideSecurityChecks) {
|
||||
if (!_overrideSecurityChecks) {
|
||||
/* don't go under BLCTRL_MIN_VALUE */
|
||||
|
||||
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
|
||||
outputs.output[i] = BLCTRL_MIN_VALUE;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* output to BLCtrl's */
|
||||
|
|
@ -675,21 +612,24 @@ MK::task_main()
|
|||
esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
|
||||
esc.esc[i].esc_rpm = (uint16_t) 0;
|
||||
esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
|
||||
|
||||
if (Motor[i].Version == 1) {
|
||||
// BLCtrl 2.0 (11Bit)
|
||||
esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits;
|
||||
esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits;
|
||||
|
||||
} else {
|
||||
// BLCtrl < 2.0 (8Bit)
|
||||
esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
// if motortest is requested - do it...
|
||||
if (_motortest == true) {
|
||||
mk_servo_test(i);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -728,7 +668,7 @@ MK::mk_servo_arm(bool status)
|
|||
unsigned int
|
||||
MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
|
||||
{
|
||||
if(initI2C) {
|
||||
if (initI2C) {
|
||||
I2C::init();
|
||||
}
|
||||
|
||||
|
|
@ -781,8 +721,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
|
|||
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(!_overrideSecurityChecks) {
|
||||
|
||||
if (!_overrideSecurityChecks) {
|
||||
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
|
||||
_task_should_exit = true;
|
||||
}
|
||||
|
|
@ -811,8 +751,8 @@ MK::mk_servo_set(unsigned int chan, short val)
|
|||
tmpVal = 0;
|
||||
}
|
||||
|
||||
Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff;
|
||||
Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07;
|
||||
Motor[chan].SetPoint = (uint8_t)(tmpVal >> 3) & 0xff;
|
||||
Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal % 8) & 0x07;
|
||||
|
||||
if (_armed == false) {
|
||||
Motor[chan].SetPoint = 0;
|
||||
|
|
@ -1019,28 +959,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
|
|||
{
|
||||
int ret;
|
||||
|
||||
// XXX disabled, confusing users
|
||||
|
||||
/* try it as a GPIO ioctl first */
|
||||
ret = gpio_ioctl(filp, cmd, arg);
|
||||
|
||||
if (ret != -ENOTTY)
|
||||
return ret;
|
||||
|
||||
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
|
||||
/*
|
||||
switch (_mode) {
|
||||
case MODE_2PWM:
|
||||
case MODE_4PWM:
|
||||
case MODE_6PWM:
|
||||
ret = pwm_ioctl(filp, cmd, arg);
|
||||
break;
|
||||
|
||||
default:
|
||||
debug("not in a PWM mode");
|
||||
break;
|
||||
}
|
||||
*/
|
||||
ret = pwm_ioctl(filp, cmd, arg);
|
||||
|
||||
/* if nobody wants it, let CDev have it */
|
||||
|
|
@ -1075,6 +993,10 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
ret = OK;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_UPDATE_RATE:
|
||||
*(uint32_t *)arg = _update_rate;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
|
||||
ret = OK;
|
||||
break;
|
||||
|
|
@ -1084,6 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
if (arg < 2150) {
|
||||
Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
|
||||
mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
|
||||
|
||||
} else {
|
||||
ret = -EINVAL;
|
||||
}
|
||||
|
|
@ -1198,139 +1121,10 @@ MK::write(file *filp, const char *buffer, size_t len)
|
|||
return count * 2;
|
||||
}
|
||||
|
||||
void
|
||||
MK::gpio_reset(void)
|
||||
{
|
||||
/*
|
||||
* Setup default GPIO config - all pins as GPIOs, GPIO driver chip
|
||||
* to input mode.
|
||||
*/
|
||||
for (unsigned i = 0; i < _ngpio; i++)
|
||||
stm32_configgpio(_gpio_tab[i].input);
|
||||
|
||||
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
|
||||
stm32_configgpio(GPIO_GPIO_DIR);
|
||||
}
|
||||
|
||||
void
|
||||
MK::gpio_set_function(uint32_t gpios, int function)
|
||||
{
|
||||
/*
|
||||
* GPIOs 0 and 1 must have the same direction as they are buffered
|
||||
* by a shared 2-port driver. Any attempt to set either sets both.
|
||||
*/
|
||||
if (gpios & 3) {
|
||||
gpios |= 3;
|
||||
|
||||
/* flip the buffer to output mode if required */
|
||||
if (GPIO_SET_OUTPUT == function)
|
||||
stm32_gpiowrite(GPIO_GPIO_DIR, 1);
|
||||
}
|
||||
|
||||
/* configure selected GPIOs as required */
|
||||
for (unsigned i = 0; i < _ngpio; i++) {
|
||||
if (gpios & (1 << i)) {
|
||||
switch (function) {
|
||||
case GPIO_SET_INPUT:
|
||||
stm32_configgpio(_gpio_tab[i].input);
|
||||
break;
|
||||
|
||||
case GPIO_SET_OUTPUT:
|
||||
stm32_configgpio(_gpio_tab[i].output);
|
||||
break;
|
||||
|
||||
case GPIO_SET_ALT_1:
|
||||
if (_gpio_tab[i].alt != 0)
|
||||
stm32_configgpio(_gpio_tab[i].alt);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* flip buffer to input mode if required */
|
||||
if ((GPIO_SET_INPUT == function) && (gpios & 3))
|
||||
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
|
||||
}
|
||||
|
||||
void
|
||||
MK::gpio_write(uint32_t gpios, int function)
|
||||
{
|
||||
int value = (function == GPIO_SET) ? 1 : 0;
|
||||
|
||||
for (unsigned i = 0; i < _ngpio; i++)
|
||||
if (gpios & (1 << i))
|
||||
stm32_gpiowrite(_gpio_tab[i].output, value);
|
||||
}
|
||||
|
||||
uint32_t
|
||||
MK::gpio_read(void)
|
||||
{
|
||||
uint32_t bits = 0;
|
||||
|
||||
for (unsigned i = 0; i < _ngpio; i++)
|
||||
if (stm32_gpioread(_gpio_tab[i].input))
|
||||
bits |= (1 << i);
|
||||
|
||||
return bits;
|
||||
}
|
||||
|
||||
int
|
||||
MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
lock();
|
||||
|
||||
switch (cmd) {
|
||||
|
||||
case GPIO_RESET:
|
||||
gpio_reset();
|
||||
break;
|
||||
|
||||
case GPIO_SET_OUTPUT:
|
||||
case GPIO_SET_INPUT:
|
||||
case GPIO_SET_ALT_1:
|
||||
gpio_set_function(arg, cmd);
|
||||
break;
|
||||
|
||||
case GPIO_SET_ALT_2:
|
||||
case GPIO_SET_ALT_3:
|
||||
case GPIO_SET_ALT_4:
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
|
||||
case GPIO_SET:
|
||||
case GPIO_CLEAR:
|
||||
gpio_write(arg, cmd);
|
||||
break;
|
||||
|
||||
case GPIO_GET:
|
||||
*(uint32_t *)arg = gpio_read();
|
||||
break;
|
||||
|
||||
default:
|
||||
ret = -ENOTTY;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
enum PortMode {
|
||||
PORT_MODE_UNSET = 0,
|
||||
PORT_FULL_GPIO,
|
||||
PORT_FULL_SERIAL,
|
||||
PORT_FULL_PWM,
|
||||
PORT_GPIO_AND_SERIAL,
|
||||
PORT_PWM_AND_SERIAL,
|
||||
PORT_PWM_AND_GPIO,
|
||||
};
|
||||
|
||||
enum MappingMode {
|
||||
MAPPING_MK = 0,
|
||||
MAPPING_PX4,
|
||||
|
|
@ -1341,20 +1135,11 @@ enum FrameType {
|
|||
FRAME_X,
|
||||
};
|
||||
|
||||
PortMode g_port_mode;
|
||||
|
||||
int
|
||||
mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
|
||||
mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
|
||||
{
|
||||
uint32_t gpio_bits;
|
||||
int shouldStop = 0;
|
||||
MK::Mode servo_mode;
|
||||
|
||||
/* reset to all-inputs */
|
||||
g_mk->ioctl(0, GPIO_RESET, 0);
|
||||
|
||||
gpio_bits = 0;
|
||||
servo_mode = MK::MODE_NONE;
|
||||
|
||||
/* native PX4 addressing) */
|
||||
g_mk->set_px4mode(px4mode);
|
||||
|
|
@ -1368,7 +1153,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
|
|||
/* ovveride security checks if enabled */
|
||||
g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
|
||||
|
||||
|
||||
/* count used motors */
|
||||
do {
|
||||
if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
|
||||
|
|
@ -1383,86 +1167,54 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
|
|||
|
||||
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);
|
||||
|
||||
|
||||
if ((servo_mode != MK::MODE_NONE) && (update_rate != 0))
|
||||
g_mk->set_pwm_rate(update_rate);
|
||||
g_mk->set_update_rate(update_rate);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
mk_start(unsigned bus, unsigned motors, char *device_path)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
if (g_mk == nullptr) {
|
||||
|
||||
g_mk = new MK(bus, device_path);
|
||||
|
||||
if (g_mk == nullptr) {
|
||||
ret = -ENOMEM;
|
||||
|
||||
} else {
|
||||
ret = g_mk->init(motors);
|
||||
|
||||
if (ret != OK) {
|
||||
delete g_mk;
|
||||
g_mk = nullptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
mk_check_for_i2c_esc_bus(char *device_path, int motors)
|
||||
mk_start(unsigned motors, char *device_path)
|
||||
{
|
||||
int ret;
|
||||
|
||||
if (g_mk == nullptr) {
|
||||
// try i2c3 first
|
||||
g_mk = new MK(3, device_path);
|
||||
|
||||
g_mk = new MK(3, device_path);
|
||||
if (!g_mk)
|
||||
return -ENOMEM;
|
||||
|
||||
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;
|
||||
}
|
||||
if (OK == g_mk->init(motors)) {
|
||||
warnx("[mkblctrl] scanning i2c3...\n");
|
||||
ret = g_mk->mk_check_for_blctrl(8, false, true);
|
||||
|
||||
if (ret > 0) {
|
||||
return OK;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
delete g_mk;
|
||||
g_mk = nullptr;
|
||||
|
||||
// fallback to bus 1
|
||||
g_mk = new MK(1, device_path);
|
||||
|
||||
if (!g_mk)
|
||||
return -ENOMEM;
|
||||
|
||||
if (OK == g_mk->init(motors)) {
|
||||
warnx("[mkblctrl] scanning i2c1...\n");
|
||||
ret = g_mk->mk_check_for_blctrl(8, false, true);
|
||||
|
||||
if (ret > 0) {
|
||||
return OK;
|
||||
}
|
||||
}
|
||||
|
||||
delete g_mk;
|
||||
g_mk = nullptr;
|
||||
|
||||
return -ENXIO;
|
||||
}
|
||||
|
||||
|
||||
} // namespace
|
||||
|
|
@ -1472,10 +1224,8 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
|
|||
int
|
||||
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 px4mode = MAPPING_PX4;
|
||||
int frametype = FRAME_PLUS; // + plus is default
|
||||
bool motortest = false;
|
||||
|
|
@ -1489,18 +1239,6 @@ mkblctrl_main(int argc, char *argv[])
|
|||
*/
|
||||
for (int i = 1; i < argc; i++) {
|
||||
|
||||
/* look for the optional i2c bus parameter */
|
||||
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
|
||||
if (argc > i + 1) {
|
||||
bus = atoi(argv[i + 1]);
|
||||
newMode = true;
|
||||
|
||||
} else {
|
||||
errx(1, "missing argument for i2c bus (-b)");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
/* look for the optional frame parameter */
|
||||
if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
|
||||
if (argc > i + 1) {
|
||||
|
|
@ -1560,51 +1298,43 @@ mkblctrl_main(int argc, char *argv[])
|
|||
fprintf(stderr, "mkblctrl: help:\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");
|
||||
fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
if (!motortest) {
|
||||
if (g_mk == nullptr) {
|
||||
if (bus == -1) {
|
||||
bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
|
||||
}
|
||||
if (g_mk == nullptr) {
|
||||
if (mk_start(motorcount, devicepath) != OK) {
|
||||
errx(1, "failed to start the MK-BLCtrl driver");
|
||||
}
|
||||
|
||||
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(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
|
||||
}
|
||||
|
||||
/* parameter set ? */
|
||||
if (newMode) {
|
||||
/* switch parameter */
|
||||
return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
|
||||
}
|
||||
exit(0);
|
||||
|
||||
exit(0);
|
||||
} else {
|
||||
errx(1, "MK-BLCtrl driver already running");
|
||||
}
|
||||
} 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 {
|
||||
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);
|
||||
} else {
|
||||
g_mk->set_motor_test(motortest);
|
||||
exit(0);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue