diff --git a/platforms/nuttx/nuttx-configs/crazyflie/nsh/defconfig b/platforms/nuttx/nuttx-configs/crazyflie/nsh/defconfig index 4a3ac530c517d519b6ebf5af5c4059288130ac12..c30fa72cf707f07ea10dcba6fa44ef341f1fdb36 100644 --- a/platforms/nuttx/nuttx-configs/crazyflie/nsh/defconfig +++ b/platforms/nuttx/nuttx-configs/crazyflie/nsh/defconfig @@ -931,7 +931,7 @@ CONFIG_DEV_FIFO_SIZE=1024 # CONFIG_POWER is not set # CONFIG_SENSORS is not set CONFIG_SERIAL=y -# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_DEV_LOWCONSOLE=y CONFIG_SERIAL_REMOVABLE=y # CONFIG_SERIAL_CONSOLE is not set # CONFIG_16550_UART is not set diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h index 671e420af49db61c21cb45c01c7383eefb6bf6b1..2000b7e81aecad9b639d3f4fe2ef96ba3a0cdbbf 100644 --- a/src/drivers/boards/crazyflie/board_config.h +++ b/src/drivers/boards/crazyflie/board_config.h @@ -168,7 +168,7 @@ #define PX4_PWM_ALTERNATE_RANGES #define PWM_LOWEST_MIN 0 #define PWM_MOTOR_OFF 0 -#define PWM_DEFAULT_MIN 0 +#define PWM_DEFAULT_MIN 20 #define PWM_HIGHEST_MIN 0 #define PWM_HIGHEST_MAX 255 #define PWM_DEFAULT_MAX 255 diff --git a/src/modules/syslink/syslink_bridge.cpp b/src/modules/syslink/syslink_bridge.cpp index 2455e2d2418e466945351898a5618d5842a681e3..d3300a735572aa00056e8cff4aff9aa7c0c42643 100644 --- a/src/modules/syslink/syslink_bridge.cpp +++ b/src/modules/syslink/syslink_bridge.cpp @@ -48,8 +48,10 @@ SyslinkBridge::SyslinkBridge(Syslink *link) : _link(link), _readbuffer(16, sizeof(crtp_message_t)) { - - + _msg_to_send.header = 0; + _msg_to_send.size = sizeof(_msg_to_send.header); + _msg_to_send.port = CRTP_PORT_MAVLINK; + _msg_to_send_size_remaining = CRTP_MAX_DATA_SIZE - 1; } SyslinkBridge::~SyslinkBridge() @@ -111,21 +113,28 @@ SyslinkBridge::read(struct file *filp, char *buffer, size_t buflen) ssize_t SyslinkBridge::write(struct file *filp, const char *buffer, size_t buflen) { - crtp_message_t msg; - - // Queue and send next time we get a RAW radio packet - int remaining = buflen; + int buflen_rem = buflen; - while (remaining > 0) { - int datasize = MIN(remaining, CRTP_MAX_DATA_SIZE); - msg.size = datasize + sizeof(msg.header); - msg.port = CRTP_PORT_MAVLINK; - memcpy(&msg.data, buffer, datasize); + while (buflen_rem > 0) { - _link->_writebuffer.force(&msg, sizeof(crtp_message_t)); + int datasize = MIN(_msg_to_send_size_remaining, buflen_rem); + _msg_to_send.size += datasize; + memcpy(&_msg_to_send.data[CRTP_MAX_DATA_SIZE - 1 - _msg_to_send_size_remaining], buffer, datasize); buffer += datasize; - remaining -= datasize; + _msg_to_send_size_remaining -= datasize; + buflen_rem -= datasize; + + + if (_msg_to_send_size_remaining == 0) { + + if (_link->_writebuffer.force(&_msg_to_send, sizeof(crtp_message_t))) { + PX4_WARN("write buffer overflow"); + } + + _msg_to_send.size = sizeof(_msg_to_send.header); + _msg_to_send_size_remaining = CRTP_MAX_DATA_SIZE - 1; + } } return buflen; diff --git a/src/modules/syslink/syslink_main.cpp b/src/modules/syslink/syslink_main.cpp index 4f859cbc9126df65f044bc24a1023cc1b3843923..69b6a41fa502d641cfbf370ce0fd96094eba2a91 100644 --- a/src/modules/syslink/syslink_main.cpp +++ b/src/modules/syslink/syslink_main.cpp @@ -47,7 +47,6 @@ #include <string.h> #include <fcntl.h> #include <sys/ioctl.h> -#include <poll.h> #include <termios.h> #include <sys/types.h> #include <sys/stat.h> @@ -70,6 +69,7 @@ #include "drv_deck.h" + __BEGIN_DECLS extern void led_init(void); extern void led_on(int led); @@ -254,7 +254,7 @@ Syslink::open_serial(const char *dev) tcgetattr(fd, &config); // clear ONLCR flag (which appends a CR for every LF) - config.c_oflag &= 0; + config.c_oflag = 0; config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); // Disable hardware flow control @@ -336,15 +336,16 @@ Syslink::task_main() syslink_parse_init(&state); - // setup initial parameters + //setup initial parameters update_params(true); while (_task_running) { - int poll_ret = px4_poll(fds, 2, 1000); + int poll_ret = px4_poll(fds, 2, 500); /* handle the poll result */ if (poll_ret == 0) { - /* this means none of our providers is giving us data */ + /* 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) { @@ -374,6 +375,7 @@ Syslink::task_main() update_params(false); } } + } close(_fd); @@ -460,7 +462,7 @@ Syslink::handle_message(syslink_message_t *msg) PX4_INFO("GOT %d", msg->type); } - // Send queued messages + //Send queued messages if (!_queue.empty()) { _queue.get(msg, sizeof(syslink_message_t)); send_message(msg); @@ -706,7 +708,6 @@ 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)) ; diff --git a/src/modules/syslink/syslink_main.h b/src/modules/syslink/syslink_main.h index d78fa2e1e88d0a8905ad2a2b1e50a5dd77babecc..b335396dd59fe43fe4af7d1cfd1dd41b51a8dd02 100644 --- a/src/modules/syslink/syslink_main.h +++ b/src/modules/syslink/syslink_main.h @@ -181,6 +181,8 @@ private: // Stores data that was received from syslink but not yet read by another driver ringbuffer::RingBuffer _readbuffer; + crtp_message_t _msg_to_send; + int _msg_to_send_size_remaining; };