From 09e60893df954fe69a32a7ebfc54fbb715f61197 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 9 May 2013 09:37:42 +0400 Subject: [PATCH 01/11] gpio_led app added --- apps/gpio_led/Makefile | 42 +++++++ apps/gpio_led/gpio_led_main.c | 184 +++++++++++++++++++++++++++++ nuttx/configs/px4fmu/nsh/appconfig | 1 + 3 files changed, 227 insertions(+) create mode 100644 apps/gpio_led/Makefile create mode 100644 apps/gpio_led/gpio_led_main.c diff --git a/apps/gpio_led/Makefile b/apps/gpio_led/Makefile new file mode 100644 index 0000000000..e9dc219ea9 --- /dev/null +++ b/apps/gpio_led/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 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 +# 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. +# +############################################################################ + +# +# Makefile to build gpio_led +# + +APPNAME = gpio_led +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/gpio_led/gpio_led_main.c b/apps/gpio_led/gpio_led_main.c new file mode 100644 index 0000000000..c0ad84da1f --- /dev/null +++ b/apps/gpio_led/gpio_led_main.c @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 gpio_led.c + * + * Drive status LED via GPIO. + * + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static bool thread_should_exit = false; +static bool thread_running = false; + +__EXPORT int gpio_led_main(int argc, char *argv[]); + +static int gpio_led_thread_main(int argc, char *argv[]); + +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: gpio_led {start|stop|status}\n\n"); + exit(1); +} + +int gpio_led_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("gpio_led already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + task_spawn("gpio_led", + SCHED_DEFAULT, + SCHED_PRIORITY_MIN, + 2048, + gpio_led_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tgpio_led is running\n"); + } else { + printf("\tgpio_led not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int gpio_led_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + printf("[gpio_led] started\n"); + + int fd = open(GPIO_DEVICE_PATH, 0); + if (fd < 0) { + printf("[gpio_led] GPIO: open fail\n"); + return ERROR; + } + + /* set GPIO EXT 1 as output */ + ioctl(fd, GPIO_SET_OUTPUT, GPIO_EXT_1); + + ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); + + /* initialize values */ + bool led_state = false; + int counter = 0; + + /* subscribe to vehicle status topic */ + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + while (!thread_should_exit) { + bool status_updated; + orb_check((vehicle_status_sub), &status_updated); + if (status_updated) + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); + + int pattern = 0; + if (status.flag_system_armed) { + if (status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x3f; // ****** solid (armed) + } else { + pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) + } + } else { + if (status.state_machine == SYSTEM_STATE_PREFLIGHT) { + pattern = 0x00; // ______ off (disarmed, preflight check) + } else if (status.state_machine == SYSTEM_STATE_STANDBY && status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x38; // ***___ slow blink (disarmed, ready) + } else { + pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) + } + } + + bool led_state_new = (pattern & (1 << counter)) != 0; + if (led_state_new != led_state) { + led_state = led_state_new; + if (led_state) { + ioctl(fd, GPIO_SET, GPIO_EXT_1); + } else { + ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); + } + } + + counter++; + if (counter > 5) + counter = 0; + + usleep(333333); // sleep ~1/3s + } + + ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); + + printf("[gpio_led] exiting\n"); + + return 0; +} diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 82ab94be78..e0ad9fb1ad 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -103,6 +103,7 @@ CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator CONFIGURED_APPS += attitude_estimator_ekf CONFIGURED_APPS += hott_telemetry +CONFIGURED_APPS += gpio_led endif # Hacking tools From 0489595e7f34f4bacc1b63f31eed5e64fe55c7e6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 9 May 2013 19:44:09 +0400 Subject: [PATCH 02/11] Use work_queue instead of thread --- apps/gpio_led/gpio_led_main.c | 196 +++++++++++++++------------------- 1 file changed, 86 insertions(+), 110 deletions(-) diff --git a/apps/gpio_led/gpio_led_main.c b/apps/gpio_led/gpio_led_main.c index c0ad84da1f..64ede01c31 100644 --- a/apps/gpio_led/gpio_led_main.c +++ b/apps/gpio_led/gpio_led_main.c @@ -45,140 +45,116 @@ #include #include #include -#include +#include +#include #include #include #include #include #include -static bool thread_should_exit = false; -static bool thread_running = false; + +struct gpio_led_s +{ + struct work_s work; + int gpio_fd; + struct vehicle_status_s status; + int vehicle_status_sub; + bool led_state; + int counter; +}; + +static struct gpio_led_s gpio_led; __EXPORT int gpio_led_main(int argc, char *argv[]); -static int gpio_led_thread_main(int argc, char *argv[]); +void gpio_led_start(FAR void *arg); -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: gpio_led {start|stop|status}\n\n"); - exit(1); -} +void gpio_led_cycle(FAR void *arg); int gpio_led_main(int argc, char *argv[]) { - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("gpio_led already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - task_spawn("gpio_led", - SCHED_DEFAULT, - SCHED_PRIORITY_MIN, - 2048, - gpio_led_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); + memset(&gpio_led, 0, sizeof(gpio_led)); + int ret = work_queue(LPWORK, &gpio_led.work, gpio_led_start, &gpio_led, 0); + if (ret != 0) { + printf("[gpio_led] Failed to queue work: %d\n", ret); + exit(1); } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tgpio_led is running\n"); - } else { - printf("\tgpio_led not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); + exit(0); } -int gpio_led_thread_main(int argc, char *argv[]) +void gpio_led_start(FAR void *arg) { - /* welcome user */ - printf("[gpio_led] started\n"); + FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; - int fd = open(GPIO_DEVICE_PATH, 0); - if (fd < 0) { + /* open GPIO device */ + priv->gpio_fd = open(GPIO_DEVICE_PATH, 0); + if (priv->gpio_fd < 0) { printf("[gpio_led] GPIO: open fail\n"); - return ERROR; + return; } - /* set GPIO EXT 1 as output */ - ioctl(fd, GPIO_SET_OUTPUT, GPIO_EXT_1); - - ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); - - /* initialize values */ - bool led_state = false; - int counter = 0; + /* configure GPIO pin */ + ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, GPIO_EXT_1); /* subscribe to vehicle status topic */ - struct vehicle_status_s status; - memset(&status, 0, sizeof(status)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + memset(&priv->status, 0, sizeof(priv->status)); + priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - while (!thread_should_exit) { - bool status_updated; - orb_check((vehicle_status_sub), &status_updated); - if (status_updated) - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); - - int pattern = 0; - if (status.flag_system_armed) { - if (status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { - pattern = 0x3f; // ****** solid (armed) - } else { - pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) - } - } else { - if (status.state_machine == SYSTEM_STATE_PREFLIGHT) { - pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (status.state_machine == SYSTEM_STATE_STANDBY && status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { - pattern = 0x38; // ***___ slow blink (disarmed, ready) - } else { - pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) - } - } - - bool led_state_new = (pattern & (1 << counter)) != 0; - if (led_state_new != led_state) { - led_state = led_state_new; - if (led_state) { - ioctl(fd, GPIO_SET, GPIO_EXT_1); - } else { - ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); - } - } - - counter++; - if (counter > 5) - counter = 0; - - usleep(333333); // sleep ~1/3s + /* add worker to queue */ + int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0); + if (ret != 0) { + printf("[gpio_led] Failed to queue work: %d\n", ret); + return; } - ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); - - printf("[gpio_led] exiting\n"); - - return 0; + printf("[gpio_led] Started\n"); +} + +void gpio_led_cycle(FAR void *arg) +{ + FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; + + /* check for status updates*/ + bool status_updated; + orb_check(priv->vehicle_status_sub, &status_updated); + if (status_updated) + orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + + /* select pattern for current status */ + int pattern = 0; + if (priv->status.flag_system_armed) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x3f; // ****** solid (armed) + } else { + pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) + } + } else { + if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { + pattern = 0x00; // ______ off (disarmed, preflight check) + } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && + priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x38; // ***___ slow blink (disarmed, ready) + } else { + pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) + } + } + + /* blink pattern */ + bool led_state_new = (pattern & (1 << priv->counter)) != 0; + if (led_state_new != priv->led_state) { + priv->led_state = led_state_new; + if (led_state_new) { + ioctl(priv->gpio_fd, GPIO_SET, GPIO_EXT_1); + } else { + ioctl(priv->gpio_fd, GPIO_CLEAR, GPIO_EXT_1); + } + } + + priv->counter++; + if (priv->counter > 5) + priv->counter = 0; + + /* repeat cycle at 5 Hz*/ + work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000)); } From 09ce3e2d0a531138c29e1d32f1a9a902b259683d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 May 2013 11:30:50 +0400 Subject: [PATCH 03/11] Added GPIO_EXT1/GPIO_EXT2 selection. --- src/modules/gpio_led/gpio_led.c | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index b9b066d24a..83b598e922 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -57,13 +57,14 @@ struct gpio_led_s { struct work_s work; int gpio_fd; + int pin; struct vehicle_status_s status; int vehicle_status_sub; bool led_state; int counter; }; -static struct gpio_led_s gpio_led; +static struct gpio_led_s gpio_led_data; __EXPORT int gpio_led_main(int argc, char *argv[]); @@ -73,8 +74,23 @@ void gpio_led_cycle(FAR void *arg); int gpio_led_main(int argc, char *argv[]) { - memset(&gpio_led, 0, sizeof(gpio_led)); - int ret = work_queue(LPWORK, &gpio_led.work, gpio_led_start, &gpio_led, 0); + int pin = GPIO_EXT_1; + if (argc > 1) { + if (!strcmp(argv[1], "-p")) { + if (!strcmp(argv[2], "1")) { + pin = GPIO_EXT_1; + } else if (!strcmp(argv[2], "2")) { + pin = GPIO_EXT_2; + } else { + printf("[gpio_led] Unsupported pin: %s\n", argv[2]); + exit(1); + } + } + } + + memset(&gpio_led_data, 0, sizeof(gpio_led_data)); + gpio_led_data.pin = pin; + int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); if (ret != 0) { printf("[gpio_led] Failed to queue work: %d\n", ret); exit(1); @@ -94,7 +110,7 @@ void gpio_led_start(FAR void *arg) } /* configure GPIO pin */ - ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, GPIO_EXT_1); + ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); /* subscribe to vehicle status topic */ memset(&priv->status, 0, sizeof(priv->status)); @@ -107,7 +123,7 @@ void gpio_led_start(FAR void *arg) return; } - printf("[gpio_led] Started\n"); + printf("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin); } void gpio_led_cycle(FAR void *arg) @@ -144,9 +160,9 @@ void gpio_led_cycle(FAR void *arg) if (led_state_new != priv->led_state) { priv->led_state = led_state_new; if (led_state_new) { - ioctl(priv->gpio_fd, GPIO_SET, GPIO_EXT_1); + ioctl(priv->gpio_fd, GPIO_SET, priv->pin); } else { - ioctl(priv->gpio_fd, GPIO_CLEAR, GPIO_EXT_1); + ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); } } From 6e8621269bc032afdb77830cebac01808299263d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 May 2013 13:59:51 +0400 Subject: [PATCH 04/11] Code style fixed --- src/modules/gpio_led/gpio_led.c | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 83b598e922..a80bf9cb83 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -53,8 +53,7 @@ #include #include -struct gpio_led_s -{ +struct gpio_led_s { struct work_s work; int gpio_fd; int pin; @@ -75,12 +74,15 @@ void gpio_led_cycle(FAR void *arg); int gpio_led_main(int argc, char *argv[]) { int pin = GPIO_EXT_1; + if (argc > 1) { if (!strcmp(argv[1], "-p")) { if (!strcmp(argv[2], "1")) { pin = GPIO_EXT_1; + } else if (!strcmp(argv[2], "2")) { pin = GPIO_EXT_2; + } else { printf("[gpio_led] Unsupported pin: %s\n", argv[2]); exit(1); @@ -91,10 +93,12 @@ int gpio_led_main(int argc, char *argv[]) memset(&gpio_led_data, 0, sizeof(gpio_led_data)); gpio_led_data.pin = pin; int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); + if (ret != 0) { printf("[gpio_led] Failed to queue work: %d\n", ret); exit(1); } + exit(0); } @@ -104,6 +108,7 @@ void gpio_led_start(FAR void *arg) /* open GPIO device */ priv->gpio_fd = open(GPIO_DEVICE_PATH, 0); + if (priv->gpio_fd < 0) { printf("[gpio_led] GPIO: open fail\n"); return; @@ -118,6 +123,7 @@ void gpio_led_start(FAR void *arg) /* add worker to queue */ int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0); + if (ret != 0) { printf("[gpio_led] Failed to queue work: %d\n", ret); return; @@ -133,23 +139,29 @@ void gpio_led_cycle(FAR void *arg) /* check for status updates*/ bool status_updated; orb_check(priv->vehicle_status_sub, &status_updated); + if (status_updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); /* select pattern for current status */ int pattern = 0; + if (priv->status.flag_system_armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) + } else { pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) } + } else { if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { pattern = 0x00; // ______ off (disarmed, preflight check) + } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && - priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) + } else { pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) } @@ -157,16 +169,20 @@ void gpio_led_cycle(FAR void *arg) /* blink pattern */ bool led_state_new = (pattern & (1 << priv->counter)) != 0; + if (led_state_new != priv->led_state) { priv->led_state = led_state_new; + if (led_state_new) { ioctl(priv->gpio_fd, GPIO_SET, priv->pin); + } else { ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); } } priv->counter++; + if (priv->counter > 5) priv->counter = 0; From 9f090e651a2f9bc7c3c63022a6ff26453b465b67 Mon Sep 17 00:00:00 2001 From: marco Date: Thu, 23 May 2013 21:03:49 +0200 Subject: [PATCH 05/11] mkblctrl cleanup --- src/drivers/mkblctrl/mkblctrl.cpp | 519 ++++++++++++++++-------------- 1 file changed, 278 insertions(+), 241 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3a735e26fb..f6e4e0ed3e 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -76,7 +76,6 @@ #include #include -#include #define I2C_BUS_SPEED 400000 #define UPDATE_RATE 400 @@ -114,6 +113,7 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual int init(unsigned motors); + virtual ssize_t write(file *filp, const char *buffer, size_t len); int set_mode(Mode mode); int set_pwm_rate(unsigned rate); @@ -177,9 +177,10 @@ private: int gpio_ioctl(file *filp, int cmd, unsigned long arg); int mk_servo_arm(bool status); - int mk_servo_set(unsigned int chan, float val); - int mk_servo_set_test(unsigned int chan, float val); + int mk_servo_set(unsigned int chan, short val); + int mk_servo_set_value(unsigned int chan, short val); int mk_servo_test(unsigned int chan); + short scaling(float val, float inMin, float inMax, float outMin, float outMax); }; @@ -207,20 +208,20 @@ const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 }; // Addresstranslat const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0}; -int addrTranslator[] = {0,0,0,0,0,0,0,0}; +int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0}; -struct MotorData_t -{ +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 - unsigned int RoundCount; + 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 + unsigned short RawPwmValue; // length of PWM pulse + // 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 + unsigned int RoundCount; }; MotorData_t Motor[MAX_MOTORS]; @@ -314,7 +315,7 @@ MK::init(unsigned motors) /* start the IO interface task */ _task = task_spawn("mkblctrl", SCHED_DEFAULT, - SCHED_PRIORITY_MAX -20, + SCHED_PRIORITY_MAX - 20, 2048, (main_t)&MK::task_main_trampoline, nullptr); @@ -346,27 +347,11 @@ MK::set_mode(Mode mode) */ 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; @@ -412,45 +397,55 @@ MK::set_frametype(int frametype) int MK::set_motor_count(unsigned count) { - if(count > 0) { + if (count > 0) { _num_outputs = count; - if(_px4mode == MAPPING_MK) { - if(_frametype == FRAME_PLUS) { + if (_px4mode == MAPPING_MK) { + if (_frametype == FRAME_PLUS) { fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n"); - } else if(_frametype == FRAME_X) { + + } else if (_frametype == FRAME_X) { fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n"); } - if(_num_outputs == 4) { - if(_frametype == FRAME_PLUS) { + + if (_num_outputs == 4) { + if (_frametype == FRAME_PLUS) { memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus)); - } else if(_frametype == FRAME_X) { + + } else if (_frametype == FRAME_X) { memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x)); } - } else if(_num_outputs == 6) { - if(_frametype == FRAME_PLUS) { + + } else if (_num_outputs == 6) { + if (_frametype == FRAME_PLUS) { memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus)); - } else if(_frametype == FRAME_X) { + + } else if (_frametype == FRAME_X) { memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x)); } - } else if(_num_outputs == 8) { - if(_frametype == FRAME_PLUS) { + + } else if (_num_outputs == 8) { + if (_frametype == FRAME_PLUS) { memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus)); - } else if(_frametype == FRAME_X) { + + } 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 (_num_outputs == 4) { fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n"); - } else if(_num_outputs == 6) { + + } else if (_num_outputs == 6) { fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n"); - } else if(_num_outputs == 8) { + + } else if (_num_outputs == 8) { fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n"); } @@ -469,16 +464,35 @@ MK::set_motor_test(bool motortest) return OK; } +short +MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) +{ + short retVal = 0; + + retVal = (val - inMin) / (inMax - inMin) * (outMax - outMin) + outMin; + + if (retVal < outMin) { + retVal = outMin; + + } else if (retVal > outMax) { + retVal = outMax; + } + + return retVal; +} void MK::task_main() { + long update_rate_in_us = 0; + float tmpVal = 0; + /* * 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)); + ORB_ID(actuator_controls_0)); /* force a reset of the update rate */ _current_update_rate = 0; @@ -488,16 +502,11 @@ MK::task_main() /* 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; @@ -505,15 +514,7 @@ MK::task_main() 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; /* loop until killed */ while (!_task_should_exit) { @@ -528,6 +529,7 @@ MK::task_main() 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; @@ -539,8 +541,9 @@ MK::task_main() _current_update_rate = _update_rate; } - /* sleep waiting for data, but no more than a second */ - int ret = ::poll(&fds[0], 2, 1000); + /* sleep waiting for data, stopping to check for PPM + * input at 100Hz */ + int ret = ::poll(&fds[0], 2, 100); /* this would be bad... */ if (ret < 0) { @@ -553,7 +556,7 @@ MK::task_main() 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); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_0), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { @@ -565,53 +568,52 @@ MK::task_main() // 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) { + 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]); + /* nothing to do here */ } 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) { + if (outputs.output[i] < -1.0f) { outputs.output[i] = -1.0f; - } else if(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) { + 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 - } + /* output to BLCtrl's */ + if (_motortest == true) { + mk_servo_test(i); + + } else { + mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine + } } - /* 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? */ @@ -622,29 +624,9 @@ MK::task_main() 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); } - // 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 2047) { - tmpVal = 2047; + if (tmpVal > 1024) { + tmpVal = 1024; + + } else if (tmpVal < 0) { + tmpVal = 0; } + Motor[chan].SetPoint = (uint8_t)(tmpVal / 4); + //Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4; - 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) { + 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])); + 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;; - 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 - } + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error } + Motor[chan].RoundCount = 0; + } 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 (OK != transfer(&msg[0], 1, nullptr, 0)) { + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error } - - 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++; + } 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) { + if (showDebug == true) { debugCounter++; - if(debugCounter == 2000) { + + if (debugCounter == 2000) { debugCounter = 0; - for(int i=0; i<_num_outputs; i++){ - if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) { + + 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) +MK::mk_servo_set_value(unsigned int chan, short val) { _retries = 0; int ret; + short tmpVal = 0; + uint8_t msg[2] = { 0, 0 }; - float tmpVal = 0; + tmpVal = val; - uint8_t msg[2] = { 0,0 }; + if (tmpVal > 1024) { + tmpVal = 1024; - tmpVal = (1023 + (1023 * val)); - if(tmpVal > 2048) { - tmpVal = 2048; + } else if (tmpVal < 0) { + tmpVal = 0; } - Motor[chan].SetPoint = (uint8_t) (tmpVal / 8); + Motor[chan].SetPoint = (uint8_t)(tmpVal / 4); - if(_armed == false) { + if (_armed == false) { Motor[chan].SetPoint = 0; Motor[chan].SetPointLowerBits = 0; } @@ -860,7 +860,6 @@ MK::mk_servo_set_test(unsigned int chan, float val) ret = transfer(&msg[0], 1, nullptr, 0); ret = OK; - return ret; } @@ -868,59 +867,61 @@ MK::mk_servo_set_test(unsigned int chan, float val) int MK::mk_servo_test(unsigned int chan) { - int ret=0; + int ret = 0; float tmpVal = 0; float val = -1; _retries = 0; - uint8_t msg[2] = { 0,0 }; + uint8_t msg[2] = { 0, 0 }; - if(debugCounter >= MOTOR_SPINUP_COUNTER) { + if (debugCounter >= MOTOR_SPINUP_COUNTER) { debugCounter = 0; _motor++; - if(_motor < _num_outputs) { + if (_motor < _num_outputs) { fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor); } - if(_motor >= _num_outputs) { + if (_motor >= _num_outputs) { _motor = -1; _motortest = false; } - } + debugCounter++; - if(_motor == chan) { + if (_motor == chan) { val = BLCTRL_MIN_VALUE; + } else { val = -1; } - tmpVal = (1023 + (1023 * val)); - if(tmpVal > 2048) { - tmpVal = 2048; + tmpVal = (511 + (511 * val)); + + if (tmpVal > 1024) { + tmpVal = 1024; } - //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; + Motor[chan].SetPoint = (uint8_t)(tmpVal / 4); - if(_motor != chan) { + if (_motor != chan) { Motor[chan].SetPoint = 0; Motor[chan].SetPointLowerBits = 0; } - if(Motor[chan].Version == BLCTRL_OLD) { + 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) { + + if (Motor[chan].Version == BLCTRL_OLD) { ret = transfer(&msg[0], 1, nullptr, 0); + } else { ret = transfer(&msg[0], 2, nullptr, 0); } @@ -931,9 +932,9 @@ MK::mk_servo_test(unsigned int chan) int MK::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -947,7 +948,6 @@ 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); @@ -978,32 +978,37 @@ 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_SET_ARM_OK: + case PWM_SERVO_CLEAR_ARM_OK: + // these are no-ops, as no safety switch + 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); + ret = OK; + break; + + case PWM_SERVO_SELECT_UPDATE_RATE: + ret = OK; break; case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): + if (arg < 2150) { + Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg; + mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024)); - /* 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; } @@ -1012,20 +1017,20 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) 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); + *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue; + break; + case PWM_SERVO_GET_RATEGROUP(0): + case PWM_SERVO_GET_RATEGROUP(1): + case PWM_SERVO_GET_RATEGROUP(2): + case PWM_SERVO_GET_RATEGROUP(3): + //*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); + break; + + case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: - /* - if (_mode == MODE_4PWM) { - *(unsigned *)arg = 4; - } else { - *(unsigned *)arg = 2; - } - */ - *(unsigned *)arg = _num_outputs; - break; case MIXERIOCRESET: @@ -1078,6 +1083,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = -EINVAL; } } + break; } @@ -1091,6 +1097,32 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) return ret; } +/* + this implements PWM output via a write() method, for compatibility + with px4io + */ +ssize_t +MK::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + uint16_t values[4]; + + if (count > 4) { + // we only have 4 PWM outputs on the FMU + count = 4; + } + + // allow for misaligned values + memcpy(values, buffer, count * 2); + + for (uint8_t i = 0; i < count; i++) { + Motor[i].RawPwmValue = (unsigned short)values[i]; + mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024)); + } + + return count * 2; +} + void MK::gpio_reset(void) { @@ -1229,10 +1261,10 @@ enum MappingMode { MAPPING_PX4, }; - enum FrameType { - FRAME_PLUS = 0, - FRAME_X, - }; +enum FrameType { + FRAME_PLUS = 0, + FRAME_X, +}; PortMode g_port_mode; @@ -1297,18 +1329,17 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, g_mk->set_motor_test(motortest); - /* (re)set count of used motors */ - ////g_mk->set_motor_count(motorcount); /* count used motors */ - do { - if(g_mk->mk_check_for_blctrl(8, false) != 0) { + if (g_mk->mk_check_for_blctrl(8, false) != 0) { shouldStop = 4; + } else { shouldStop++; } + sleep(1); - } while ( shouldStop < 3); + } while (shouldStop < 3); g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true)); @@ -1375,7 +1406,8 @@ mkblctrl_main(int argc, char *argv[]) if (argc > i + 1) { bus = atoi(argv[i + 1]); newMode = true; - } else { + + } else { errx(1, "missing argument for i2c bus (-b)"); return 1; } @@ -1384,17 +1416,21 @@ mkblctrl_main(int argc, char *argv[]) /* look for the optional frame parameter */ 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) { + if (strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) { px4mode = MAPPING_MK; newMode = true; - if(strcmp(argv[i + 1], "+") == 0) { + + if (strcmp(argv[i + 1], "+") == 0) { frametype = FRAME_PLUS; + } else { frametype = FRAME_X; } + } else { errx(1, "only + or x for frametype supported !"); } + } else { errx(1, "missing argument for mkmode (-mkmode)"); return 1; @@ -1409,12 +1445,12 @@ mkblctrl_main(int argc, char *argv[]) /* look for the optional -h --help parameter */ if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { - showHelp == true; + showHelp = true; } } - if(showHelp) { + if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n"); exit(1); @@ -1424,6 +1460,7 @@ mkblctrl_main(int argc, char *argv[]) if (g_mk == nullptr) { if (mk_start(bus, motorcount) != OK) { errx(1, "failed to start the MK-BLCtrl driver"); + } else { newMode = true; } From 8e1571cf0230ff31cbbb1af64a793c3957827cc2 Mon Sep 17 00:00:00 2001 From: marco Date: Fri, 24 May 2013 20:16:47 +0200 Subject: [PATCH 06/11] mkblctrl cleanup & tested --- src/drivers/mkblctrl/mkblctrl.cpp | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index f6e4e0ed3e..4220f552e4 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -491,8 +491,10 @@ 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_0)); + // loeschen _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + // loeschen ORB_ID(actuator_controls_0)); + _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + /* force a reset of the update rate */ _current_update_rate = 0; @@ -502,11 +504,19 @@ MK::task_main() /* 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; @@ -556,7 +566,8 @@ MK::task_main() 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_0), _t_actuators, &_controls); + // loeschen orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_0), _t_actuators, &_controls); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { @@ -1105,13 +1116,19 @@ ssize_t MK::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - uint16_t values[4]; + // loeschen uint16_t values[4]; + uint16_t values[8]; - if (count > 4) { - // we only have 4 PWM outputs on the FMU - count = 4; + // loeschen if (count > 4) { + // loeschen // we only have 4 PWM outputs on the FMU + // loeschen count = 4; + // loeschen } + if (count > _num_outputs) { + // we only have 8 I2C outputs in the driver + count = _num_outputs; } + // allow for misaligned values memcpy(values, buffer, count * 2); From 1edc36bfd494a3a8bff967592774ce75ca4ce151 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 May 2013 23:01:55 +0200 Subject: [PATCH 07/11] More documentation --- src/examples/fixedwing_control/main.c | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 9fd11062a7..89fbef020a 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -33,10 +33,13 @@ ****************************************************************************/ /** * @file main.c - * Implementation of a fixed wing attitude controller. This file is a complete - * fixed wing controller flying manual attitude control or auto waypoint control. + * + * Example implementation of a fixed wing attitude controller. This file is a complete + * fixed wing controller for manual attitude control or auto waypoint control. * There is no need to touch any other system components to extend / modify the * complete control architecture. + * + * @author Lorenz Meier */ #include @@ -60,7 +63,6 @@ #include #include #include -#include #include #include #include @@ -306,7 +308,10 @@ int fixedwing_control_thread_main(int argc, char *argv[]) int ret = poll(fds, 2, 500); if (ret < 0) { - /* poll error, this will not really happen in practice */ + /* + * Poll error, this will not really happen in practice, + * but its good design practice to make output an error message. + */ warnx("poll error"); } else if (ret == 0) { @@ -332,6 +337,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) orb_check(global_pos_sub, &pos_updated); bool global_sp_updated; orb_check(global_sp_sub, &global_sp_updated); + bool manual_sp_updated; + orb_check(manual_sp_sub, &manual_sp_updated); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -357,10 +364,11 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } } - /* get the RC (or otherwise user based) input */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + if (manual_sp_updated) + /* get the RC (or otherwise user based) input */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - /* check if the throttle was ever more than 50% - go only to failsafe if yes */ + /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ if (isfinite(manual_sp.throttle) && (manual_sp.throttle >= 0.6f) && (manual_sp.throttle <= 1.0f)) { @@ -372,6 +380,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* control */ + /* if in auto mode, fly global position setpoint */ if (vstatus.state_machine == SYSTEM_STATE_AUTO || vstatus.state_machine == SYSTEM_STATE_STABILIZED) { @@ -391,6 +400,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* set flaps to zero */ actuators.control[4] = 0.0f; + /* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */ } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { From 73d2baeb20d3c7757c5aca59e6a66bb4d5bb3533 Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 26 May 2013 16:49:33 +0200 Subject: [PATCH 08/11] comments cleaned --- src/drivers/mkblctrl/mkblctrl.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 4220f552e4..c67276f8af 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -491,8 +491,6 @@ MK::task_main() * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. */ - // loeschen _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - // loeschen ORB_ID(actuator_controls_0)); _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); /* force a reset of the update rate */ @@ -551,8 +549,7 @@ MK::task_main() _current_update_rate = _update_rate; } - /* sleep waiting for data, stopping to check for PPM - * input at 100Hz */ + /* sleep waiting for data max 100ms */ int ret = ::poll(&fds[0], 2, 100); /* this would be bad... */ @@ -566,7 +563,6 @@ MK::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - // loeschen orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_0), _t_actuators, &_controls); orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); /* can we mix? */ From eab01a2efd0c1f1fc9cf32181c63a7e5494f0004 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 26 May 2013 20:51:20 +0200 Subject: [PATCH 09/11] Hotfix: Generate map files for modules as well for more in-depth memory-use debugging. --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index eeba0ff2c6..c75a08bd16 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -219,7 +219,7 @@ endef define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 + $(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1 endef # Update the archive $1 with the files in $2 From f1a8f6e75b98768daa5bc5290ccf60156d8185da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 May 2013 16:58:03 +0200 Subject: [PATCH 10/11] Hotfix: Made HMC driver more verbose to prevent false alarm --- src/drivers/hmc5883/hmc5883.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 78eda327c3..7de394f244 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1224,10 +1224,12 @@ start() errx(1, "already started"); /* create the driver, attempt expansion bus first */ + warnx("probing for external sensor.."); g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION); if (g_dev != nullptr && OK != g_dev->init()) { delete g_dev; g_dev = nullptr; + warnx("no external sensor, using internal.."); } From 27ee36b2049167a1272122548fe61aa2993d79c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 May 2013 07:18:07 +0200 Subject: [PATCH 11/11] Hotfix: Completely silencing HMC5883 probing to not confuse users --- src/drivers/hmc5883/hmc5883.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 7de394f244..59e90d86c1 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -329,7 +329,7 @@ HMC5883::HMC5883(int bus) : _calibrated(false) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // default scaling _scale.x_offset = 0; @@ -1224,12 +1224,10 @@ start() errx(1, "already started"); /* create the driver, attempt expansion bus first */ - warnx("probing for external sensor.."); g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION); if (g_dev != nullptr && OK != g_dev->init()) { delete g_dev; g_dev = nullptr; - warnx("no external sensor, using internal.."); }