From 09e60893df954fe69a32a7ebfc54fbb715f61197 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 9 May 2013 09:37:42 +0400 Subject: 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 000000000..e9dc219ea --- /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 000000000..c0ad84da1 --- /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 82ab94be7..e0ad9fb1a 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 -- cgit v1.2.3 From 0489595e7f34f4bacc1b63f31eed5e64fe55c7e6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 9 May 2013 19:44:09 +0400 Subject: Use work_queue instead of thread --- apps/gpio_led/gpio_led_main.c | 186 ++++++++++++++++++------------------------ 1 file changed, 81 insertions(+), 105 deletions(-) diff --git a/apps/gpio_led/gpio_led_main.c b/apps/gpio_led/gpio_led_main.c index c0ad84da1..64ede01c3 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; -__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[]) +struct gpio_led_s { - if (argc < 1) - usage("missing command"); + struct work_s work; + int gpio_fd; + struct vehicle_status_s status; + int vehicle_status_sub; + bool led_state; + int counter; +}; - if (!strcmp(argv[1], "start")) { +static struct gpio_led_s gpio_led; - if (thread_running) { - printf("gpio_led already running\n"); - /* this is not an error */ - exit(0); - } +__EXPORT int gpio_led_main(int argc, char *argv[]); - 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); - } +void gpio_led_start(FAR void *arg); - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } +void gpio_led_cycle(FAR void *arg); - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tgpio_led is running\n"); - } else { - printf("\tgpio_led not started\n"); - } - exit(0); +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); + if (ret != 0) { + printf("[gpio_led] Failed to queue work: %d\n", ret); + exit(1); } - - 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); + /* configure GPIO pin */ + ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, GPIO_EXT_1); - ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); + /* subscribe to vehicle status topic */ + memset(&priv->status, 0, sizeof(priv->status)); + priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* 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; + } - /* initialize values */ - bool led_state = false; - int counter = 0; + printf("[gpio_led] Started\n"); +} - /* 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) - } +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 { - 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) - } + pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) } - - 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); - } + } 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) } - - counter++; - if (counter > 5) - counter = 0; - - usleep(333333); // sleep ~1/3s } - ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); + /* 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); + } + } - printf("[gpio_led] exiting\n"); + priv->counter++; + if (priv->counter > 5) + priv->counter = 0; - return 0; + /* repeat cycle at 5 Hz*/ + work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000)); } -- cgit v1.2.3 From 09ce3e2d0a531138c29e1d32f1a9a902b259683d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 May 2013 11:30:50 +0400 Subject: 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 b9b066d24..83b598e92 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); } } -- cgit v1.2.3 From 6e8621269bc032afdb77830cebac01808299263d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 May 2013 13:59:51 +0400 Subject: 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 83b598e92..a80bf9cb8 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; -- cgit v1.2.3 From 9f090e651a2f9bc7c3c63022a6ff26453b465b67 Mon Sep 17 00:00:00 2001 From: marco Date: Thu, 23 May 2013 21:03:49 +0200 Subject: mkblctrl cleanup --- src/drivers/mkblctrl/mkblctrl.cpp | 507 ++++++++++++++++++++------------------ 1 file changed, 272 insertions(+), 235 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3a735e26f..f6e4e0ed3 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) { + if (_motortest == true) { mk_servo_test(i); + } else { - //mk_servo_set(i, outputs.output[i]); - mk_servo_set_test(i, outputs.output[i]); // 8Bit + 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])); - - 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; + 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 (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 } + } + + } 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]; - 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 - } + 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++; + } + + 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 MIXERIOCGETOUTPUTCOUNT: - /* - if (_mode == MODE_4PWM) { - *(unsigned *)arg = 4; - } else { - *(unsigned *)arg = 2; - } - */ + 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: *(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; } -- cgit v1.2.3 From 8e1571cf0230ff31cbbb1af64a793c3957827cc2 Mon Sep 17 00:00:00 2001 From: marco Date: Fri, 24 May 2013 20:16:47 +0200 Subject: mkblctrl cleanup & tested --- src/drivers/mkblctrl/mkblctrl.cpp | 33 +++++++++++++++++++++++++-------- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index f6e4e0ed3..4220f552e 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]; - - if (count > 4) { - // we only have 4 PWM outputs on the FMU - count = 4; + // loeschen uint16_t values[4]; + uint16_t values[8]; + + // 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); -- cgit v1.2.3 From 214ddd6f1ca12bf52d533aba791877d9cdfe6345 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 May 2013 18:16:15 +0200 Subject: Adjusted example params and extensively commented example --- src/examples/fixedwing_control/main.c | 97 +++++++++++++++++++++++++++------ src/examples/fixedwing_control/params.c | 2 +- 2 files changed, 80 insertions(+), 19 deletions(-) diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 6a9ad9e1d..a1b9c78f9 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -73,8 +73,15 @@ #include "params.h" /* Prototypes */ + /** * Daemon management function. + * + * This function allows to start / stop the background task (daemon). + * The purpose of it is to be able to start the controller on the + * command line, query its status and stop it, without giving up + * the command line to one particular process or the need for bg/fg + * ^Z support by the shell. */ __EXPORT int ex_fixedwing_control_main(int argc, char *argv[]); @@ -88,10 +95,34 @@ int fixedwing_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); +/** + * Control roll and pitch angle. + * + * This very simple roll and pitch controller takes the current roll angle + * of the system and compares it to a reference. Pitch is controlled to zero and yaw remains + * uncontrolled (tutorial code, not intended for flight). + * + * @param att_sp The current attitude setpoint - the values the system would like to reach. + * @param att The current attitude. The controller should make the attitude match the setpoint + * @param speed_body The velocity of the system. Currently unused. + * @param rates_sp The angular rate setpoint. This is the output of the controller. + */ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, + float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); +/** + * Control heading. + * + * This very simple heading to roll angle controller outputs the desired roll angle based on + * the current position of the system, the desired position (the setpoint) and the current + * heading. + * + * @param pos The current position of the system + * @param sp The current position setpoint + * @param att The current attitude + * @param att_sp The attitude setpoint. This is the output of the controller + */ void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp, const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); @@ -103,7 +134,7 @@ static struct params p; static struct param_handles ph; void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, + float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { @@ -148,7 +179,10 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v * Calculate heading error of current position to desired position */ - /* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */ + /* + * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution, + * so they need to be scaled by 1e7 and converted to IEEE double precision floating point. + */ float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d); /* calculate heading error */ @@ -157,10 +191,10 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v float roll_command = yaw_err * p.hdng_p; /* limit output, this commonly is a tuning parameter, too */ - if (att_sp->roll_body < -0.5f) { - att_sp->roll_body = -0.5f; - } else if (att_sp->roll_body > 0.5f) { - att_sp->roll_body = 0.5f; + if (att_sp->roll_body < -0.6f) { + att_sp->roll_body = -0.6f; + } else if (att_sp->roll_body > 0.6f) { + att_sp->roll_body = 0.6f; } } @@ -183,7 +217,32 @@ int fixedwing_control_thread_main(int argc, char *argv[]) parameters_init(&ph); parameters_update(&ph, &p); - /* declare and safely initialize all structs to zero */ + + /* + * PX4 uses a publish/subscribe design pattern to enable + * multi-threaded communication. + * + * The most elegant aspect of this is that controllers and + * other processes can either 'react' to new data, or run + * at their own pace. + * + * PX4 developer guide: + * https://pixhawk.ethz.ch/px4/dev/shared_object_communication + * + * Wikipedia description: + * http://en.wikipedia.org/wiki/Publish–subscribe_pattern + * + */ + + + + + /* + * Declare and safely initialize all structs to zero. + * + * These structs contain the system state and things + * like attitude, position, the current waypoint, etc. + */ struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -199,20 +258,24 @@ int fixedwing_control_thread_main(int argc, char *argv[]) struct vehicle_global_position_setpoint_s global_sp; memset(&global_sp, 0, sizeof(global_sp)); - /* output structs */ + /* output structs - this is what is sent to the mixer */ struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); - /* publish actuator controls */ + /* publish actuator controls with zero values */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } + /* + * Advertise that this controller will publish actuator + * control values and the rate setpoint + */ orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - /* subscribe */ + /* subscribe to topics. */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -222,7 +285,6 @@ int fixedwing_control_thread_main(int argc, char *argv[]) int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ - float gyro[3] = {0.0f, 0.0f, 0.0f}; float speed_body[3] = {0.0f, 0.0f, 0.0f}; struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, { .fd = att_sub, .events = POLLIN }}; @@ -275,6 +337,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (global_sp_updated) orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp); + /* currently speed in body frame is not used, but here for reference */ if (pos_updated) { orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); @@ -292,13 +355,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); + /* get the system status and the flight mode we're in */ orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - /* control */ if (vstatus.state_machine == SYSTEM_STATE_AUTO || @@ -312,7 +373,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) actuators.control[2] = 0.0f; /* simple attitude control */ - control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators); + control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators); /* pass through throttle */ actuators.control[3] = att_sp.thrust; @@ -355,7 +416,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) att_sp.timestamp = hrt_absolute_time(); /* attitude control */ - control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators); + control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators); /* pass through throttle */ actuators.control[3] = att_sp.thrust; diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c index 8042c74f5..c2e94ff3d 100644 --- a/src/examples/fixedwing_control/params.c +++ b/src/examples/fixedwing_control/params.c @@ -45,7 +45,7 @@ /** * */ -PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f); +PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.1f); /** * -- cgit v1.2.3 From bc7a7167ae955a810299831a8504bac7c9cd60fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 May 2013 18:21:39 +0200 Subject: Go only to RC failsafe if throttle was half once - to prevent failsafe when armed on ground --- src/examples/fixedwing_control/main.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index a1b9c78f9..9fd11062a 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -286,6 +286,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* Setup of loop */ float speed_body[3] = {0.0f, 0.0f, 0.0f}; + /* RC failsafe check */ + bool throttle_half_once = false; struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, { .fd = att_sub, .events = POLLIN }}; @@ -357,6 +359,14 @@ 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); + + /* check if the throttle was ever more than 50% - go only to failsafe if yes */ + if (isfinite(manual_sp.throttle) && + (manual_sp.throttle >= 0.6f) && + (manual_sp.throttle <= 1.0f)) { + throttle_half_once = true; + } + /* get the system status and the flight mode we're in */ orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); @@ -385,7 +395,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost) { + if (vstatus.rc_signal_lost && throttle_half_once) { /* put plane into loiter */ att_sp.roll_body = 0.3f; -- cgit v1.2.3 From 1edc36bfd494a3a8bff967592774ce75ca4ce151 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 May 2013 23:01:55 +0200 Subject: 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 9fd11062a..89fbef020 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) { -- cgit v1.2.3 From 73d2baeb20d3c7757c5aca59e6a66bb4d5bb3533 Mon Sep 17 00:00:00 2001 From: marco Date: Sun, 26 May 2013 16:49:33 +0200 Subject: 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 4220f552e..c67276f8a 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? */ -- cgit v1.2.3 From eab01a2efd0c1f1fc9cf32181c63a7e5494f0004 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 26 May 2013 20:51:20 +0200 Subject: 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 eeba0ff2c..c75a08bd1 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 -- cgit v1.2.3 From f1a8f6e75b98768daa5bc5290ccf60156d8185da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 May 2013 16:58:03 +0200 Subject: 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 78eda327c..7de394f24 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.."); } -- cgit v1.2.3 From 27ee36b2049167a1272122548fe61aa2993d79c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 May 2013 07:18:07 +0200 Subject: 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 7de394f24..59e90d86c 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.."); } -- cgit v1.2.3