diff --git a/apps/drivers/mkblctrl/Makefile b/apps/drivers/mkblctrl/Makefile new file mode 100644 index 0000000000..b3be2c4687 --- /dev/null +++ b/apps/drivers/mkblctrl/Makefile @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Interface driver for the Mikrokopter BLCtrl +# + +APPNAME = mkblctrl +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mkblctrl/mkblctrl.cpp b/apps/drivers/mkblctrl/mkblctrl.cpp new file mode 100644 index 0000000000..c14fdfd1d8 --- /dev/null +++ b/apps/drivers/mkblctrl/mkblctrl.cpp @@ -0,0 +1,1403 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mkblctrl.cpp + * + * Driver/configurator for the Mikrokopter BL-Ctrl. + * Marco Bauer + * + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#define I2C_BUS_SPEED 400000 +#define UPDATE_RATE 400 +#define MAX_MOTORS 8 +#define BLCTRL_BASE_ADDR 0x29 +#define BLCTRL_OLD 0 +#define BLCTRL_NEW 1 +#define BLCTRL_MIN_VALUE -0.984F +#define MOTOR_STATE_PRESENT_MASK 0x80 +#define MOTOR_STATE_ERROR_MASK 0x7F +#define MOTOR_SPINUP_COUNTER 2500 + + +class MK : public device::I2C +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_NONE + }; + + enum MappingMode { + MAPPING_MK = 0, + MAPPING_PX4, + }; + + enum FrameType { + FRAME_PLUS = 0, + FRAME_X, + }; + + MK(int bus); + ~MK(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual int init(unsigned motors); + + int set_mode(Mode mode); + int set_pwm_rate(unsigned rate); + int set_motor_count(unsigned count); + int set_motor_test(bool motortest); + int set_px4mode(int px4mode); + int set_frametype(int frametype); + +private: + static const unsigned _max_actuators = MAX_MOTORS; + static const bool showDebug = false; + + Mode _mode; + int _update_rate; + int _current_update_rate; + int _task; + int _t_actuators; + int _t_armed; + unsigned int _motor; + int _px4mode; + int _frametype; + orb_advert_t _t_outputs; + orb_advert_t _t_actuators_effective; + unsigned int _num_outputs; + bool _primary_pwm_device; + bool _motortest; + + volatile bool _task_should_exit; + bool _armed; + + unsigned long debugCounter; + + MixerGroup *_mixers; + + actuator_controls_s _controls; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main() __attribute__((noreturn)); + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + 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_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); + + +}; + +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 +const int blctrlAddr_octo_plus[] = { 0, 3, -1, 0, 3, 0, 0, -5 }; // Addresstranslator for Octo + configuration + +const int blctrlAddr_quad_x[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad X configuration +const int blctrlAddr_hexa_x[] = { 2, 4, -2, 0, -3, -1, 0, 0 }; // Addresstranslator for Hexa X configuration +const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 }; // Addresstranslator for Octo X configuration + +const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0}; + +int addrTranslator[] = {0,0,0,0,0,0,0,0}; + +struct MotorData_t +{ + unsigned int Version; // the version of the BL (0 = old) + unsigned int SetPoint; // written by attitude controller + unsigned int SetPointLowerBits; // for higher Resolution of new BLs + unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present + unsigned int ReadMode; // select data to read + // the following bytes must be exactly in that order! + unsigned int Current; // in 0.1 A steps, read back from BL + unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit + unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in °C + unsigned int RoundCount; +}; + +MotorData_t Motor[MAX_MOTORS]; + + +namespace +{ + +MK *g_mk; + +} // namespace + +MK::MK(int bus) : + I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), + _mode(MODE_NONE), + _update_rate(50), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _t_outputs(0), + _t_actuators_effective(0), + _num_outputs(0), + _motortest(false), + _motor(-1), + _px4mode(MAPPING_MK), + _frametype(FRAME_PLUS), + _primary_pwm_device(false), + _task_should_exit(false), + _armed(false), + _mixers(nullptr) +{ + _debug_enabled = true; +} + +MK::~MK() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + g_mk = nullptr; +} + +int +MK::init(unsigned motors) +{ + _num_outputs = motors; + debugCounter = 0; + int ret; + ASSERT(_task == -1); + + ret = I2C::init(); + + if (ret != OK) { + warnx("I2C init failed"); + return ret; + } + + 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 (ret == OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + + /* reset GPIOs */ + gpio_reset(); + + /* check for connected blctrls */ + //mk_check_for_blctrl(_num_outputs); + + /* start the IO interface task */ + _task = task_spawn("mkblctrl", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX -20, + 2048, + (main_t)&MK::task_main_trampoline, + nullptr); + + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +MK::task_main_trampoline(int argc, char *argv[]) +{ + g_mk->task_main(); +} + +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: + if(_num_outputs == 4) { + debug("MODE_QUAD"); + } else if(_num_outputs == 6) { + debug("MODE_HEXA"); + } else if(_num_outputs == 8) { + debug("MODE_OCTO"); + } + //up_pwm_servo_init(0x3); + up_pwm_servo_deinit(); + _update_rate = UPDATE_RATE; /* default output rate */ + break; + + case MODE_4PWM: + if(_num_outputs == 4) { + debug("MODE_QUADRO"); + } else if(_num_outputs == 6) { + debug("MODE_HEXA"); + } else if(_num_outputs == 8) { + debug("MODE_OCTO"); + } + //up_pwm_servo_init(0xf); + 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) +{ + if ((rate > 500) || (rate < 10)) + return -EINVAL; + + _update_rate = rate; + return OK; +} + +int +MK::set_px4mode(int px4mode) +{ + _px4mode = px4mode; +} + +int +MK::set_frametype(int frametype) +{ + _frametype = frametype; +} + + +int +MK::set_motor_count(unsigned count) +{ + _num_outputs = count; + + if(_px4mode == MAPPING_MK) { + if(_frametype == FRAME_PLUS) { + fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n"); + } else if(_frametype == FRAME_X) { + fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n"); + } + if(_num_outputs == 4) { + if(_frametype == FRAME_PLUS) { + memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus)); + } else if(_frametype == FRAME_X) { + memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x)); + } + } else if(_num_outputs == 6) { + if(_frametype == FRAME_PLUS) { + memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus)); + } else if(_frametype == FRAME_X) { + memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x)); + } + } else if(_num_outputs == 8) { + if(_frametype == FRAME_PLUS) { + memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus)); + } else if(_frametype == FRAME_X) { + memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x)); + } + } + } else { + fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n"); + memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4)); + } + + /* check for connected blctrls */ + mk_check_for_blctrl(_num_outputs); + + return OK; +} + +int +MK::set_motor_test(bool motortest) +{ + _motortest = motortest; + return OK; +} + + +void +MK::task_main() +{ + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); + /* force a reset of the update rate */ + _current_update_rate = 0; + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + /* advertise the mixed control outputs */ + _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); + + pollfd fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + + log("starting"); + long update_rate_in_us = 0; + + /* loop until killed */ + while (!_task_should_exit) { + + /* handle update rate changes */ + if (_current_update_rate != _update_rate) { + int update_rate_in_ms = int(1000 / _update_rate); + update_rate_in_us = long(1000000 / _update_rate); + + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { + update_rate_in_ms = 2; + _update_rate = 500; + } + /* reject slower than 50 Hz updates */ + if (update_rate_in_ms > 20) { + update_rate_in_ms = 20; + _update_rate = 50; + } + + orb_set_interval(_t_actuators, update_rate_in_ms); + up_pwm_servo_set_rate(_update_rate); + _current_update_rate = _update_rate; + } + + /* sleep waiting for data, but no more than a second */ + int ret = ::poll(&fds[0], 2, 1000); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + /* do mixing */ + 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)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + + /* iterate actuators */ + for (unsigned int i = 0; i < _num_outputs; i++) { + + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + //outputs.output[i] = 1500 + (600 * outputs.output[i]); + //outputs.output[i] = 127 + (127 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + if(outputs.output[i] < -1.0f) { + outputs.output[i] = -1.0f; + } else if(outputs.output[i] > 1.0f) { + outputs.output[i] = 1.0f; + } else { + outputs.output[i] = -1.0f; + } + } + + /* don't go under BLCTRL_MIN_VALUE */ + if(outputs.output[i] < BLCTRL_MIN_VALUE) { + outputs.output[i] = BLCTRL_MIN_VALUE; + } + //_motortest = true; + /* output to BLCtrl's */ + if(_motortest == true) { + mk_servo_test(i); + } else { + //mk_servo_set(i, outputs.output[i]); + mk_servo_set_test(i, outputs.output[i]); // 8Bit + } + + + } + + /* and publish for anyone that cares to see */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + + /* update PWM servo armed status if armed and not locked down */ + ////up_pwm_servo_arm(aa.armed && !aa.lockdown); + mk_servo_arm(aa.armed && !aa.lockdown); + } + } + + ::close(_t_actuators); + ::close(_t_actuators_effective); + ::close(_t_armed); + + + /* make sure servos are off */ + up_pwm_servo_deinit(); + + log("stopping"); + + /* note - someone else is responsible for restoring the GPIO config */ + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + + +int +MK::mk_servo_arm(bool status) +{ + _armed = status; + return 0; +} + + +int +MK::mk_check_for_blctrl(unsigned int count) +{ + _retries = 10; + uint8_t foundMotorCount = 0; + + for(unsigned i=0; i 2047) { + tmpVal = 2047; + } + + + Motor[chan].SetPoint = (uint8_t) tmpVal / 3; // divide 8 + Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 8; // rest of divide 8 + //rod = (uint8_t) tmpVal % 8; + //Motor[chan].SetPointLowerBits = rod<<1; // rest of divide 8 + Motor[chan].SetPointLowerBits = 0; + + if(_armed == false) { + Motor[chan].SetPoint = 0; + Motor[chan].SetPointLowerBits = 0; + } + + //if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) { + set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + + if(Motor[chan].Version == BLCTRL_OLD) { + /* + * Old BL-Ctrl 8Bit served. Version < 2.0 + */ + msg[0] = Motor[chan].SetPoint; + if(Motor[chan].RoundCount >= 16) { + // on each 16th cyle we read out the status messages from the blctrl + if (OK == transfer(&msg[0], 1, &result[0], 2)) { + Motor[chan].Current = result[0]; + Motor[chan].MaxPWM = result[1]; + Motor[chan].Temperature = 255;; + } else { + if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + } + Motor[chan].RoundCount = 0; + } else { + if (OK != transfer(&msg[0], 1, nullptr, 0)) { + if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + } + } + + } else { + /* + * New BL-Ctrl 11Bit served. Version >= 2.0 + */ + msg[0] = Motor[chan].SetPoint; + msg[1] = Motor[chan].SetPointLowerBits; + + if(Motor[chan].SetPointLowerBits == 0) { + bytesToSendBL2 = 1; // if setpoint lower bits are zero, we send only the higher bits - this saves time + } + + if(Motor[chan].RoundCount >= 16) { + // on each 16th cyle we read out the status messages from the blctrl + if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) { + Motor[chan].Current = result[0]; + Motor[chan].MaxPWM = result[1]; + Motor[chan].Temperature = result[2]; + } else { + if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + } + Motor[chan].RoundCount = 0; + } else { + if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) { + if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + } + } + + } + + Motor[chan].RoundCount++; + //} + + if(showDebug == true) { + debugCounter++; + if(debugCounter == 2000) { + debugCounter = 0; + for(int i=0; i<_num_outputs; i++){ + if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) { + fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); + } + } + fprintf(stderr, "\n"); + } + } + return 0; +} + +int +MK::mk_servo_set_test(unsigned int chan, float val) +{ + _retries = 0; + int ret; + + float tmpVal = 0; + + uint8_t msg[2] = { 0,0 }; + + tmpVal = (1023 + (1023 * val)); + if(tmpVal > 2048) { + tmpVal = 2048; + } + + Motor[chan].SetPoint = (uint8_t) (tmpVal / 8); + + if(_armed == false) { + Motor[chan].SetPoint = 0; + Motor[chan].SetPointLowerBits = 0; + } + + msg[0] = Motor[chan].SetPoint; + + set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + ret = transfer(&msg[0], 1, nullptr, 0); + + ret = OK; + + return ret; +} + + +int +MK::mk_servo_test(unsigned int chan) +{ + int ret=0; + float tmpVal = 0; + float val = -1; + _retries = 0; + uint8_t msg[2] = { 0,0 }; + + if(debugCounter >= MOTOR_SPINUP_COUNTER) { + debugCounter = 0; + _motor++; + + if(_motor < _num_outputs) { + fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor); + } + + if(_motor >= _num_outputs) { + _motor = -1; + _motortest = false; + } + + } + debugCounter++; + + if(_motor == chan) { + val = BLCTRL_MIN_VALUE; + } else { + val = -1; + } + + tmpVal = (1023 + (1023 * val)); + if(tmpVal > 2048) { + tmpVal = 2048; + } + + //Motor[chan].SetPoint = (uint8_t) (tmpVal / 8); + //Motor[chan].SetPointLowerBits = (uint8_t) (tmpVal % 8) & 0x07; + Motor[chan].SetPoint = (uint8_t) tmpVal>>3; + Motor[chan].SetPointLowerBits = (uint8_t) tmpVal & 0x07; + + if(_motor != chan) { + Motor[chan].SetPoint = 0; + Motor[chan].SetPointLowerBits = 0; + } + + if(Motor[chan].Version == BLCTRL_OLD) { + msg[0] = Motor[chan].SetPoint; + } else { + msg[0] = Motor[chan].SetPoint; + msg[1] = Motor[chan].SetPointLowerBits; + } + + set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); + if(Motor[chan].Version == BLCTRL_OLD) { + ret = transfer(&msg[0], 1, nullptr, 0); + } else { + ret = transfer(&msg[0], 2, nullptr, 0); + } + + return ret; +} + + +int +MK::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls->control[control_index]; + return 0; +} + +int +MK::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + + // XXX disabled, confusing users + //debug("ioctl 0x%04x 0x%08x", cmd, arg); + + /* 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: + ret = pwm_ioctl(filp, cmd, arg); + break; + + default: + debug("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) + ret = CDev::ioctl(filp, cmd, arg); + + return ret; +} + +int +MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + int channel; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + ////up_pwm_servo_arm(true); + mk_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + ////up_pwm_servo_arm(false); + mk_servo_arm(false); + break; + + case PWM_SERVO_SET_UPDATE_RATE: + set_pwm_rate(arg); + break; + + + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): + + /* fake an update to the selected 'servo' channel */ + if ((arg >= 0) && (arg <= 255)) { + channel = cmd - PWM_SERVO_SET(0); + //mk_servo_set(channel, arg); + } else { + ret = -EINVAL; + } + + break; + + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): + /* copy the current output value from the channel */ + *(servo_position_t *)arg = cmd - PWM_SERVO_GET(0); + break; + + case MIXERIOCGETOUTPUTCOUNT: + /* + if (_mode == MODE_4PWM) { + *(unsigned *)arg = 4; + } else { + *(unsigned *)arg = 2; + } + */ + + *(unsigned *)arg = _num_outputs; + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); + } + + break; + } + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + + if (_mixers == nullptr) { + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + } + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +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, +}; + + enum FrameType { + FRAME_PLUS = 0, + FRAME_X, + }; + +PortMode g_port_mode; + +int +mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype) +{ + uint32_t gpio_bits; + MK::Mode servo_mode; + + /* reset to all-inputs */ + g_mk->ioctl(0, GPIO_RESET, 0); + + gpio_bits = 0; + servo_mode = MK::MODE_NONE; + + switch (new_mode) { + case PORT_FULL_GPIO: + case PORT_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT_FULL_SERIAL: + /* set all multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = MK::MODE_4PWM; + break; + + case PORT_GPIO_AND_SERIAL: + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = MK::MODE_2PWM; + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = MK::MODE_2PWM; + break; + } + + /* adjust GPIO config for serial mode(s) */ + if (gpio_bits != 0) + g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + + /* native PX4 addressing) */ + g_mk->set_px4mode(px4mode); + + /* set frametype (geometry) */ + g_mk->set_frametype(frametype); + + /* (re)set count of used motors */ + g_mk->set_motor_count(motorcount); + + /* (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); + + return OK; +} + +int +mk_start(unsigned bus, unsigned motors) +{ + int ret = OK; + + if (g_mk == nullptr) { + + g_mk = new MK(bus); + + if (g_mk == nullptr) { + ret = -ENOMEM; + + } else { + ret = g_mk->init(motors); + + if (ret != OK) { + delete g_mk; + g_mk = nullptr; + } + } + } + + return ret; +} + + +} // namespace + +extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); + +int +mkblctrl_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNSET; + int pwm_update_rate_in_hz = UPDATE_RATE; + int motorcount = 0; + int bus = 1; + bool motortest = false; + int px4mode = MAPPING_MK; + int frametype = FRAME_PLUS; // + plus is default + + /* + * 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; + } else if (!strcmp(argv[i], "mode_hexa")) { + new_mode = PORT_FULL_PWM; + motorcount = 6; + } else if (!strcmp(argv[i], "mode_octo")) { + 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]); + } else { + errx(1, "missing argument for update rate (-u)"); + return 1; + } + } + + /* 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]); + } else { + errx(1, "missing argument for i2c bus (-b)"); + return 1; + } + } + + /* look for the optional frame parameter */ + if (strcmp(argv[i], "-f") == 0 || strcmp(argv[i], "--frame") == 0) { + 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) { + frametype = FRAME_PLUS; + } else { + frametype = FRAME_PLUS; + } + } else { + errx(1, "only + or x for frametype supported !"); + } + } else { + errx(1, "missing argument for frametype (-f)"); + 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; + } + + } + + 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"); + exit(1); + } + + if (mk_start(bus, motorcount) != OK) + errx(1, "failed to start the MK-BLCtrl driver"); + + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNSET) { + + /* yes but it's the same mode */ + //if (new_mode == g_port_mode) + //return OK; + + /* switch modes */ + fprintf(stderr, "[mkblctrl] %iHz Update Rate\n",pwm_update_rate_in_hz); + return mk_new_mode(new_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype); + } + + /* test, etc. here g*/ + + exit(1); +} diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 80cf6f312c..5bac6214c9 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -124,6 +124,7 @@ CONFIGURED_APPS += drivers/blinkm CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/stm32/adc CONFIGURED_APPS += drivers/px4fmu +CONFIGURED_APPS += drivers/mkblctrl CONFIGURED_APPS += drivers/hil CONFIGURED_APPS += drivers/gps CONFIGURED_APPS += drivers/mb12xx