From c842b0457ff87146279bde81b23c6c81142474cb Mon Sep 17 00:00:00 2001 From: Dennis Shtatnov Date: Sun, 28 Aug 2016 09:36:06 -0400 Subject: [PATCH] Syslink for battery, RSSI, and initial radio control --- ROMFS/px4fmu_common/init.d/rcS | 4 + cmake/configs/nuttx_crazyflie_default.cmake | 1 + nuttx-configs/crazyflie/nsh/defconfig | 2 +- src/modules/syslink/CMakeLists.txt | 44 +++ src/modules/syslink/crtp.h | 74 ++++ src/modules/syslink/syslink.c | 153 ++++++++ src/modules/syslink/syslink.h | 101 +++++ src/modules/syslink/syslink_main.cpp | 411 ++++++++++++++++++++ src/modules/syslink/syslink_main.h | 80 ++++ src/modules/syslink/syslink_params.c | 76 ++++ 10 files changed, 945 insertions(+), 1 deletion(-) create mode 100644 src/modules/syslink/CMakeLists.txt create mode 100644 src/modules/syslink/crtp.h create mode 100644 src/modules/syslink/syslink.c create mode 100644 src/modules/syslink/syslink.h create mode 100644 src/modules/syslink/syslink_main.cpp create mode 100644 src/modules/syslink/syslink_main.h create mode 100644 src/modules/syslink/syslink_params.c diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2b0f3ce9d9..7d6c48560f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -518,6 +518,10 @@ then then frsky_telemetry start -d /dev/ttyS2 fi + if param compare SYS_COMPANION 20 + then + syslink start + fi if param compare SYS_COMPANION 921600 then mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x diff --git a/cmake/configs/nuttx_crazyflie_default.cmake b/cmake/configs/nuttx_crazyflie_default.cmake index 6356b9700c..95a5f958b8 100644 --- a/cmake/configs/nuttx_crazyflie_default.cmake +++ b/cmake/configs/nuttx_crazyflie_default.cmake @@ -44,6 +44,7 @@ set(config_module_list modules/land_detector modules/dummy + modules/syslink # # Estimation modules (EKF/ SO3 / other filters) diff --git a/nuttx-configs/crazyflie/nsh/defconfig b/nuttx-configs/crazyflie/nsh/defconfig index 7b5b196317..df827a525e 100644 --- a/nuttx-configs/crazyflie/nsh/defconfig +++ b/nuttx-configs/crazyflie/nsh/defconfig @@ -386,7 +386,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=42 +CONFIG_NFILE_DESCRIPTORS=51 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/src/modules/syslink/CMakeLists.txt b/src/modules/syslink/CMakeLists.txt new file mode 100644 index 0000000000..e2f02fc53d --- /dev/null +++ b/src/modules/syslink/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ +px4_add_module( + MODULE modules__syslink + MAIN syslink + STACK_MAIN 2000 + SRCS + syslink_main.cpp + syslink_params.c + syslink.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/syslink/crtp.h b/src/modules/syslink/crtp.h new file mode 100644 index 0000000000..596dc3429c --- /dev/null +++ b/src/modules/syslink/crtp.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +#pragma once + + +#include + +#define CRTP_PORT_CONSOLE 0x00 +#define CRTP_PORT_PARAM 0x02 +#define CRTP_PORT_COMMANDER 0x03 +#define CRTP_PORT_MEM 0x04 +#define CRTP_PORT_LOG 0x05 + +#define CRTP_PORT_MAVLINK 0x08 // Non-standard port for transmitting mavlink messages + +#define CRTP_PORT_PLATFORM 0x0D +#define CRTP_PORT_DEBUG 0x0E +#define CRTP_PORT_LINK 0x0F + +#define CRTP_NULL(x) (((x).header & 0xf3) == 0xf3) + +#define CRTP_MAX_DATA_SIZE 29 + +typedef struct { + union { + uint8_t header; + struct { + uint8_t channel : 2; + uint8_t link : 2; + uint8_t port : 4; + }; + }; + + uint8_t data[CRTP_MAX_DATA_SIZE]; +} __attribute__((packed)) crtp_message_t; + + +typedef struct { + float roll; // -20 to 20 + float pitch; + float yaw; // -150 to 150 + uint16_t thrust; +} crtp_commander; diff --git a/src/modules/syslink/syslink.c b/src/modules/syslink/syslink.c new file mode 100644 index 0000000000..303ba7fc02 --- /dev/null +++ b/src/modules/syslink/syslink.c @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 syslink.c + * + * Crazyflie Syslink protocol implementation + * + * @author Dennis Shtatnov + */ + + +#include +#include +#include +#include +#include +#include +#include + + +#include "syslink.h" + + +const char *syslink_magic = "\xbc\xcf"; + +void syslink_parse_init(syslink_parse_state *state) +{ + state->state = SYSLINK_STATE_START; + state->index = 0; +} + + +int syslink_parse_char(syslink_parse_state *state, char c, syslink_message_t *msg) +{ + + switch (state->state) { + case SYSLINK_STATE_START: + if (c == syslink_magic[state->index]) { + state->index++; + + } else { + state->index = 0; + } + + if (syslink_magic[state->index] == '\x00') { + state->state = SYSLINK_STATE_TYPE; + } + + break; + + case SYSLINK_STATE_TYPE: + msg->type = c; + state->state = SYSLINK_STATE_LENGTH; + break; + + case SYSLINK_STATE_LENGTH: + msg->length = c; + + if (c > SYSLINK_MAX_DATA_LEN) { // Too long + state->state = SYSLINK_STATE_START; + + } else { + state->state = c > 0 ? SYSLINK_STATE_DATA : SYSLINK_STATE_CKSUM; + } + + state->index = 0; + break; + + case SYSLINK_STATE_DATA: + msg->data[state->index++] = c; + + if (state->index >= msg->length) { + state->state = SYSLINK_STATE_CKSUM; + state->index = 0; + syslink_compute_cksum(msg); + } + + break; + + case SYSLINK_STATE_CKSUM: + if (c != msg->cksum[state->index]) { + PX4_INFO("Bad checksum"); + state->state = SYSLINK_STATE_START; + state->index = 0; + break; + } + + state->index++; + + if (state->index >= sizeof(msg->cksum)) { + state->state = SYSLINK_STATE_START; + state->index = 0; + return 1; + } + + + break; + } + + return 0; + +} + +/* + Computes Fletcher 8bit checksum per RFC1146 + A := A + D[i] + B := B + A +*/ +void syslink_compute_cksum(syslink_message_t *msg) +{ + uint8_t a = 0, b = 0; + uint8_t *Di = (uint8_t *)msg, *end = Di + (2 + msg->length) * sizeof(uint8_t); + + while (Di < end) { + a = a + *Di; + b = b + a; + ++Di; + } + + msg->cksum[0] = a; + msg->cksum[1] = b; +} diff --git a/src/modules/syslink/syslink.h b/src/modules/syslink/syslink.h new file mode 100644 index 0000000000..2038bad7d9 --- /dev/null +++ b/src/modules/syslink/syslink.h @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + + +#include + + +extern const char *syslink_magic; + + +#define SYSLINK_RADIO_RAW 0x00 +#define SYSLINK_RADIO_CHANNEL 0x01 +#define SYSLINK_RADIO_DATARATE 0x02 +#define SYSLINK_RADIO_CONTWAVE 0x03 +#define SYSLINK_RADIO_RSSI 0x04 +#define SYSLINK_RADIO_ADDRESS 0x05 + +#define SYSLINK_PM_SOURCE 0x10 +#define SYSLINK_PM_ONOFF_SWITCHOFF 0x11 +#define SYSLINK_PM_BATTERY_VOLTAGE 0x12 +#define SYSLINK_PM_BATTERY_STATE 0x13 +#define SYSLINK_PM_BATTERY_AUTOUPDATE 0x14 + +#define SYSLINK_OW_SCAN 0x20 +#define SYSLINK_OW_GETINFO 0x21 +#define SYSLINK_OW_READ 0x22 +#define SYSLINK_OW_WRITE 0x23 + +#define SYSLINK_MAX_DATA_LEN 32 + +#define SYSLINK_RADIO_RATE_250K 0 +#define SYSLINK_RADIO_RATE_1M 1 +#define SYSLINK_RADIO_RATE_2M 2 + + +typedef struct { + uint8_t type; + uint8_t length; + uint8_t data[SYSLINK_MAX_DATA_LEN]; + uint8_t cksum[2]; +} syslink_message_t; + +typedef enum { + SYSLINK_STATE_START = 0, + SYSLINK_STATE_TYPE, + SYSLINK_STATE_LENGTH, + SYSLINK_STATE_DATA, + SYSLINK_STATE_CKSUM +} SYSLINK_STATE; + +typedef struct { + SYSLINK_STATE state; + int index; + +} syslink_parse_state; + + +#ifdef __cplusplus +extern "C" { +#endif + +extern void syslink_parse_init(syslink_parse_state *state); + +// Returns true if a full message was parsed +extern int syslink_parse_char(syslink_parse_state *state, char c, syslink_message_t *msg); + +extern void syslink_compute_cksum(syslink_message_t *msg); + +#ifdef __cplusplus +} +#endif diff --git a/src/modules/syslink/syslink_main.cpp b/src/modules/syslink/syslink_main.cpp new file mode 100644 index 0000000000..eeb49293c3 --- /dev/null +++ b/src/modules/syslink/syslink_main.cpp @@ -0,0 +1,411 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 syslink_main.cpp + * Entry point for syslink module used to communicate with the NRF module on a Crazyflie + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + +#include "crtp.h" +#include "syslink_main.h" + +extern "C" { __EXPORT int syslink_main(int argc, char *argv[]); } + + +Syslink *g_syslink; + +Syslink::Syslink() : + _syslink_task(-1), + _task_running(false), + _fd(0), + _battery_pub(nullptr), + _rc_pub(nullptr), + _cmd_pub(nullptr), + _rssi(RC_INPUT_RSSI_MAX) +{ + +} + + +int +Syslink::start() +{ + _task_running = true; + _syslink_task = px4_task_spawn_cmd( + "syslink", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 3600, + Syslink::task_main_trampoline, + NULL + ); + + return 0; +} + + +int +Syslink::set_datarate(uint8_t datarate) +{ + syslink_message_t msg; + msg.type = SYSLINK_RADIO_DATARATE; + msg.length = 1; + msg.data[0] = datarate; + return send_message(&msg); +} + +int +Syslink::set_channel(uint8_t channel) +{ + syslink_message_t msg; + msg.type = SYSLINK_RADIO_CHANNEL; + msg.length = 1; + msg.data[0] = channel; + return send_message(&msg); +} + +int +Syslink::set_address(uint64_t addr) +{ + syslink_message_t msg; + msg.type = SYSLINK_RADIO_ADDRESS; + msg.length = 5; + memcpy(&msg.data, &addr, 5); + return send_message(&msg); +} + + +// 1M 8N1 serial connection to NRF51 +int +Syslink::open_serial(const char *dev) +{ + int rate = B1000000; + + // open uart + int fd = px4_open(dev, O_RDWR | O_NOCTTY); + int termios_state = -1; + + if (fd < 0) { + PX4_ERR("failed to open uart device!"); + return -1; + } + + // set baud rate + struct termios config; + tcgetattr(fd, &config); + + // clear ONLCR flag (which appends a CR for every LF) + config.c_oflag &= ~ONLCR; + + // Disable hardware flow control + // TODO: CF2 uses a TX EN as output flow control + config.c_cflag &= ~CRTSCTS; + + + /* Set baud rate */ + if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) { + warnx("ERR SET BAUD %s: %d\n", dev, termios_state); + px4_close(fd); + return -1; + } + + if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) { + PX4_WARN("ERR SET CONF %s\n", dev); + px4_close(fd); + return -1; + } + + return fd; +} + + + +int +Syslink::task_main_trampoline(int argc, char *argv[]) +{ + g_syslink->task_main(); + return 0; +} + +void +Syslink::task_main() +{ + param_t _param_radio_channel = param_find("SLNK_RADIO_CHAN"); + param_t _param_radio_rate = param_find("SLNK_RADIO_RATE"); + param_t _param_radio_addr1 = param_find("SLNK_RADIO_ADDR1"); + param_t _param_radio_addr2 = param_find("SLNK_RADIO_ADDR2"); + + uint32_t channel, rate; + uint64_t addr = 0; + + param_get(_param_radio_channel, &channel); + param_get(_param_radio_rate, &rate); + param_get(_param_radio_addr1, &addr + 4); + param_get(_param_radio_addr2, &addr); + + + + _battery.reset(&_battery_status); + + + // int ret; + + /* Open serial port */ + const char *dev = "/dev/ttyS2"; + _fd = open_serial(dev); + + if (_fd < 0) { + err(1, "can't open %s", dev); + return; + } + + + /* Set non-blocking */ + int flags = fcntl(_fd, F_GETFL, 0); + fcntl(_fd, F_SETFL, flags | O_NONBLOCK); + + + set_datarate(rate); + usleep(1000); + set_channel(channel); + usleep(1000); + set_address(addr); + + + px4_pollfd_struct_t fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + int error_counter = 0; + + char buf[64]; + int nread; + + syslink_parse_state state; + syslink_message_t msg; + + syslink_parse_init(&state); + + + while (_task_running) { + /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ + int poll_ret = px4_poll(fds, 1, 1000); + + /* handle the poll result */ + if (poll_ret == 0) { + /* this means none of our providers is giving us data */ + } else if (poll_ret < 0) { + /* this is seriously bad - should be an emergency */ + if (error_counter < 10 || error_counter % 50 == 0) { + /* use a counter to prevent flooding (and slowing us down) */ + PX4_ERR("[syslink] ERROR return value from poll(): %d" + , poll_ret); + } + + error_counter++; + + } else { + if (fds[0].revents & POLLIN) { + if ((nread = read(_fd, buf, sizeof(buf))) < 0) { + continue; + } + + for (int i = 0; i < nread; i++) { + if (syslink_parse_char(&state, buf[i], &msg)) { + handle_message(&msg); + } + } + } + } + } + + close(_fd); + +} + + + +static int count = 0; +static int null_count = 0; +static hrt_abstime lasttime = 0; + + +void +Syslink::handle_message(syslink_message_t *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - lasttime > 1000000) { + PX4_INFO("%d packets per second (%d null)", count, null_count); + lasttime = t; + count = 0; + null_count = 0; + } + + count++; + + if (msg->type == SYSLINK_PM_ONOFF_SWITCHOFF) { + // When the power button is hit + } else if (msg->type == SYSLINK_PM_BATTERY_STATE) { + + if (msg->length != 9) { + return; + } + + //uint8_t flags = msg->data[0]; + //float iset = *((float *)&msg->data[5]); + float vbat; + memcpy(&vbat, &msg->data[1], sizeof(float)); + + _battery.updateBatteryStatus(t, vbat, 0, 0, false, &_battery_status); + + // announce the battery status if needed, just publish else + if (_battery_pub != nullptr) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } + + } else if (msg->type == SYSLINK_RADIO_RSSI) { + uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm + _rssi = 140 - rssi * 100 / (100 - 40); + + } else if (msg->type == SYSLINK_RADIO_CHANNEL) { + PX4_INFO("Channel ACK %d", msg->data[0]); + + } else if (msg->type == SYSLINK_RADIO_DATARATE) { + PX4_INFO("Datarate ACK %d", msg->data[0]); + + } else if (msg->type == SYSLINK_RADIO_RAW) { + handle_raw(msg); + } + + +} + +void +Syslink::handle_raw(syslink_message_t *sys) +{ + // TODO: Now allow one raw message to be sent from the queue + + crtp_message_t *c = (crtp_message_t *) &sys->data; + + if (CRTP_NULL(*c)) { + null_count++; + return; + } + + + if (c->port == CRTP_PORT_COMMANDER) { + + crtp_commander *cmd = (crtp_commander *) &c->data[0]; + + struct rc_input_values rc = {}; + + rc.timestamp_publication = hrt_absolute_time(); + rc.timestamp_last_signal = rc.timestamp_publication; + rc.channel_count = 5; + rc.rc_failsafe = false; + rc.rc_lost = false; + rc.rc_lost_frame_count = 0; + rc.rc_total_frame_count = 1; + rc.rc_ppm_frame_length = 0; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; + rc.rssi = _rssi; + + + double pitch = cmd->pitch, roll = cmd->roll, yaw = cmd->yaw; + + /* channels (scaled to rc limits) */ + rc.values[0] = pitch * 500 / 20 + 1500; + rc.values[1] = roll * 500 / 20 + 1500; + rc.values[2] = yaw * 500 / 150 + 1500; + rc.values[3] = cmd->thrust * 1000 / USHRT_MAX + 1000; + rc.values[4] = 1000; // Dummy channel as px4 needs at least 5 + + if (_rc_pub == nullptr) { + _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); + + } else { + orb_publish(ORB_ID(input_rc), _rc_pub, &rc); + } + + } else if (c->port == CRTP_PORT_MAVLINK) { + /* Pipe to Mavlink bridge */ + } + +} + +int +Syslink::send_message(syslink_message_t *msg) +{ + syslink_compute_cksum(msg); + write(_fd, syslink_magic, 2); + write(_fd, &msg->type, sizeof(msg->type)); + write(_fd, &msg->length, sizeof(msg->length)); + write(_fd, &msg->data, msg->length); + write(_fd, &msg->cksum, sizeof(msg->cksum)); + return 0; +} + +int syslink_main(int argc, char *argv[]) +{ + PX4_INFO("Starting daemon..."); + g_syslink = new Syslink(); + g_syslink->start(); + + return 0; +} diff --git a/src/modules/syslink/syslink_main.h b/src/modules/syslink/syslink_main.h new file mode 100644 index 0000000000..3b216fcf24 --- /dev/null +++ b/src/modules/syslink/syslink_main.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +#include +#include +#include +#include "syslink.h" + +class Syslink +{ +public: + Syslink(); + + int start(); + + // TODO: These should wait for an ACK + int set_datarate(uint8_t datarate); + int set_channel(uint8_t channel); + int set_address(uint64_t addr); + +private: + + int open_serial(const char *dev); + + void handle_message(syslink_message_t *msg); + void handle_raw(syslink_message_t *sys); + + // Checksums and transmits a syslink message + int send_message(syslink_message_t *msg); + + int _syslink_task; + bool _task_running; + + int _fd; + + orb_advert_t _battery_pub; + orb_advert_t _rc_pub; + orb_advert_t _cmd_pub; + + struct battery_status_s _battery_status; + + Battery _battery; + + int32_t _rssi; + + static int task_main_trampoline(int argc, char *argv[]); + + void task_main(); + +}; diff --git a/src/modules/syslink/syslink_params.c b/src/modules/syslink/syslink_params.c new file mode 100644 index 0000000000..0d6ac9d087 --- /dev/null +++ b/src/modules/syslink/syslink_params.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 syslink_params.c + * + * Parameters defined by the syslink module and the exposed NRF51 radio + * + * @author Dennis Shtatnov + */ + +#include +#include + + +/** + * Operating channel of the NRF51 + * + * @min 0 + * @max 125 + * @group Syslink + */ +PARAM_DEFINE_INT32(SLNK_RADIO_CHAN, 80); + +/** + * Operating datarate of the NRF51 + * + * @min 0 + * @max 2 + * @group Syslink + */ +PARAM_DEFINE_INT32(SLNK_RADIO_RATE, 0); + +/** + * Operating address of the NRF51 (most significant byte) + * + * @group Syslink + */ +PARAM_DEFINE_INT32(SLNK_RADIO_ADDR1, (uint32_t) 0xE7); + +/** + * Operating address of the NRF51 (least significant 4 bytes) + * + * @group Syslink + */ +PARAM_DEFINE_INT32(SLNK_RADIO_ADDR2, (uint32_t) 0xE7E7E7E7);