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

First version working with new Serial API on voxl2

This commit is contained in:
Eric Katzfey 2024-03-06 16:30:59 -08:00
parent 313003d2ac
commit 129b11424c
4 changed files with 64 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/modalai/voxl2-slpi/default.px4board
View File

@ -10,6 +10,7 @@ CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
CONFIG_DRIVERS_RC_CRSF_RC=y
CONFIG_DRIVERS_QSHELL_QURT=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y

2
src/drivers/rc/crsf_rc/CMakeLists.txt
View File

@ -46,6 +46,4 @@ px4_add_module(
MODULE_CONFIG
module.yaml
DEPENDS
rc
)

117
src/drivers/rc/crsf_rc/CrsfRc.cpp
View File

@ -35,8 +35,6 @@
#include "CrsfParser.hpp"
#include "Crc8.hpp"
#include <poll.h>
#include <termios.h>
#include <fcntl.h>
#include <uORB/topics/battery_status.h>
@ -118,76 +116,76 @@ void CrsfRc::Run()
{
if (should_exit()) {
ScheduleClear();
::close(_rc_fd);
_rc_fd = -1;
if (_uart) {
(void) _uart->close();
delete _uart;
_uart = nullptr;
}
exit_and_cleanup();
return;
}
if (_rc_fd < 0) {
_rc_fd = ::open(_device, O_RDWR | O_NONBLOCK);
if (_uart == nullptr) {
// Create the UART port instance
_uart = new Serial(_device);
if (_rc_fd >= 0) {
struct termios t;
tcgetattr(_rc_fd, &t);
cfsetspeed(&t, CRSF_BAUDRATE);
t.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
t.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
t.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
t.c_oflag = 0;
tcsetattr(_rc_fd, TCSANOW, &t);
if (board_rc_swap_rxtx(_device)) {
#if defined(TIOCSSWAP)
ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED);
#endif // TIOCSSWAP
}
if (board_rc_singlewire(_device)) {
_is_singlewire = true;
#if defined(TIOCSSINGLEWIRE)
ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
#endif // TIOCSSINGLEWIRE
}
PX4_INFO("Crsf serial opened sucessfully");
if (_is_singlewire) {
PX4_INFO("Crsf serial is single wire. Telemetry disabled");
}
tcflush(_rc_fd, TCIOFLUSH);
Crc8Init(0xd5);
if (_uart == nullptr) {
PX4_ERR("Error creating serial device %s", _device);
px4_sleep(1);
return;
}
}
if (! _uart->isOpen()) {
// Configure the desired baudrate if one was specified by the user.
// Otherwise the default baudrate will be used.
if (! _uart->setBaudrate(CRSF_BAUDRATE)) {
PX4_ERR("Error setting baudrate to %u on %s", CRSF_BAUDRATE, _device);
px4_sleep(1);
return;
}
// Open the UART. If this is successful then the UART is ready to use.
if (! _uart->open()) {
PX4_ERR("Error opening serial device %s", _device);
px4_sleep(1);
return;
}
// if (board_rc_swap_rxtx(_device)) {
// #if defined(TIOCSSWAP)
// ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED);
// #endif // TIOCSSWAP
// }
//
// if (board_rc_singlewire(_device)) {
// _is_singlewire = true;
// #if defined(TIOCSSINGLEWIRE)
// ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
// #endif // TIOCSSINGLEWIRE
// }
//
// PX4_INFO("Crsf serial opened sucessfully");
//
// if (_is_singlewire) {
// PX4_INFO("Crsf serial is single wire. Telemetry disabled");
// }
//
// tcflush(_rc_fd, TCIOFLUSH);
Crc8Init(0xd5);
_input_rc.rssi_dbm = NAN;
_input_rc.link_quality = -1;
CrsfParser_Init();
}
// poll with 100mS timeout
pollfd fds[1];
fds[0].fd = _rc_fd;
fds[0].events = POLLIN;
int ret = ::poll(fds, 1, 100);
if (ret < 0) {
PX4_ERR("poll error");
// try again with delay
ScheduleDelayed(100_ms);
return;
}
const hrt_abstime time_now_us = hrt_absolute_time();
perf_count_interval(_cycle_interval_perf, time_now_us);
// Read all available data from the serial RC input UART
int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
int new_bytes = _uart->readAtLeast(&_rcs_buf[0], RC_MAX_BUFFER_SIZE, 1, 100);
if (new_bytes > 0) {
_bytes_rx += new_bytes;
@ -433,7 +431,8 @@ bool CrsfRc::SendTelemetryBattery(const uint16_t voltage, const uint16_t current
write_uint24_t(buf, offset, fuel);
write_uint8_t(buf, offset, remaining);
WriteFrameCrc(buf, offset, sizeof(buf));
return write(_rc_fd, buf, offset) == offset;
return _uart->write((void *) buf, (size_t) offset);
}
bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed,
@ -449,7 +448,7 @@ bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, c
write_uint16_t(buf, offset, altitude);
write_uint8_t(buf, offset, num_satellites);
WriteFrameCrc(buf, offset, sizeof(buf));
return write(_rc_fd, buf, offset) == offset;
return _uart->write((void *) buf, (size_t) offset);
}
bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw)
@ -461,7 +460,7 @@ bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, cons
write_uint16_t(buf, offset, roll);
write_uint16_t(buf, offset, yaw);
WriteFrameCrc(buf, offset, sizeof(buf));
return write(_rc_fd, buf, offset) == offset;
return _uart->write((void *) buf, (size_t) offset);
}
bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode)
@ -480,7 +479,7 @@ bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode)
offset += length;
buf[offset - 1] = 0; // ensure null-terminated string
WriteFrameCrc(buf, offset, length + 4);
return write(_rc_fd, buf, offset) == offset;
return _uart->write((void *) buf, (size_t) offset);
}
int CrsfRc::print_status()

6
src/drivers/rc/crsf_rc/CrsfRc.hpp
View File

@ -40,6 +40,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/Serial.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
@ -53,6 +54,8 @@
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_status.h>
using namespace device;
class CrsfRc : public ModuleBase<CrsfRc>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
@ -87,7 +90,8 @@ private:
bool SendTelemetryFlightMode(const char *flight_mode);
int _rc_fd{-1};
Serial *_uart = nullptr; ///< UART interface to RC
char _device[20] {}; ///< device / serial port path
bool _is_singlewire{false};