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 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..d589025cce --- /dev/null +++ b/apps/drivers/blinkm/blinkm.cpp @@ -0,0 +1,465 @@ +/**************************************************************************** + * + * 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; + } + + /* set some sensible defaults */ + set_fade_speed(25); + + /* 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; + g_blinkm = nullptr; + 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/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index efd396fa65..31ed89ecca 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -89,6 +89,8 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); + bool dump_one; + private: static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; @@ -179,6 +181,7 @@ PX4IO *g_dev; PX4IO::PX4IO() : CDev("px4io", "/dev/px4io"), + dump_one(false), _serial_fd(-1), _io_stream(nullptr), _task(-1), @@ -494,6 +497,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(); } @@ -681,6 +694,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 @@ -756,8 +793,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'"); } 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; 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/px4io/comms.c b/apps/px4io/comms.c index f4eddbdd3b..5033aeb982 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. @@ -130,15 +130,9 @@ 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]; - - 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 0e02103731..e64f8b204e 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -55,6 +55,7 @@ #include #include #include +#include #define DEBUG #include "px4io.h" @@ -63,15 +64,18 @@ #define RC_CHANNEL_HIGH_THRESH 1600 #define RC_CHANNEL_LOW_THRESH 1400 +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; @@ -79,10 +83,32 @@ 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. + */ + bool locked = false; + if (fds[0].revents & POLLIN) - dsm_input(); + locked |= dsm_input(); if (fds[1].revents & POLLIN) - sbus_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(); /* force manual input override */ if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { @@ -92,16 +118,48 @@ controls_main(void) system_state.mixer_use_fmu = true; } - /* detect rc loss event */ - if (hrt_absolute_time() - system_state.rc_channels_timestamp > RC_FAILSAFE_TIMEOUT) { + /* + * 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; system_state.rc_lost = true; + + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; } - /* XXX detect fmu loss event */ - - /* XXX handle failsave events - RC loss and FMU loss - 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..2611f3a034 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 @@ -275,10 +281,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 +302,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 +316,40 @@ 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; + value += 998; - /* stuff the decoded channel into the PPM input buffer */ - dsm_channels[channel] = 988 + value; - } - - /* DSM input is valid */ - system_state.dsm_input_ok = true; - - /* 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]; + /* + * 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; } - - /* 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; + system_state.rc_channel_data[channel] = value; } + + /* 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; + + /* 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 d9626c1e3f..4fa3e1d5df 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. */ @@ -167,30 +157,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 9bfe3b1e4d..e0b7cdda13 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -69,17 +69,13 @@ 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) */ - int rc_channels; + unsigned rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; @@ -184,9 +180,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 c3949f2b04..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; } /* @@ -195,17 +202,19 @@ 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; + /* 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; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -228,14 +237,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; } 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. * diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index ca9f3f4ee3..5e2a32a460 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -97,6 +97,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