zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
29381 Commits
14 Branches
0 Tags
306 MiB
 
 
 
 
 
 
Tree: 29915cbc6d
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '29915cbc6d'
${ noResults }
px4_autopilot/boards/bitcraze/crazyflie/syslink/syslink_main.cpp

903 lines
19 KiB
Raw Blame History

/****************************************************************************
*
* 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 <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <mqueue.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_board_led.h>
#include <systemlib/err.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/input_rc.h>
#include <board_config.h>
#include "crtp.h"
#include "syslink_main.h"
#include "drv_deck.h"
__BEGIN_DECLS
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
extern void led_toggle(int led);
__END_DECLS
extern "C" { __EXPORT int syslink_main(int argc, char *argv[]); }
Syslink *g_syslink = nullptr;
Syslink::Syslink() :
pktrate(0),
nullrate(0),
rxrate(0),
txrate(0),
_syslink_task(-1),
_task_running(false),
_bootloader_mode(false),
_count(0),
_null_count(0),
_count_in(0),
_count_out(0),
_lasttime(0),
_lasttxtime(0),
_lastrxtime(0),
_fd(0),
_queue(2, sizeof(syslink_message_t)),
_writebuffer(16, sizeof(crtp_message_t)),
_battery_pub(nullptr),
_rc_pub(nullptr),
_cmd_pub(nullptr),
_rssi(RC_INPUT_RSSI_MAX),
_bstate(BAT_DISCHARGING)
{
px4_sem_init(&memory_sem, 0, 0);
/* memory_sem use case is a signal */
px4_sem_setprotocol(&memory_sem, SEM_PRIO_NONE);
}
int
Syslink::start()
{
_task_running = true;
_syslink_task = px4_task_spawn_cmd(
"syslink",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
1500,
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);
}
int
Syslink::send_queued_raw_message()
{
if (_writebuffer.empty()) {
return 0;
}
_lasttxtime = hrt_absolute_time();
syslink_message_t msg;
msg.type = SYSLINK_RADIO_RAW;
_count_out++;
_writebuffer.get(&msg.length, sizeof(crtp_message_t));
return send_message(&msg);
}
void
Syslink::update_params(bool force_set)
{
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");
// reading parameter values into temp variables
int32_t channel, rate, addr1, addr2;
uint64_t addr = 0;
param_get(_param_radio_channel, &channel);
param_get(_param_radio_rate, &rate);
param_get(_param_radio_addr1, &addr1);
param_get(_param_radio_addr2, &addr2);
memcpy(&addr, &addr2, 4);
memcpy(((char *)&addr) + 4, &addr1, 4);
hrt_abstime t = hrt_absolute_time();
// request updates if needed
if (channel != this->_channel || force_set) {
this->_channel = channel;
set_channel(channel);
this->_params_update[0] = t;
this->_params_ack[0] = 0;
}
if (rate != this->_rate || force_set) {
this->_rate = rate;
set_datarate(rate);
this->_params_update[1] = t;
this->_params_ack[1] = 0;
}
if (addr != this->_addr || force_set) {
this->_addr = addr;
set_address(addr);
this->_params_update[2] = t;
this->_params_ack[2] = 0;
}
}
// 1M 8N1 serial connection to NRF51
int
Syslink::open_serial(const char *dev)
{
#ifndef B1000000
#define B1000000 1000000
#endif
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 = 0;
config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
// Disable hardware 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()
{
_bridge = new SyslinkBridge(this);
_bridge->init();
_memory = new SyslinkMemory(this);
_memory->init();
_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);
*/
px4_arch_configgpio(GPIO_NRF_TXEN);
px4_pollfd_struct_t fds[2];
fds[0].fd = _fd;
fds[0].events = POLLIN;
_params_sub = orb_subscribe(ORB_ID(parameter_update));
fds[1].fd = _params_sub;
fds[1].events = POLLIN;
int error_counter = 0;
char buf[64];
int nread;
syslink_parse_state state;
syslink_message_t msg;
syslink_parse_init(&state);
//setup initial parameters
update_params(true);
while (_task_running) {
int poll_ret = px4_poll(fds, 2, 500);
/* handle the poll result */
if (poll_ret == 0) {
/* timeout: 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);
}
}
}
if (fds[1].revents & POLLIN) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
update_params(false);
}
}
}
close(_fd);
}
void
Syslink::handle_message(syslink_message_t *msg)
{
hrt_abstime t = hrt_absolute_time();
if (t - _lasttime > 1000000) {
pktrate = _count;
rxrate = _count_in;
txrate = _count_out;
nullrate = _null_count;
_lasttime = t;
_count = 0;
_null_count = 0;
_count_in = 0;
_count_out = 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];
int charging = flags & 1;
int powered = flags & 2;
float vbat; //, iset;
memcpy(&vbat, &msg->data[1], sizeof(float));
//memcpy(&iset, &msg->data[5], sizeof(float));
_battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, false, &_battery_status);
// Update battery charge state
if (charging) {
_bstate = BAT_CHARGING;
}
/* With the usb plugged in and battery disconnected, it appears to be charged. The voltage check ensures that a battery is connected */
else if (powered && !charging && _battery_status.voltage_filtered_v > 3.7f) {
_bstate = BAT_CHARGED;
} else {
_bstate = BAT_DISCHARGING;
}
// 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_RAW) {
handle_raw(msg);
_lastrxtime = t;
} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) {
handle_radio(msg);
} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) {
memcpy(&_memory->msgbuf, msg, sizeof(syslink_message_t));
px4_sem_post(&memory_sem);
} else {
PX4_INFO("GOT %d", msg->type);
}
//Send queued messages
if (!_queue.empty()) {
_queue.get(msg, sizeof(syslink_message_t));
send_message(msg);
}
float p = (t % 500000) / 500000.0f;
/* Use LED_GREEN for charging indicator */
if (_bstate == BAT_CHARGED) {
led_on(LED_GREEN);
} else if (_bstate == BAT_CHARGING && p < 0.25f) {
led_on(LED_GREEN);
} else {
led_off(LED_GREEN);
}
/* Alternate RX/TX LEDS when transfering */
bool rx = t - _lastrxtime < 200000,
tx = t - _lasttxtime < 200000;
if (rx && p < 0.25f) {
led_on(LED_RX);
} else {
led_off(LED_RX);
}
if (tx && p > 0.5f && p > 0.75f) {
led_on(LED_TX);
} else {
led_off(LED_TX);
}
// resend parameters if they haven't been acknowledged
if (_params_ack[0] == 0 && t - _params_update[0] > 10000) {
set_channel(_channel);
} else if (_params_ack[1] == 0 && t - _params_update[1] > 10000) {
set_datarate(_rate);
} else if (_params_ack[2] == 0 && t - _params_update[2] > 10000) {
set_address(_addr);
}
}
void
Syslink::handle_radio(syslink_message_t *sys)
{
hrt_abstime t = hrt_absolute_time();
// record acknowlegements to parameter messages
if (sys->type == SYSLINK_RADIO_CHANNEL) {
_params_ack[0] = t;
} else if (sys->type == SYSLINK_RADIO_DATARATE) {
_params_ack[1] = t;
} else if (sys->type == SYSLINK_RADIO_ADDRESS) {
_params_ack[2] = t;
}
}
void
Syslink::handle_raw(syslink_message_t *sys)
{
crtp_message_t *c = (crtp_message_t *) &sys->length;
if (CRTP_NULL(*c)) {
if (c->size >= 3) {
handle_bootloader(sys);
}
_null_count++;
} else if (c->port == CRTP_PORT_COMMANDER) {
crtp_commander *cmd = (crtp_commander *) &c->data[0];
input_rc_s rc = {};
rc.timestamp = hrt_absolute_time();
rc.timestamp_last_signal = rc.timestamp;
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) {
_count_in++;
/* Pipe to Mavlink bridge */
_bridge->pipe_message(c);
} else {
handle_raw_other(sys);
}
// Block all non-requested messaged in bootloader mode
if (_bootloader_mode) {
return;
}
// Allow one raw message to be sent from the queue
send_queued_raw_message();
}
void
Syslink::handle_bootloader(syslink_message_t *sys)
{
// Minimal bootloader emulation for being detectable
// To the bitcraze utilities, the STM32 will appear to have no flashable pages
// Upon receiving a bootloader message, all outbound packets are blocked in 'bootloader mode' due to how fragile the aforementioned utilities are to extra packets
crtp_message_t *c = (crtp_message_t *) &sys->length;
uint8_t target = c->data[0];
uint8_t cmd = c->data[1];
if (target != 0xFF) { // CF2 STM32 target
return;
}
_bootloader_mode = true;
if (cmd == 0x10) { // GET_INFO
c->size = 1 + 23;
memset(&c->data[2], 0, 21);
c->data[22] = 0x10; // Protocol version
send_message(sys);
}
}
void
Syslink::handle_raw_other(syslink_message_t *sys)
{
// This function doesn't actually do anything
// It is just here to return null responses to most standard messages
crtp_message_t *c = (crtp_message_t *) &sys->length;
if (c->port == CRTP_PORT_LOG) {
PX4_INFO("Log: %d %d", c->channel, c->data[0]);
if (c->channel == 0) { // Table of Contents Access
uint8_t cmd = c->data[0];
if (cmd == 0) { // GET_ITEM
//int id = c->data[1];
memset(&c->data[2], 0, 3);
c->data[2] = 1; // type
c->size = 1 + 5;
send_message(sys);
} else if (cmd == 1) { // GET_INFO
memset(&c->data[1], 0, 7);
c->size = 1 + 8;
send_message(sys);
}
} else if (c->channel == 1) { // Log control
uint8_t cmd = c->data[0];
PX4_INFO("Responding to cmd: %d", cmd);
c->data[2] = 0; // Success
c->size = 3 + 1;
// resend message
send_message(sys);
} else if (c->channel == 2) { // Log data
}
} else if (c->port == CRTP_PORT_MEM) {
if (c->channel == 0) { // Info
int cmd = c->data[0];
if (cmd == 1) { // GET_NBR_OF_MEMS
c->data[1] = 0;
c->size = 2 + 1;
// resend message
send_message(sys);
}
}
} else if (c->port == CRTP_PORT_PARAM) {
if (c->channel == 0) { // TOC Access
// uint8_t msgId = c->data[0];
c->data[1] = 0; // Last parameter (id = 0)
memset(&c->data[2], 0, 10);
c->size = 1 + 8;
send_message(sys);
}
else if (c->channel == 1) { // Param read
// 0 is ok
c->data[1] = 0; // value
c->size = 1 + 2;
send_message(sys);
}
} else {
PX4_INFO("Got raw: %d", c->port);
}
}
int
Syslink::send_bytes(const void *data, size_t len)
{
// TODO: This could be way more efficient
// Using interrupts/DMA/polling would be much better
for (size_t i = 0; i < len; i++) {
// Block until we can send a byte
while (px4_arch_gpioread(GPIO_NRF_TXEN)) ;
write(_fd, ((const char *)data) + i, 1);
}
return 0;
}
int
Syslink::send_message(syslink_message_t *msg)
{
syslink_compute_cksum(msg);
send_bytes(syslink_magic, 2);
send_bytes(&msg->type, sizeof(msg->type));
send_bytes(&msg->length, sizeof(msg->length));
send_bytes(&msg->data, msg->length);
send_bytes(&msg->cksum, sizeof(msg->cksum));
return 0;
}
namespace syslink
{
void usage();
void start();
void status();
void test();
void attached(int pid);
void usage()
{
warnx("missing command: try 'start', 'status', 'attached', 'test'");
}
void start()
{
if (g_syslink != nullptr) {
printf("Already started\n");
exit(1);
}
g_syslink = new Syslink();
g_syslink->start();
// Wait for task and bridge to start
usleep(5000);
warnx("Started syslink on /dev/ttyS2\n");
exit(0);
}
void status()
{
if (g_syslink == nullptr) {
printf("Please start syslink first\n");
exit(1);
}
printf("Connection activity:\n");
printf("- total rx: %d p/s\n", g_syslink->pktrate);
printf("- radio rx: %d p/s (%d null)\n", g_syslink->rxrate, g_syslink->nullrate);
printf("- radio tx: %d p/s\n\n", g_syslink->txrate);
printf("Parameter Status:\n");
const char *goodParam = "good";
const char *badParam = "fail!";
printf("- channel: %s\n", g_syslink->is_good(0) ? goodParam : badParam);
printf("- data rate: %s\n", g_syslink->is_good(1) != 0 ? goodParam : badParam);
printf("- address: %s\n\n", g_syslink->is_good(2) != 0 ? goodParam : badParam);
int deckfd = open(DECK_DEVICE_PATH, O_RDONLY);
int ndecks = 0; ioctl(deckfd, DECKIOGNUM, (unsigned long) &ndecks);
printf("Deck scan: (found %d)\n", ndecks);
for (int i = 0; i < ndecks; i++) {
ioctl(deckfd, DECKIOSNUM, (unsigned long) &i);
uint8_t *id;
int idlen = ioctl(deckfd, DECKIOID, (unsigned long) &id);
// TODO: Validate the ID
printf("%i: ROM ID: ", i);
for (int idi = 0; idi < idlen; idi++) {
printf("%02X", id[idi]);
}
deck_descriptor_t desc;
read(deckfd, &desc, sizeof(desc));
printf(", VID: %02X , PID: %02X\n", desc.header, desc.vendorId, desc.productId);
// Print pages of memory
for (size_t di = 0; di < sizeof(desc); di++) {
if (di % 16 == 0) {
printf("\n");
}
printf("%02X ", ((uint8_t *)&desc)[di]);
}
printf("\n\n");
}
close(deckfd);
exit(0);
}
void attached(int pid)
{
bool found = false;
int deckfd = open(DECK_DEVICE_PATH, O_RDONLY);
int ndecks = 0; ioctl(deckfd, DECKIOGNUM, (unsigned long) &ndecks);
for (int i = 0; i < ndecks; i++) {
ioctl(deckfd, DECKIOSNUM, (unsigned long) &i);
deck_descriptor_t desc;
read(deckfd, &desc, sizeof(desc));
if (desc.productId == pid) {
found = true;
break;
}
}
close(deckfd);
exit(found ? 1 : 0);
}
void test()
{
// TODO: Ensure battery messages are recent
// TODO: Read and write from memory to ensure it is working
}
}
int syslink_main(int argc, char *argv[])
{
if (argc < 2) {
syslink::usage();
exit(1);
}
const char *verb = argv[1];
if (!strcmp(verb, "start")) {
syslink::start();
}
if (!strcmp(verb, "status")) {
syslink::status();
}
if (!strcmp(verb, "attached")) {
if (argc != 3) {
errx(1, "usage: syslink attached [deck_pid]");
}
int pid = atoi(argv[2]);
syslink::attached(pid);
}
if (!strcmp(verb, "test")) {
syslink::test();
}
syslink::usage();
exit(1);
return 0;
}