dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Syslink for battery, RSSI, and initial radio control

This commit is contained in:
Dennis Shtatnov 2016-08-28 09:36:06 -04:00 committed by Lorenz Meier
parent b99b886576
commit c842b0457f
10 changed files with 945 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d/rcS
View File

@ -518,6 +518,10 @@ then
then then
frsky_telemetry start -d /dev/ttyS2 frsky_telemetry start -d /dev/ttyS2
fi fi
if param compare SYS_COMPANION 20
then
syslink start
fi
if param compare SYS_COMPANION 921600 if param compare SYS_COMPANION 921600
then then
mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x

1
cmake/configs/nuttx_crazyflie_default.cmake
View File

@ -44,6 +44,7 @@ set(config_module_list
modules/land_detector modules/land_detector
modules/dummy modules/dummy
modules/syslink
# #
# Estimation modules (EKF/ SO3 / other filters) # Estimation modules (EKF/ SO3 / other filters)

2
nuttx-configs/crazyflie/nsh/defconfig
View File

@ -386,7 +386,7 @@ CONFIG_SIG_SIGWORK=4
CONFIG_MAX_TASKS=32 CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10 CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4 CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=42 CONFIG_NFILE_DESCRIPTORS=51
CONFIG_NFILE_STREAMS=8 CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32 CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_PREALLOC_MQ_MSGS=4

44
src/modules/syslink/CMakeLists.txt Normal file
View File

@ -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 :

74
src/modules/syslink/crtp.h Normal file
View File

@ -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 <stdint.h>
#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;

153
src/modules/syslink/syslink.c Normal file
View File

@ -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 <densht@gmail.com>
*/
#include <px4_defines.h>
#include <fcntl.h>
#include <stdio.h>
#include <sys/ioctl.h>
#include <systemlib/err.h>
#include <poll.h>
#include <termios.h>
#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;
}

101
src/modules/syslink/syslink.h Normal file
View File

@ -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 <stdint.h>
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

411
src/modules/syslink/syslink_main.cpp Normal file
View File

@ -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 <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <px4_defines.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <poll.h>
#include <termios.h>
#include <drivers/drv_rc_input.h>
#include <systemlib/err.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/input_rc.h>
#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;
}

80
src/modules/syslink/syslink_main.h Normal file
View File

@ -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 <stdint.h>
#include <systemlib/battery.h>
#include <uORB/uORB.h>
#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();
};

76
src/modules/syslink/syslink_params.c Normal file
View File

@ -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 <densht@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* 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);