From 6f572637de44b364ff0bec3f3947fb73de74e88b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 30 Nov 2012 14:26:03 -0800 Subject: [PATCH 1/9] Fixed missing heading for mtkcustom and nmea --- apps/gps/mtk.c | 2 +- apps/gps/nmea_helper.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c index 7ba4f52b0a..3b0ee45657 100644 --- a/apps/gps/mtk.c +++ b/apps/gps/mtk.c @@ -128,7 +128,7 @@ int mtk_parse(uint8_t b, char *gps_rx_buffer) mtk_gps->eph = packet->hdop; mtk_gps->epv = 65535; //unknown in mtk custom mode mtk_gps->vel = packet->ground_speed; - mtk_gps->cog = 65535; //unknown in mtk custom mode + mtk_gps->cog = (uint16_t)packet->heading; //mtk: degrees *1e2, mavlink/ubx: degrees *1e2 mtk_gps->satellites_visible = packet->satellites; /* convert time and date information to unix timestamp */ diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c index 577a3a01cc..1a50371c11 100644 --- a/apps/gps/nmea_helper.c +++ b/apps/gps/nmea_helper.c @@ -214,7 +214,7 @@ void *nmea_loop(void *args) nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling nmea_gps->epv = (uint16_t)(info->VDOP * 100); //TODO:test scaling nmea_gps->vel = (uint16_t)(info->speed * 1000 / 36); //*1000/3600*100 - nmea_gps->cog = 65535; + nmea_gps->cog = (uint16_t)info->direction*100; //nmea: degrees float, ubx/mavlink: degrees*1e2 nmea_gps->satellites_visible = (uint8_t)info->satinfo.inview; int i = 0; From 404332aefe5e2ad3d0766f99bd8d8efebdd4d331 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 1 Dec 2012 14:55:48 -0800 Subject: [PATCH 2/9] Break out the task stack usage sniffer so we can use it on systems where the task layout has been shrunk (e.g. PX4IO) --- Debug/NuttX | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/Debug/NuttX b/Debug/NuttX index 8257335547..8e65448427 100644 --- a/Debug/NuttX +++ b/Debug/NuttX @@ -168,17 +168,29 @@ define _showsemaphore printf "\n" end +# +# Print information about a task's stack usage +# +define showtaskstack + set $task = (struct _TCB *)$arg0 + + set $stack_free = 0 + while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free) + set $stack_free = $stack_free + 1 + end + printf" stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free +end + +# +# Print details of a task +# define showtask set $task = (struct _TCB *)$arg0 printf "%p %.2d ", $task, $task->pid _showtaskstate $task printf " %s\n", $task->name - set $stack_free = 0 - while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free) - set $stack_free = $stack_free + 1 - end - printf" stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free + showtaskstack $task if $task->task_state == TSTATE_WAIT_SEM printf " waiting on %p ", $task->waitsem From 2a8ef50df4747f0242459bdc22f11a46410f43bb Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 1 Dec 2012 19:29:36 -0800 Subject: [PATCH 3/9] A driver and shell command for the BlinkM I2C LED controller. --- apps/drivers/blinkm/Makefile | 42 +++ apps/drivers/blinkm/blinkm.cpp | 461 +++++++++++++++++++++++++++++ apps/drivers/device/i2c.cpp | 4 +- apps/drivers/device/i2c.h | 2 +- apps/drivers/drv_blinkm.h | 69 +++++ nuttx/configs/px4fmu/nsh/appconfig | 1 + 6 files changed, 576 insertions(+), 3 deletions(-) create mode 100644 apps/drivers/blinkm/Makefile create mode 100644 apps/drivers/blinkm/blinkm.cpp create mode 100644 apps/drivers/drv_blinkm.h diff --git a/apps/drivers/blinkm/Makefile b/apps/drivers/blinkm/Makefile new file mode 100644 index 0000000000..5a623693df --- /dev/null +++ b/apps/drivers/blinkm/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# BlinkM I2C LED driver +# + +APPNAME = blinkm +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp new file mode 100644 index 0000000000..96c6ae33f8 --- /dev/null +++ b/apps/drivers/blinkm/blinkm.cpp @@ -0,0 +1,461 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file blinkm.cpp + * + * Driver for the BlinkM LED controller connected via I2C. + */ + + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +class BlinkM : public device::I2C +{ +public: + BlinkM(int bus); + ~BlinkM(); + + virtual int init(); + virtual int probe(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + static const char *script_names[]; + +private: + enum ScriptID { + USER = 0, + RGB, + WHITE_FLASH, + RED_FLASH, + GREEN_FLASH, + BLUE_FLASH, + CYAN_FLASH, + MAGENTA_FLASH, + YELLOW_FLASH, + BLACK, + HUE_CYCLE, + MOOD_LIGHT, + VIRTUAL_CANDLE, + WATER_REFLECTIONS, + OLD_NEON, + THE_SEASONS, + THUNDERSTORM, + STOP_LIGHT, + MORSE_CODE + }; + + work_s _work; + static const unsigned _monitor_interval = 250; + + static void monitor_trampoline(void *arg); + void monitor(); + + int set_rgb(uint8_t r, uint8_t g, uint8_t b); + + int fade_rgb(uint8_t r, uint8_t g, uint8_t b); + int fade_hsb(uint8_t h, uint8_t s, uint8_t b); + + int fade_rgb_random(uint8_t r, uint8_t g, uint8_t b); + int fade_hsb_random(uint8_t h, uint8_t s, uint8_t b); + + int set_fade_speed(uint8_t s); + + int play_script(uint8_t script_id); + int play_script(const char *script_name); + int stop_script(); + + int write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3); + int read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]); + int set_script(uint8_t length, uint8_t repeats); + + int get_rgb(uint8_t &r, uint8_t &g, uint8_t &b); + + int get_firmware_version(uint8_t version[2]); +}; + +/* for now, we only support one BlinkM */ +namespace +{ +BlinkM *g_blinkm; + +} + +/* list of script names, must match script ID numbers */ +const char *BlinkM::script_names[] = { + "USER", + "RGB", + "WHITE_FLASH", + "RED_FLASH", + "GREEN_FLASH", + "BLUE_FLASH", + "CYAN_FLASH", + "MAGENTA_FLASH", + "YELLOW_FLASH", + "BLACK", + "HUE_CYCLE", + "MOOD_LIGHT", + "VIRTUAL_CANDLE", + "WATER_REFLECTIONS", + "OLD_NEON", + "THE_SEASONS", + "THUNDERSTORM", + "STOP_LIGHT", + "MORSE_CODE", + nullptr +}; + +extern "C" __EXPORT int blinkm_main(int argc, char *argv[]); + +BlinkM::BlinkM(int bus) : + I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000) +{ +} + +BlinkM::~BlinkM() +{ +} + +int +BlinkM::init() +{ + int ret; + + ret = I2C::init(); + + if (ret != OK) { + warnx("I2C init failed"); + return ret; + } + + /* turn off by default */ + play_script(BLACK); + + /* start the system monitor as a low-priority workqueue entry */ + work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, 1); + + return OK; +} + +int +BlinkM::probe() +{ + uint8_t version[2]; + int ret; + + ret = get_firmware_version(version); + + if (ret == OK) + log("found BlinkM firmware version %c%c", version[1], version[0]); + + return ret; +} + +int +BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case BLINKM_PLAY_SCRIPT_NAMED: + if (arg == 0) { + ret = EINVAL; + break; + } + ret = play_script((const char *)arg); + break; + + case BLINKM_PLAY_SCRIPT: + ret = play_script(arg); + break; + + case BLINKM_SET_USER_SCRIPT: { + if (arg == 0) { + ret = EINVAL; + break; + } + + unsigned lines = 0; + const uint8_t *script = (const uint8_t *)arg; + + while ((lines < 50) && (script[1] != 0)) { + ret = write_script_line(lines, script[0], script[1], script[2], script[3], script[4]); + if (ret != OK) + break; + script += 5; + } + if (ret == OK) + ret = set_script(lines, 0); + break; + } + + default: + break; + } + + return ret; +} + +void +BlinkM::monitor_trampoline(void *arg) +{ + BlinkM *bm = (BlinkM *)arg; + + bm->monitor(); +} + +void +BlinkM::monitor() +{ + /* check system state, possibly update LED to suit */ + + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, _monitor_interval); +} + +int +BlinkM::set_rgb(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[4] = { 'n', r, g, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_rgb(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[4] = { 'c', r, g, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_hsb(uint8_t h, uint8_t s, uint8_t b) +{ + const uint8_t msg[4] = { 'h', h, s, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_rgb_random(uint8_t r, uint8_t g, uint8_t b) +{ + const uint8_t msg[4] = { 'C', r, g, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::fade_hsb_random(uint8_t h, uint8_t s, uint8_t b) +{ + const uint8_t msg[4] = { 'H', h, s, b }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::set_fade_speed(uint8_t s) +{ + const uint8_t msg[2] = { 'f', s }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::play_script(uint8_t script_id) +{ + const uint8_t msg[4] = { 'p', script_id, 0, 0 }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::play_script(const char *script_name) +{ + /* handle HTML colour encoding */ + if (isxdigit(script_name[0]) && (strlen(script_name) == 6)) { + char code[3]; + uint8_t r, g, b; + + code[2] = '\0'; + + code[0] = script_name[1]; + code[1] = script_name[2]; + r = strtol(code, 0, 16); + code[0] = script_name[3]; + code[1] = script_name[4]; + g = strtol(code, 0, 16); + code[0] = script_name[5]; + code[1] = script_name[6]; + b = strtol(code, 0, 16); + + stop_script(); + return set_rgb(r, g, b); + } + + for (unsigned i = 0; script_names[i] != nullptr; i++) + if (!strcasecmp(script_name, script_names[i])) + return play_script(i); + + return -1; +} + +int +BlinkM::stop_script() +{ + const uint8_t msg[1] = { 'o' }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::write_script_line(uint8_t line, uint8_t ticks, uint8_t cmd, uint8_t arg1, uint8_t arg2, uint8_t arg3) +{ + const uint8_t msg[8] = { 'W', 0, line, ticks, cmd, arg1, arg2, arg3 }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::read_script_line(uint8_t line, uint8_t &ticks, uint8_t cmd[4]) +{ + const uint8_t msg[3] = { 'R', 0, line }; + uint8_t result[5]; + + int ret = transfer(msg, sizeof(msg), result, sizeof(result)); + + if (ret == OK) { + ticks = result[0]; + cmd[0] = result[1]; + cmd[1] = result[2]; + cmd[2] = result[3]; + cmd[3] = result[4]; + } + + return ret; +} + +int +BlinkM::set_script(uint8_t len, uint8_t repeats) +{ + const uint8_t msg[4] = { 'L', 0, len, repeats }; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b) +{ + const uint8_t msg = 'g'; + uint8_t result[3]; + + int ret = transfer(&msg, sizeof(msg), result, sizeof(result)); + + if (ret == OK) { + r = result[0]; + g = result[1]; + b = result[2]; + } + + return ret; +} + + +int +BlinkM::get_firmware_version(uint8_t version[2]) +{ + const uint8_t msg = 'Z'; + + return transfer(&msg, sizeof(msg), version, sizeof(version)); +} + +int +blinkm_main(int argc, char *argv[]) +{ + if (!strcmp(argv[1], "start")) { + if (g_blinkm != nullptr) + errx(1, "already started"); + + g_blinkm = new BlinkM(3); + + if (g_blinkm == nullptr) + errx(1, "new failed"); + + if (OK != g_blinkm->init()) { + delete g_blinkm; + errx(1, "init failed"); + } + + exit(0); + } + + if (g_blinkm == nullptr) + errx(1, "not started"); + + if (!strcmp(argv[1], "list")) { + for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) + fprintf(stderr, " %s\n", BlinkM::script_names[i]); + fprintf(stderr, " \n"); + exit(0); + } + + /* things that require access to the device */ + int fd = open(BLINKM_DEVICE_PATH, 0); + if (fd < 0) + err(1, "can't open BlinkM device"); + + if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK) + exit(0); + + errx(1, "missing command, try 'start', 'list' or a script name"); +} \ No newline at end of file diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index 56112d7677..474190d839 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -115,7 +115,7 @@ I2C::probe() } int -I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { struct i2c_msg_s msgv[2]; unsigned msgs; @@ -130,7 +130,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len if (send_len > 0) { msgv[msgs].addr = _address; msgv[msgs].flags = 0; - msgv[msgs].buffer = send; + msgv[msgs].buffer = const_cast(send); msgv[msgs].length = send_len; msgs++; } diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 3112e8e376..4d630b8a81 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -97,7 +97,7 @@ protected: * @return OK if the transfer was successful, -errno * otherwise. */ - int transfer(uint8_t *send, unsigned send_len, + int transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); /** diff --git a/apps/drivers/drv_blinkm.h b/apps/drivers/drv_blinkm.h new file mode 100644 index 0000000000..9c278f6c50 --- /dev/null +++ b/apps/drivers/drv_blinkm.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_blinkm.h + * + * BlinkM driver API + * + * This could probably become a more generalised API for multi-colour LED + * driver systems, or be merged with the generic LED driver. + */ + +#pragma once + +#include +#include + +#define BLINKM_DEVICE_PATH "/dev/blinkm" + +/* + * ioctl() definitions + */ + +#define _BLINKMIOCBASE (0x2900) +#define _BLINKMIOC(_n) (_IOC(_BLINKMIOCBASE, _n)) + +/** play the named script in *(char *)arg, repeating forever */ +#define BLINKM_PLAY_SCRIPT_NAMED _BLINKMIOC(1) + +/** play the numbered script in (arg), repeating forever */ +#define BLINKM_PLAY_SCRIPT _BLINKMIOC(2) + +/** + * Set the user script; (arg) is a pointer to an array of script lines, + * where each line is an array of four bytes giving , , arg[0-2] + * + * The script is terminated by a zero command. + */ +#define BLINKM_SET_USER_SCRIPT _BLINKMIOC(3) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 5833e35755..58eda35a09 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -98,6 +98,7 @@ CONFIGURED_APPS += drivers/l3gd20 CONFIGURED_APPS += drivers/px4io CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += drivers/led +CONFIGURED_APPS += drivers/blinkm CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil From 269bd9f4038189716bd630031c90e98b421e5a79 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 1 Dec 2012 19:36:02 -0800 Subject: [PATCH 4/9] Force the fade speed to something sensible. Deal better with failed probes. --- apps/drivers/blinkm/blinkm.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 96c6ae33f8..d589025cce 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -178,6 +178,9 @@ BlinkM::init() return ret; } + /* set some sensible defaults */ + set_fade_speed(25); + /* turn off by default */ play_script(BLACK); @@ -433,6 +436,7 @@ blinkm_main(int argc, char *argv[]) if (OK != g_blinkm->init()) { delete g_blinkm; + g_blinkm = nullptr; errx(1, "init failed"); } From 451ecc1bf4c310652ed8450ce0341bb148c66567 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 2 Dec 2012 17:53:31 -0800 Subject: [PATCH 5/9] Remove a few cut-and-paste author attributions. --- apps/px4/tests/test_bson.c | 1 - apps/px4/tests/tests_param.c | 1 - apps/systemcmds/bl_update/bl_update.c | 1 - apps/systemcmds/boardinfo/boardinfo.c | 1 - apps/systemcmds/perf/perf.c | 5 ++--- 5 files changed, 2 insertions(+), 7 deletions(-) diff --git a/apps/px4/tests/test_bson.c b/apps/px4/tests/test_bson.c index d03a8d332c..e2a10ec293 100644 --- a/apps/px4/tests/test_bson.c +++ b/apps/px4/tests/test_bson.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/apps/px4/tests/tests_param.c b/apps/px4/tests/tests_param.c index 88eff30f1c..13f17bc436 100644 --- a/apps/px4/tests/tests_param.c +++ b/apps/px4/tests/tests_param.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/apps/systemcmds/bl_update/bl_update.c b/apps/systemcmds/bl_update/bl_update.c index 745f76852d..45715b7918 100644 --- a/apps/systemcmds/bl_update/bl_update.c +++ b/apps/systemcmds/bl_update/bl_update.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/apps/systemcmds/boardinfo/boardinfo.c index 2328ebdb26..fb8b07ef4f 100644 --- a/apps/systemcmds/boardinfo/boardinfo.c +++ b/apps/systemcmds/boardinfo/boardinfo.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/apps/systemcmds/perf/perf.c b/apps/systemcmds/perf/perf.c index 72406e9c79..ced942fd6d 100644 --- a/apps/systemcmds/perf/perf.c +++ b/apps/systemcmds/perf/perf.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 Lorenz Meier. All rights reserved. - * Author: Lorenz Meier + * Copyright (C) 2012 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -13,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 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. * From 6e328b4d7ab31faef5796956cffb985a9859549d Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 3 Dec 2012 23:19:12 -0800 Subject: [PATCH 6/9] Add a 'monitor' verb to the px4io command so we can watch inputs to IO (it could get smarter). --- apps/drivers/px4io/px4io.cpp | 42 +++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 456564ba73..9f3dba047f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -88,6 +88,8 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); + bool dump_one; + private: static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; @@ -175,6 +177,7 @@ PX4IO *g_dev; PX4IO::PX4IO() : CDev("px4io", "/dev/px4io"), + dump_one(false), _serial_fd(-1), _io_stream(nullptr), _task(-1), @@ -478,6 +481,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) _send_needed = true; + /* if monitoring, dump the received info */ + if (dump_one) { + dump_one = false; + + printf("IO: %s armed ", rep->armed ? "" : "not"); + for (unsigned i = 0; i < rep->channel_count; i++) + printf("%d: %d ", i, rep->rc_channel[i]); + printf("\n"); + } + out: unlock(); } @@ -665,6 +678,30 @@ test(void) exit(0); } +void +monitor(void) +{ + unsigned cancels = 4; + printf("Hit three times to exit monitor mode\n"); + + for (;;) { + pollfd fds[1]; + + fds[0].fd = 0; + fds[0].events = POLLIN; + poll(fds, 1, 500); + + if (fds[0].revents == POLLIN) { + int c; + read(0, &c, 1); + if (cancels-- == 0) + exit(0); + } + + if (g_dev != nullptr) + g_dev->dump_one = true; + } +} } int @@ -740,8 +777,11 @@ px4io_main(int argc, char *argv[]) !strcmp(argv[1], "rx_sbus") || !strcmp(argv[1], "rx_ppm")) errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); + if (!strcmp(argv[1], "test")) test(); + if (!strcmp(argv[1], "monitor")) + monitor(); - errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'"); + errx(1, "need a command, try 'start', 'test', 'monitor' or 'update'"); } From 1485a4ec1aa8328cc50d99a1195b20df2b11045e Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 3 Dec 2012 23:20:28 -0800 Subject: [PATCH 7/9] Fix breakage to the DSM parser introduced with the input prioritisation logic. Back out to a "any input wins" strategy; connecting multiple receivers to I/O at the same time is currently not supported (read: strange things will happen). --- apps/px4io/comms.c | 12 +++------- apps/px4io/controls.c | 56 +++++++++++++++++++++++++++++++++++++++++-- apps/px4io/dsm.c | 36 ++++++++++------------------ apps/px4io/mixer.c | 44 +++------------------------------- apps/px4io/px4io.c | 1 + apps/px4io/px4io.h | 8 ++----- apps/px4io/sbus.c | 15 ++++++------ 7 files changed, 84 insertions(+), 88 deletions(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 40ea38cf74..480e3f5cc4 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -100,8 +100,8 @@ comms_main(void) debug("FMU: ready"); for (;;) { - /* wait for serial data, but no more than 100ms */ - poll(&fds, 1, 100); + /* wait for serial data, but no more than 10ms */ + poll(&fds, 1, 10); /* * Pull bytes from FMU and feed them to the HX engine. @@ -132,13 +132,7 @@ comms_main(void) /* populate the report */ for (int i = 0; i < system_state.rc_channels; i++) report.rc_channel[i] = system_state.rc_channel_data[i]; - - if (system_state.sbus_input_ok || system_state.dsm_input_ok || system_state.ppm_input_ok) { - report.channel_count = system_state.rc_channels; - } else { - report.channel_count = 0; - } - + report.channel_count = system_state.rc_channels; report.armed = system_state.armed; /* and send it */ diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index d4eace3df5..6cf3c80ac2 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -55,19 +55,23 @@ #include #include #include +#include #define DEBUG #include "px4io.h" +static void ppm_input(void); + void controls_main(void) { struct pollfd fds[2]; + /* DSM input */ fds[0].fd = dsm_init("/dev/ttyS0"); fds[0].events = POLLIN; - + /* S.bus input */ fds[1].fd = sbus_init("/dev/ttyS2"); fds[1].events = POLLIN; @@ -75,14 +79,62 @@ controls_main(void) /* run this loop at ~100Hz */ poll(fds, 2, 10); + /* + * Gather R/C control inputs from supported sources. + * + * Note that if you're silly enough to connect more than + * one control input source, they're going to fight each + * other. Don't do that. + */ if (fds[0].revents & POLLIN) dsm_input(); if (fds[1].revents & POLLIN) sbus_input(); + ppm_input(); - /* XXX do ppm processing, bypass mode, etc. here */ + /* + * If we haven't seen any new control data in 200ms, assume we + * have lost input and tell FMU + */ + if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { + + /* set the number of channels to zero - no inputs */ + system_state.rc_channels = 0; + + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; + } + + /* XXX do bypass mode, etc. here */ /* do PWM output updates */ mixer_tick(); } } + +static void +ppm_input(void) +{ + /* + * Look for new PPM input. + */ + if (ppm_last_valid_decode != 0) { + + /* avoid racing with PPM updates */ + irqstate_t state = irqsave(); + + /* PPM data exists, copy it */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + + /* copy the timestamp and clear it */ + system_state.rc_channels_timestamp = ppm_last_valid_decode; + ppm_last_valid_decode = 0; + + irqrestore(state); + + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; + } +} diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 744dac3d64..5841f29a36 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -275,10 +275,13 @@ dsm_decode(hrt_abstime frame_time) */ if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) dsm_guess_format(true); + + /* we have received something we think is a frame */ last_frame_time = frame_time; + + /* if we don't know the frame format, update the guessing state machine */ if (channel_shift == 0) { dsm_guess_format(false); - system_state.dsm_input_ok = false; return; } @@ -293,10 +296,6 @@ dsm_decode(hrt_abstime frame_time) * seven channels are being transmitted. */ - const unsigned dsm_chancount = (DSM_FRAME_CHANNELS < PX4IO_INPUT_CHANNELS) ? DSM_FRAME_CHANNELS : PX4IO_INPUT_CHANNELS; - - uint16_t dsm_channels[dsm_chancount]; - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; @@ -311,31 +310,22 @@ dsm_decode(hrt_abstime frame_time) continue; /* update the decoded channel count */ - if (channel > ppm_decoded_channels) - ppm_decoded_channels = channel; + if (channel >= system_state.rc_channels) + system_state.rc_channels = channel + 1; /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ if (channel_shift == 11) value /= 2; /* stuff the decoded channel into the PPM input buffer */ - dsm_channels[channel] = 988 + value; + /* XXX check actual values */ + system_state.rc_channel_data[channel] = 988 + value; } - /* DSM input is valid */ - system_state.dsm_input_ok = true; + /* and note that we have received data from the R/C controller */ + /* XXX failsafe will cause problems here - need a strategy for detecting it */ + system_state.rc_channels_timestamp = frame_time; - /* check if no S.BUS data is available */ - if (!system_state.sbus_input_ok) { - - for (unsigned i = 0; i < dsm_chancount; i++) { - system_state.rc_channel_data[i] = dsm_channels[i]; - } - - /* and note that we have received data from the R/C controller */ - /* XXX failsafe will cause problems here - need a strategy for detecting it */ - system_state.rc_channels_timestamp = frame_time; - system_state.rc_channels = dsm_chancount; - system_state.fmu_report_due = true; - } + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 483e9fe4d4..f02e98ae45 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -50,8 +50,6 @@ #include -#include - #include "px4io.h" /* @@ -59,10 +57,6 @@ */ static unsigned fmu_input_drops; #define FMU_INPUT_DROP_LIMIT 20 -/* - * Collect RC input data from the controller source(s). - */ -static void mixer_get_rc_input(void); /* * Update a mixer based on the current control signals. @@ -88,12 +82,6 @@ mixer_tick(void) int i; bool should_arm; - /* - * Start by looking for R/C control inputs. - * This updates system_state with any control inputs received. - */ - mixer_get_rc_input(); - /* * Decide which set of inputs we're using. */ @@ -122,8 +110,10 @@ mixer_tick(void) } else { /* we have no control input */ + /* XXX builtin failsafe would activate here */ control_count = 0; } + /* * Tickle each mixer, if we have control data. */ @@ -142,8 +132,7 @@ mixer_tick(void) /* * Decide whether the servos should be armed right now. */ - - should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu; + should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); @@ -166,30 +155,3 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) mixers[mixer].current_value = 0; } } - -static void -mixer_get_rc_input(void) -{ - /* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */ - if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) { - - /* input was ok and timed out, mark as update */ - if (system_state.ppm_input_ok) { - system_state.ppm_input_ok = false; - system_state.fmu_report_due = true; - } - return; - } - - /* mark PPM as valid */ - system_state.ppm_input_ok = true; - - /* check if no DSM and S.BUS data is available */ - if (!system_state.sbus_input_ok && !system_state.dsm_input_ok) { - /* otherwise, copy channel data */ - system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) - system_state.rc_channel_data[i] = ppm_buffer[i]; - system_state.fmu_report_due = true; - } -} diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 77524797ff..a3ac9e3e78 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -44,6 +44,7 @@ #include #include #include +#include #include diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 483b9bcc81..63a55d840e 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -69,12 +69,8 @@ struct sys_state_s { - bool armed; /* IO armed */ - bool arm_ok; /* FMU says OK to arm */ - - bool ppm_input_ok; /* valid PPM input data */ - bool dsm_input_ok; /* valid Spektrum DSM data */ - bool sbus_input_ok; /* valid Futaba S.Bus data */ + bool armed; /* IO armed */ + bool arm_ok; /* FMU says OK to arm */ /* * Data from the remote control input(s) diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index c3949f2b04..25fe0cf382 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -195,17 +195,16 @@ sbus_decode(hrt_abstime frame_time) /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; - system_state.sbus_input_ok = false; return; } - /* if the failsafe bit is set, we consider that a loss of RX signal */ + /* if the failsafe bit is set, we consider the frame invalid */ if (frame[23] & (1 << 4)) { - system_state.sbus_input_ok = false; return; } - unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS; + unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -228,14 +227,16 @@ sbus_decode(hrt_abstime frame_time) } if (PX4IO_INPUT_CHANNELS >= 18) { - /* decode two switch channels */ chancount = 18; + /* XXX decode the two switch channels */ } + /* note the number of channels decoded */ system_state.rc_channels = chancount; - system_state.sbus_input_ok = true; - system_state.fmu_report_due = true; /* and note that we have received data from the R/C controller */ system_state.rc_channels_timestamp = frame_time; + + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; } From 7c3b28d503123121403b4ad68c934bb91b05d878 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 4 Dec 2012 09:52:16 -0800 Subject: [PATCH 8/9] Lock out the PPM decoder if the DSM or S.bus decoders have seen a frame recently. --- apps/px4io/controls.c | 22 ++++++++++++++++++---- apps/px4io/dsm.c | 12 +++++++++--- apps/px4io/px4io.h | 4 ++-- apps/px4io/sbus.c | 16 +++++++++++++--- 4 files changed, 42 insertions(+), 12 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 6cf3c80ac2..3b37829188 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -86,15 +86,29 @@ controls_main(void) * one control input source, they're going to fight each * other. Don't do that. */ + bool locked = false; + if (fds[0].revents & POLLIN) - dsm_input(); + locked |= dsm_input(); if (fds[1].revents & POLLIN) - sbus_input(); - ppm_input(); + locked |= sbus_input(); + + /* + * If we don't have lock from one of the serial receivers, + * look for PPM. It shares an input with S.bus, so there's + * a possibility it will mis-parse an S.bus frame. + * + * XXX each S.bus frame will cause a PPM decoder interrupt + * storm (lots of edges). It might be sensible to actually + * disable the PPM decoder completely if we have an alternate + * receiver lock. + */ + if (!locked) + ppm_input(); /* * If we haven't seen any new control data in 200ms, assume we - * have lost input and tell FMU + * have lost input and tell FMU. */ if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 5841f29a36..04aca709bf 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -104,7 +104,7 @@ dsm_init(const char *device) return dsm_fd; } -void +bool dsm_input(void) { ssize_t ret; @@ -141,7 +141,7 @@ dsm_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - return; + goto out; last_rx_time = now; /* @@ -153,7 +153,7 @@ dsm_input(void) * If we don't have a full frame, return */ if (partial_frame_count < DSM_FRAME_SIZE) - return; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -161,6 +161,12 @@ dsm_input(void) */ dsm_decode(now); partial_frame_count = 0; + +out: + /* + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + */ + return (now - last_frame_time) < 200000; } static bool diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 63a55d840e..0032e6d800 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -165,9 +165,9 @@ extern void comms_main(void) __attribute__((noreturn)); */ extern void controls_main(void); extern int dsm_init(const char *device); -extern void dsm_input(void); +extern bool dsm_input(void); extern int sbus_init(const char *device); -extern void sbus_input(void); +extern bool sbus_input(void); /* * Assertion codes diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 25fe0cf382..a8f628a84e 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -58,6 +58,7 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; +static hrt_abstime last_frame_time; static uint8_t frame[SBUS_FRAME_SIZE]; @@ -94,7 +95,7 @@ sbus_init(const char *device) return sbus_fd; } -void +bool sbus_input(void) { ssize_t ret; @@ -131,7 +132,7 @@ sbus_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - return; + goto out; last_rx_time = now; /* @@ -143,7 +144,7 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - return; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -151,6 +152,12 @@ sbus_input(void) */ sbus_decode(now); partial_frame_count = 0; + +out: + /* + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + */ + return (now - last_frame_time) < 200000; } /* @@ -203,6 +210,9 @@ sbus_decode(hrt_abstime frame_time) return; } + /* we have received something we think is a frame */ + last_frame_time = frame_time; + unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; From fd771f67f2a2392d5ba2b7dd74100338859af6d7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 4 Dec 2012 22:00:24 -0800 Subject: [PATCH 9/9] Adjust the control mapping from DSM receivers to correspond to the standard PPM control mapping for channels 0-4. --- apps/px4io/comms.c | 2 +- apps/px4io/dsm.c | 24 +++++++++++++++++++++--- apps/px4io/px4io.h | 2 +- 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 480e3f5cc4..83a006d43b 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -130,7 +130,7 @@ comms_main(void) last_report_time = now; /* populate the report */ - for (int i = 0; i < system_state.rc_channels; i++) + for (unsigned i = 0; i < system_state.rc_channels; i++) report.rc_channel[i] = system_state.rc_channel_data[i]; report.channel_count = system_state.rc_channels; report.armed = system_state.armed; diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 04aca709bf..2611f3a034 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -322,10 +322,28 @@ dsm_decode(hrt_abstime frame_time) /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ if (channel_shift == 11) value /= 2; + value += 998; - /* stuff the decoded channel into the PPM input buffer */ - /* XXX check actual values */ - system_state.rc_channel_data[channel] = 988 + value; + /* + * Store the decoded channel into the R/C input buffer, taking into + * account the different ideas about channel assignement that we have. + * + * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, + * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. + */ + switch (channel) { + case 0: + channel = 2; + break; + case 1: + channel = 0; + break; + case 2: + channel = 1; + default: + break; + } + system_state.rc_channel_data[channel] = value; } /* and note that we have received data from the R/C controller */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 0032e6d800..45b7cf847a 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -75,7 +75,7 @@ struct sys_state_s /* * Data from the remote control input(s) */ - int rc_channels; + unsigned rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp;