From 59573e5b69f9afc5dbe00bc165ae8e1f4d457f89 Mon Sep 17 00:00:00 2001 From: marco Date: Wed, 17 Apr 2013 19:46:01 +0200 Subject: [PATCH 01/18] BLCtrl 2.0 testing - currently only 8 Bit resolution - this should fly --- apps/drivers/mkblctrl/Makefile | 44 + apps/drivers/mkblctrl/mkblctrl.cpp | 1403 ++++++++++++++++++++++++++++ nuttx/configs/px4fmu/nsh/appconfig | 1 + 3 files changed, 1448 insertions(+) create mode 100644 apps/drivers/mkblctrl/Makefile create mode 100644 apps/drivers/mkblctrl/mkblctrl.cpp 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 From 9da16afcc2985503523460c4629343a556ef40d7 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 25 Apr 2013 08:59:48 +0200 Subject: [PATCH 02/18] Add support for V for quads with offset arms such as the TBS and SteadiDrone QU4D --- apps/systemlib/mixer/mixer.h | 1 + apps/systemlib/mixer/mixer_multirotor.cpp | 11 +++++++++++ apps/systemlib/mixer/multi_tables | 9 ++++++++- 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 71386cba77..40d37fce2b 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -418,6 +418,7 @@ public: enum Geometry { QUAD_X = 0, /**< quad in X configuration */ QUAD_PLUS, /**< quad in + configuration */ + QUAD_V, /**< quad in V configuration */ HEX_X, /**< hex in X configuration */ HEX_PLUS, /**< hex in + configuration */ OCTA_X, diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 4b9cfc023a..a45ca3f21a 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -82,6 +82,12 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { 0.000000, 1.000000, -1.00 }, { -0.000000, -1.000000, -1.00 }, }; +const MultirotorMixer::Rotor _config_quad_v[] = { + { -0.882948, 0.469472, 1.00 }, + { 0.731354, -0.681998, 1.00 }, + { 0.882948, 0.469472, -1.00 }, + { -0.731354, -0.681998, -1.00 }, +}; const MultirotorMixer::Rotor _config_hex_x[] = { { -1.000000, 0.000000, -1.00 }, { 1.000000, 0.000000, 1.00 }, @@ -121,6 +127,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], + &_config_quad_v[0], &_config_hex_x[0], &_config_hex_plus[0], &_config_octa_x[0], @@ -129,6 +136,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { 4, /* quad_x */ 4, /* quad_plus */ + 4, /* quad_v */ 6, /* hex_x */ 6, /* hex_plus */ 8, /* octa_x */ @@ -184,6 +192,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "4x")) { geometry = MultirotorMixer::QUAD_X; + } else if (!strcmp(geomname, "4v")) { + geometry = MultirotorMixer::QUAD_V; + } else if (!strcmp(geomname, "6+")) { geometry = MultirotorMixer::HEX_PLUS; diff --git a/apps/systemlib/mixer/multi_tables b/apps/systemlib/mixer/multi_tables index f17ae30ca6..0c5689143b 100755 --- a/apps/systemlib/mixer/multi_tables +++ b/apps/systemlib/mixer/multi_tables @@ -20,6 +20,13 @@ set quad_plus { 180 CW } +set quad_v { + 62 CCW + -133 CCW + -62 CW + 133 CW +} + set hex_x { 90 CW -90 CCW @@ -60,7 +67,7 @@ set octa_plus { 90 CW } -set tables {quad_x quad_plus hex_x hex_plus octa_x octa_plus} +set tables {quad_x quad_plus quad_v hex_x hex_plus octa_x octa_plus} proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} From ed9fbbce5946d22ded1519ddec1b5ff6a8f2e511 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Apr 2013 17:25:42 +0200 Subject: [PATCH 03/18] HIL bugfixing --- apps/drivers/gps/ubx.cpp | 18 ++++++++---- apps/examples/kalman_demo/KalmanNav.cpp | 2 ++ apps/mavlink/mavlink_receiver.c | 14 +++++---- apps/uORB/topics/vehicle_gps_position.h | 39 +++++++++++++------------ 4 files changed, 43 insertions(+), 30 deletions(-) diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 0d4894b8de..c150f52715 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -33,7 +33,14 @@ * ****************************************************************************/ -/* @file U-Blox protocol implementation */ +/** + * @file ubx.cpp + * + * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description + * including Prototol Specification. + * + * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf + */ #include #include @@ -633,16 +640,17 @@ UBX::handle_message() } case NAV_VELNED: { -// printf("GOT NAV_VELNED MESSAGE\n"); if (!_waiting_for_ack) { + /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */ gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; _gps_position->vel_m_s = (float)packet->speed * 1e-2f; - _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; + _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; _gps_position->vel_ned_valid = true; _gps_position->timestamp_velocity = hrt_absolute_time(); } diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 6df454a555..4ef150da1e 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -355,6 +355,8 @@ int KalmanNav::predictState(float dt) float LDot = vN / R; float lDot = vE / (cosLSing * R); float rotRate = 2 * omega + lDot; + + // XXX position prediction using speed float vNDot = fN - vE * rotRate * sinL + vD * LDot; float vDDot = fD - vE * rotRate * cosL - diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index a30f0bf3c1..22c2fcdade 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -477,25 +477,27 @@ handle_message(mavlink_message_t *msg) /* gps */ hil_gps.timestamp_position = gps.time_usec; -// hil_gps.counter = hil_counter++; hil_gps.time_gps_usec = gps.time_usec; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; -// hil_gps.counter_pos_valid = hil_counter++; hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m hil_gps.s_variance_m_s = 5.0f; hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s - /* gps.cog is in degrees 0..360 * 100, heading is -PI..PI */ - float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f - M_PI_F; + /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ + float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; + /* go back to -PI..PI */ + if (heading_rad > M_PI_F) + heading_rad -= 2.0f * M_PI_F; hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad); hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad); hil_gps.vel_d_m_s = 0.0f; - /* COG (course over ground) is speced as 0..360 degrees (compass) */ - hil_gps.cog_rad = heading_rad + M_PI_F; // from deg*100 to rad + hil_gps.vel_ned_valid = true; + /* COG (course over ground) is speced as -PI..+PI */ + hil_gps.cog_rad = heading_rad; hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index 5463a460da..0a7fb4e9da 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -55,38 +55,39 @@ */ struct vehicle_gps_position_s { - uint64_t timestamp_position; /**< Timestamp for position information */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ + uint64_t timestamp_position; /**< Timestamp for position information */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + float p_variance_m; /**< position accuracy estimate m */ + float c_variance_rad; /**< course accuracy estimate rad */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ - float vel_m_s; /**< GPS ground speed (m/s) */ - float vel_n_m_s; /**< GPS ground speed in m/s */ - float vel_e_m_s; /**< GPS ground speed in m/s */ - float vel_d_m_s; /**< GPS ground speed in m/s */ - float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad */ - bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ + float vel_m_s; /**< GPS ground speed (m/s) */ + float vel_n_m_s; /**< GPS ground speed in m/s */ + float vel_e_m_s; /**< GPS ground speed in m/s */ + float vel_d_m_s; /**< GPS ground speed in m/s */ + float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ uint64_t timestamp_time; /**< Timestamp for time information */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ - uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ + uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ uint8_t satellite_prn[20]; /**< Global satellite ID */ uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ - uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ - bool satellite_info_available; /**< 0 for no info, 1 for info available */ + bool satellite_info_available; /**< 0 for no info, 1 for info available */ }; /** From 1063ae9de9f2efe7b8c468936695255f252a08f4 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 25 Apr 2013 21:40:43 +0200 Subject: [PATCH 04/18] Add a mixer file for the V quad --- ROMFS/mixers/FMU_quad_v.mix | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 ROMFS/mixers/FMU_quad_v.mix diff --git a/ROMFS/mixers/FMU_quad_v.mix b/ROMFS/mixers/FMU_quad_v.mix new file mode 100644 index 0000000000..2a4a0f3419 --- /dev/null +++ b/ROMFS/mixers/FMU_quad_v.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the V configuration. All controls +are mixed 100%. + +R: 4v 10000 10000 10000 0 From a6b8463308d62be1657fb773445375b035a6765f Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 25 Apr 2013 23:44:22 +0200 Subject: [PATCH 05/18] Update the makefile to include the new mixer config. --- ROMFS/Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/Makefile b/ROMFS/Makefile index ed39ab8257..11a4650fa6 100644 --- a/ROMFS/Makefile +++ b/ROMFS/Makefile @@ -32,6 +32,7 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \ $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \ $(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \ $(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \ + $(SRCROOT)/mixers/FMU_quad_v.mix~mixers/FMU_quad_v.mix \ $(SRCROOT)/mixers/FMU_hex_x.mix~mixers/FMU_hex_x.mix \ $(SRCROOT)/mixers/FMU_hex_+.mix~mixers/FMU_hex_+.mix \ $(SRCROOT)/mixers/FMU_octo_x.mix~mixers/FMU_octo_x.mix \ From afbb4d55b3fc4e0e2a1fc1a1b052e9f4fb034d51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Apr 2013 15:13:47 +0200 Subject: [PATCH 06/18] Finished conversion to C++ --- apps/attitude_estimator_ekf/Makefile | 5 +++-- ...f_main.c => attitude_estimator_ekf_main.cpp} | 17 ++++++++++++----- 2 files changed, 15 insertions(+), 7 deletions(-) rename apps/attitude_estimator_ekf/{attitude_estimator_ekf_main.c => attitude_estimator_ekf_main.cpp} (98%) diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index 734af7cc1c..46a54c6607 100755 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -35,8 +35,9 @@ APPNAME = attitude_estimator_ekf PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 -CSRCS = attitude_estimator_ekf_main.c \ - attitude_estimator_ekf_params.c \ +CXXSRCS = attitude_estimator_ekf_main.cpp + +CSRCS = attitude_estimator_ekf_params.c \ codegen/eye.c \ codegen/attitudeKalmanfilter.c \ codegen/mrdivide.c \ diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp similarity index 98% rename from apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c rename to apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index bd972f03f3..1a50dde0f4 100755 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -66,11 +66,17 @@ #include #include +#ifdef __cplusplus +extern "C" { +#endif #include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter.h" #include "attitude_estimator_ekf_params.h" +#ifdef __cplusplus +} +#endif -__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); +extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -265,10 +271,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* Main loop*/ while (!thread_should_exit) { - struct pollfd fds[2] = { - { .fd = sub_raw, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN } - }; + struct pollfd fds[2]; + fds[0].fd = sub_raw; + fds[0].events = POLLIN; + fds[1].fd = sub_params; + fds[1].events = POLLIN; int ret = poll(fds, 2, 1000); if (ret < 0) { From 556a017444b809c18e2ce495a2fd00380960e0f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Apr 2013 17:41:46 +0200 Subject: [PATCH 07/18] Hotfix: GPS MAVLink transmission fixes --- apps/mavlink/orb_listener.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 1dd3fc2d8e..5f15facf87 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -229,6 +229,13 @@ l_vehicle_gps_position(const struct listener *l) /* copy gps data into local buffer */ orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); + /* GPS COG is 0..2PI in degrees * 1e2 */ + float cog_deg = gps.cog_rad; + if (cog_deg > M_PI_F) + cog_deg -= 2.0f * M_PI_F; + cog_deg *= M_RAD_TO_DEG_F; + + /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps.timestamp_position, @@ -236,13 +243,14 @@ l_vehicle_gps_position(const struct listener *l) gps.lat, gps.lon, gps.alt, - (uint16_t)(gps.eph_m * 1e2f), // from m to cm - (uint16_t)(gps.epv_m * 1e2f), // from m to cm - (uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s - (uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100 + gps.eph_m * 1e2f, // from m to cm + gps.epv_m * 1e2f, // from m to cm + gps.vel_m_s * 1e2f, // from m/s to cm/s + cog_deg * 1e2f, // from rad to deg * 100 gps.satellites_visible); - if (gps.satellite_info_available && (gps_counter % 4 == 0)) { + /* update SAT info every 10 seconds */ + if (gps.satellite_info_available && (gps_counter % 50 == 0)) { mavlink_msg_gps_status_send(MAVLINK_COMM_0, gps.satellites_visible, gps.satellite_prn, From d6e9a35aa2eeda921ad8de93578db2ee69060ef5 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 26 Apr 2013 18:34:12 +0200 Subject: [PATCH 08/18] Update the quad V values. --- apps/systemlib/mixer/mixer_multirotor.cpp | 8 ++++---- apps/systemlib/mixer/multi_tables | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index a45ca3f21a..d79811c0ff 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -83,10 +83,10 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { -0.000000, -1.000000, -1.00 }, }; const MultirotorMixer::Rotor _config_quad_v[] = { - { -0.882948, 0.469472, 1.00 }, - { 0.731354, -0.681998, 1.00 }, - { 0.882948, 0.469472, -1.00 }, - { -0.731354, -0.681998, -1.00 }, + { -0.927184, 0.374607, 1.00 }, + { 0.694658, -0.719340, 1.00 }, + { 0.927184, 0.374607, -1.00 }, + { -0.694658, -0.719340, -1.00 }, }; const MultirotorMixer::Rotor _config_hex_x[] = { { -1.000000, 0.000000, -1.00 }, diff --git a/apps/systemlib/mixer/multi_tables b/apps/systemlib/mixer/multi_tables index 0c5689143b..19a8239a6d 100755 --- a/apps/systemlib/mixer/multi_tables +++ b/apps/systemlib/mixer/multi_tables @@ -21,10 +21,10 @@ set quad_plus { } set quad_v { - 62 CCW - -133 CCW - -62 CW - 133 CW + 68 CCW + -136 CCW + -68 CW + 136 CW } set hex_x { From ff15efb9c91540303622c3f9c48e22bbc9488ae0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 12:59:12 -0700 Subject: [PATCH 09/18] Build utility apps -Os to save ROM space. --- apps/examples/nsh/Makefile | 2 ++ apps/nshlib/Makefile | 2 ++ apps/px4/tests/Makefile | 2 ++ apps/system/i2c/Makefile | 1 + apps/systemcmds/bl_update/Makefile | 2 ++ apps/systemcmds/boardinfo/Makefile | 2 ++ apps/systemcmds/calibration/Makefile | 2 ++ apps/systemcmds/delay_test/Makefile | 2 ++ apps/systemcmds/eeprom/Makefile | 2 ++ apps/systemcmds/mixer/Makefile | 2 ++ apps/systemcmds/param/Makefile | 2 ++ apps/systemcmds/perf/Makefile | 2 ++ apps/systemcmds/preflight_check/Makefile | 2 ++ apps/systemcmds/reboot/Makefile | 2 ++ apps/systemcmds/top/Makefile | 2 ++ 15 files changed, 29 insertions(+) diff --git a/apps/examples/nsh/Makefile b/apps/examples/nsh/Makefile index c7d212fc2c..ad687958f6 100644 --- a/apps/examples/nsh/Makefile +++ b/apps/examples/nsh/Makefile @@ -64,6 +64,8 @@ ROOTDEPPATH = --dep-path . VPATH = +MAXOPTIMIZATION = -Os + all: .built .PHONY: clean depend distclean diff --git a/apps/nshlib/Makefile b/apps/nshlib/Makefile index 76cdac40d5..4256a10918 100644 --- a/apps/nshlib/Makefile +++ b/apps/nshlib/Makefile @@ -107,6 +107,8 @@ endif ROOTDEPPATH = --dep-path . VPATH = +MAXOPTIMIZATION = -Os + # Build targets all: .built diff --git a/apps/px4/tests/Makefile b/apps/px4/tests/Makefile index cb1c3c6185..34f058be44 100644 --- a/apps/px4/tests/Makefile +++ b/apps/px4/tests/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 12000 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/system/i2c/Makefile b/apps/system/i2c/Makefile index 1ed7a2faef..c98e2c0e2b 100644 --- a/apps/system/i2c/Makefile +++ b/apps/system/i2c/Makefile @@ -64,6 +64,7 @@ VPATH = APPNAME = i2c PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 +MAXOPTIMIZATION = -Os # Build targets diff --git a/apps/systemcmds/bl_update/Makefile b/apps/systemcmds/bl_update/Makefile index 9d0e156f60..d05493577a 100644 --- a/apps/systemcmds/bl_update/Makefile +++ b/apps/systemcmds/bl_update/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/boardinfo/Makefile b/apps/systemcmds/boardinfo/Makefile index 753a6843e5..6f1be149c6 100644 --- a/apps/systemcmds/boardinfo/Makefile +++ b/apps/systemcmds/boardinfo/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/calibration/Makefile b/apps/systemcmds/calibration/Makefile index aa1aa77610..a1735962e1 100644 --- a/apps/systemcmds/calibration/Makefile +++ b/apps/systemcmds/calibration/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_MAX - 1 STACKSIZE = 4096 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/delay_test/Makefile b/apps/systemcmds/delay_test/Makefile index d30fcba27b..e54cf2f4e4 100644 --- a/apps/systemcmds/delay_test/Makefile +++ b/apps/systemcmds/delay_test/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/eeprom/Makefile b/apps/systemcmds/eeprom/Makefile index 2f3db0fdce..79a05550ec 100644 --- a/apps/systemcmds/eeprom/Makefile +++ b/apps/systemcmds/eeprom/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/mixer/Makefile b/apps/systemcmds/mixer/Makefile index b016ddc578..3d8ac38cb7 100644 --- a/apps/systemcmds/mixer/Makefile +++ b/apps/systemcmds/mixer/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/param/Makefile b/apps/systemcmds/param/Makefile index 603746a206..f19cadbb60 100644 --- a/apps/systemcmds/param/Makefile +++ b/apps/systemcmds/param/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/perf/Makefile b/apps/systemcmds/perf/Makefile index 0134c9948b..f8bab41b65 100644 --- a/apps/systemcmds/perf/Makefile +++ b/apps/systemcmds/perf/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/preflight_check/Makefile b/apps/systemcmds/preflight_check/Makefile index f138e2640b..98aadaa86f 100644 --- a/apps/systemcmds/preflight_check/Makefile +++ b/apps/systemcmds/preflight_check/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/reboot/Makefile b/apps/systemcmds/reboot/Makefile index 9609a24fd0..15dd199829 100644 --- a/apps/systemcmds/reboot/Makefile +++ b/apps/systemcmds/reboot/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/top/Makefile b/apps/systemcmds/top/Makefile index c45775f4be..f58f9212e7 100644 --- a/apps/systemcmds/top/Makefile +++ b/apps/systemcmds/top/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT - 10 STACKSIZE = 3000 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os From de412b6467378ea5d4e61928f5795f309012a081 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 12:59:35 -0700 Subject: [PATCH 10/18] Pass -g to the link phase for PX4IO the same way we do for FMU --- nuttx/configs/px4io/common/Make.defs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nuttx/configs/px4io/common/Make.defs b/nuttx/configs/px4io/common/Make.defs index 4958f70926..7f782b5b22 100644 --- a/nuttx/configs/px4io/common/Make.defs +++ b/nuttx/configs/px4io/common/Make.defs @@ -112,7 +112,6 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") ARCHOPTIMIZATION += -g -ARCHSCRIPT += -g endif ARCHCFLAGS = -std=gnu99 @@ -145,7 +144,7 @@ ARCHDEFINES = ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 # this seems to be the only way to add linker flags -ARCHSCRIPT += --warn-common \ +EXTRA_LIBS += --warn-common \ --gc-sections CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common From 74c62a131e34d416ef29436b12d78f961e14807a Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 13:00:12 -0700 Subject: [PATCH 11/18] Fix the way that we idle the tone_alarm pin so that the board defines what is the 'safe' state. --- apps/drivers/stm32/tone_alarm/tone_alarm.cpp | 8 ++++---- nuttx/configs/px4fmu/include/board.h | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index baa652d8ad..ac5511e60a 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -494,7 +494,7 @@ ToneAlarm::init() return ret; /* configure the GPIO to the idle state */ - stm32_configgpio(GPIO_TONE_ALARM); + stm32_configgpio(GPIO_TONE_ALARM_IDLE); /* clock/power on our timer */ modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); @@ -606,6 +606,8 @@ ToneAlarm::start_note(unsigned note) rEGR = GTIM_EGR_UG; // force a reload of the period rCCER |= TONE_CCER; // enable the output + // configure the GPIO to enable timer output + stm32_configgpio(GPIO_TONE_ALARM); } void @@ -616,10 +618,8 @@ ToneAlarm::stop_note() /* * Make sure the GPIO is not driving the speaker. - * - * XXX this presumes PX4FMU and the onboard speaker driver FET. */ - stm32_gpiowrite(GPIO_TONE_ALARM, 0); + stm32_configgpio(GPIO_TONE_ALARM_IDLE); } void diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index 8ad56a4c6a..0bc0b30216 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -326,6 +326,7 @@ */ #define TONE_ALARM_TIMER 3 /* timer 3 */ #define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8) #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) /************************************************************************************ From 264cf65d0f6b1259c64aad7d7bb45aff155a8e35 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 13:00:27 -0700 Subject: [PATCH 12/18] Fix an error in a #error --- apps/drivers/stm32/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index bb67d5e6d2..cec0c49ffd 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -125,7 +125,7 @@ # define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC # define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN # if CONFIG_STM32_TIM8 -# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6 +# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8 # endif #elif HRT_TIMER == 9 # define HRT_TIMER_BASE STM32_TIM9_BASE From 46085b43d14b1ea1237aa176460ff648e0ff2d2a Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 13:00:48 -0700 Subject: [PATCH 13/18] Use the I2C bus number from the board config, not a hardcoded value. --- apps/drivers/blinkm/blinkm.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 56265995f3..3fabfd9a54 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -92,6 +92,7 @@ #include +#include #include #include @@ -841,7 +842,7 @@ int blinkm_main(int argc, char *argv[]) { - int i2cdevice = 3; + int i2cdevice = PX4_I2C_BUS_EXPANSION; int blinkmadr = 9; int x; From 5514d6879ab3caae0458fe01da574d7e09b1b447 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 14:09:48 +0200 Subject: [PATCH 14/18] Docs changes --- apps/examples/kalman_demo/Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/examples/kalman_demo/Makefile b/apps/examples/kalman_demo/Makefile index 99c34d934e..6c592d645c 100644 --- a/apps/examples/kalman_demo/Makefile +++ b/apps/examples/kalman_demo/Makefile @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 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 @@ -32,7 +32,7 @@ ############################################################################ # -# Basic example application +# Full attitude / position Extended Kalman Filter # APPNAME = kalman_demo From d9f9ecb084e862bd72528d118c570533deb6eccd Mon Sep 17 00:00:00 2001 From: marco Date: Sat, 27 Apr 2013 14:46:25 +0200 Subject: [PATCH 15/18] BLCtrl 2.0 testing - currently only 8 Bit resolution - ppm added --- apps/drivers/mkblctrl/mkblctrl.cpp | 36 +++++++++++++++++++++++++++--- 1 file changed, 33 insertions(+), 3 deletions(-) diff --git a/apps/drivers/mkblctrl/mkblctrl.cpp b/apps/drivers/mkblctrl/mkblctrl.cpp index c14fdfd1d8..664271bee5 100644 --- a/apps/drivers/mkblctrl/mkblctrl.cpp +++ b/apps/drivers/mkblctrl/mkblctrl.cpp @@ -62,9 +62,9 @@ #include #include #include - #include #include +#include #include #include @@ -76,6 +76,7 @@ #include #include +#include #define I2C_BUS_SPEED 400000 #define UPDATE_RATE 400 @@ -83,7 +84,7 @@ #define BLCTRL_BASE_ADDR 0x29 #define BLCTRL_OLD 0 #define BLCTRL_NEW 1 -#define BLCTRL_MIN_VALUE -0.984F +#define BLCTRL_MIN_VALUE -0.920F #define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_ERROR_MASK 0x7F #define MOTOR_SPINUP_COUNTER 2500 @@ -494,6 +495,14 @@ MK::task_main() fds[0].events = POLLIN; fds[1].fd = _t_armed; fds[1].events = POLLIN; + + // rc input, published to ORB + struct rc_input_values rc_in; + orb_advert_t to_input_rc = 0; + + memset(&rc_in, 0, sizeof(rc_in)); + rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; + log("starting"); long update_rate_in_us = 0; @@ -608,6 +617,27 @@ MK::task_main() ////up_pwm_servo_arm(aa.armed && !aa.lockdown); mk_servo_arm(aa.armed && !aa.lockdown); } + + // see if we have new PPM input data + if (ppm_last_valid_decode != rc_in.timestamp) { + // we have a new PPM frame. Publish it. + rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i=0; i Date: Mon, 29 Apr 2013 18:32:30 +0200 Subject: [PATCH 16/18] BLCtrl 2.0 testing - currently only 8 Bit resolution - motor detection and px4 mode as default --- apps/drivers/mkblctrl/mkblctrl.cpp | 83 ++++++++++++++++++++---------- 1 file changed, 55 insertions(+), 28 deletions(-) diff --git a/apps/drivers/mkblctrl/mkblctrl.cpp b/apps/drivers/mkblctrl/mkblctrl.cpp index 664271bee5..bcdb80b5d0 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; iset_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); } From 130c7a353055da628518f6de1bbd58c855fad5bd Mon Sep 17 00:00:00 2001 From: marco Date: Mon, 29 Apr 2013 19:56:50 +0200 Subject: [PATCH 17/18] BLCtrl 2.0 testing - currently only 8 Bit resolution - motor detection and px4 mode as default - with safety shutdown --- apps/drivers/mkblctrl/mkblctrl.cpp | 42 +++++------------------------- 1 file changed, 6 insertions(+), 36 deletions(-) diff --git a/apps/drivers/mkblctrl/mkblctrl.cpp b/apps/drivers/mkblctrl/mkblctrl.cpp index bcdb80b5d0..057ee2128d 100644 --- a/apps/drivers/mkblctrl/mkblctrl.cpp +++ b/apps/drivers/mkblctrl/mkblctrl.cpp @@ -311,9 +311,6 @@ MK::init(unsigned motors) /* 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, @@ -717,16 +714,13 @@ MK::mk_check_for_blctrl(unsigned int count, unsigned int showOutput) 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(foundMotorCount == 0) { + _task_should_exit = true; + } } return foundMotorCount; - /* - if(foundMotorCount == count) { - return true; - } else { - return false; - } - */ } @@ -1302,12 +1296,12 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, do { if(g_mk->mk_check_for_blctrl(8, 0) != 0) { - shouldStop = 3; + shouldStop = 4; } else { shouldStop++; } sleep(1); - } while ( shouldStop != 3); + } while ( shouldStop < 3); g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, 1)); @@ -1371,31 +1365,7 @@ mkblctrl_main(int argc, char *argv[]) * 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) { From ee4a93d668540dea3b7b33eafb94a888f802f466 Mon Sep 17 00:00:00 2001 From: marco Date: Mon, 29 Apr 2013 20:42:06 +0200 Subject: [PATCH 18/18] BLCtrl 2.0 testing - currently only 8 Bit resolution - motor detection and px4 mode as default - with safety shutdown - fix --- apps/drivers/mkblctrl/mkblctrl.cpp | 89 ++++++++++++++++-------------- 1 file changed, 48 insertions(+), 41 deletions(-) diff --git a/apps/drivers/mkblctrl/mkblctrl.cpp b/apps/drivers/mkblctrl/mkblctrl.cpp index 057ee2128d..e70bd16945 100644 --- a/apps/drivers/mkblctrl/mkblctrl.cpp +++ b/apps/drivers/mkblctrl/mkblctrl.cpp @@ -121,7 +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); + unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput); private: static const unsigned _max_actuators = MAX_MOTORS; @@ -412,47 +412,54 @@ MK::set_frametype(int frametype) int MK::set_motor_count(unsigned count) { - _num_outputs = count; + if(count > 0) { - 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"); + _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)); } + 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)); - } + 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; + } else { - fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n"); - memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4)); + return -1; } - 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; } int @@ -668,7 +675,7 @@ MK::mk_servo_arm(bool status) unsigned int -MK::mk_check_for_blctrl(unsigned int count, unsigned int showOutput) +MK::mk_check_for_blctrl(unsigned int count, bool showOutput) { _retries = 50; uint8_t foundMotorCount = 0; @@ -709,13 +716,13 @@ MK::mk_check_for_blctrl(unsigned int count, unsigned int showOutput) } } - if(showOutput == 1) { + if(showOutput) { fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount); - for(unsigned i=0; i< count; i++) { + for(unsigned i=0; i< foundMotorCount; 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(foundMotorCount == 0) { + if(foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { _task_should_exit = true; } } @@ -1295,7 +1302,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, 0) != 0) { + if(g_mk->mk_check_for_blctrl(8, false) != 0) { shouldStop = 4; } else { shouldStop++; @@ -1303,7 +1310,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, 1)); + g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true)); /* (re)set the PWM output mode */ g_mk->set_mode(servo_mode);