fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 18:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
8338f4e5433e00a5cec8e30f378deb82dd5e9d13
PX4-Autopilot/boards/bitcraze/crazyflie/syslink/syslink_main.cpp
T
Julian Oes f650b91718 battery: check source param inside battery lib 
This moves the handling of the BAT%d_SOURCE param inside of the battery
library. Users of the library now pass the source instead of the flag
whether to publish. The battery library then checks if the source is
selected using the param and publishes accordingly.

Since we removed the strange system_source flag, we now need to look at
all batteries in commander.
For current estimation - I think - it makes sense to sum them up.
2020-04-06 15:56:54 +02:00

884 lines
18 KiB
C++
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_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/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 <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)),
_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();
// 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,
battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0);
// 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 && vbat > 3.7f) {
_bstate = BAT_CHARGED;
} else {
_bstate = BAT_DISCHARGING;
}
} 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
_rc_pub.publish(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;
}
Reference in New Issue View Git Blame Copy Permalink