forked from Archive/PX4-Autopilot
Compare commits
7 Commits
main
...
pr-crsf_ne
Author | SHA1 | Date |
---|---|---|
Eric Katzfey | a6c1105384 | |
Eric Katzfey | eb9f71892f | |
Eric Katzfey | d6f3a206e0 | |
Eric Katzfey | 3ab3fe2ee7 | |
Eric Katzfey | e02c01bd21 | |
Eric Katzfey | 9bc7b55af1 | |
Eric Katzfey | d641a92b61 |
|
@ -11,6 +11,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
|
||||
|
|
|
@ -80,6 +80,11 @@ ssize_t Serial::write(const void *buffer, size_t buffer_size)
|
|||
return _impl.write(buffer, buffer_size);
|
||||
}
|
||||
|
||||
void Serial::flush()
|
||||
{
|
||||
return _impl.flush();
|
||||
}
|
||||
|
||||
uint32_t Serial::getBaudrate() const
|
||||
{
|
||||
return _impl.getBaudrate();
|
||||
|
@ -130,6 +135,24 @@ bool Serial::setFlowcontrol(FlowControl flowcontrol)
|
|||
return _impl.setFlowcontrol(flowcontrol);
|
||||
}
|
||||
|
||||
bool Serial::getSingleWireMode() const
|
||||
{
|
||||
return _impl.getSingleWireMode();
|
||||
}
|
||||
bool Serial::setSingleWireMode()
|
||||
{
|
||||
return _impl.setSingleWireMode();
|
||||
}
|
||||
|
||||
bool Serial::getSwapRxTxMode() const
|
||||
{
|
||||
return _impl.getSwapRxTxMode();
|
||||
}
|
||||
bool Serial::setSwapRxTxMode()
|
||||
{
|
||||
return _impl.setSwapRxTxMode();
|
||||
}
|
||||
|
||||
const char *Serial::getPort() const
|
||||
{
|
||||
return _impl.getPort();
|
||||
|
|
|
@ -64,6 +64,8 @@ public:
|
|||
|
||||
ssize_t write(const void *buffer, size_t buffer_size);
|
||||
|
||||
void flush();
|
||||
|
||||
// If port is already open then the following configuration functions
|
||||
// will reconfigure the port. If the port is not yet open then they will
|
||||
// simply store the configuration in preparation for the port to be opened.
|
||||
|
@ -83,6 +85,12 @@ public:
|
|||
FlowControl getFlowcontrol() const;
|
||||
bool setFlowcontrol(FlowControl flowcontrol);
|
||||
|
||||
bool getSingleWireMode() const;
|
||||
bool setSingleWireMode();
|
||||
|
||||
bool getSwapRxTxMode() const;
|
||||
bool setSwapRxTxMode();
|
||||
|
||||
const char *getPort() const;
|
||||
|
||||
private:
|
||||
|
|
|
@ -210,6 +210,14 @@ bool SerialImpl::open()
|
|||
|
||||
_open = true;
|
||||
|
||||
if (_single_wire_mode) {
|
||||
setSingleWireMode();
|
||||
}
|
||||
|
||||
if (_swap_rx_tx_mode) {
|
||||
setSwapRxTxMode();
|
||||
}
|
||||
|
||||
return _open;
|
||||
}
|
||||
|
||||
|
@ -319,6 +327,13 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
|
|||
return written;
|
||||
}
|
||||
|
||||
void SerialImpl::flush()
|
||||
{
|
||||
if (_open) {
|
||||
tcflush(_serial_fd, TCIOFLUSH);
|
||||
}
|
||||
}
|
||||
|
||||
const char *SerialImpl::getPort() const
|
||||
{
|
||||
return _port;
|
||||
|
@ -391,4 +406,44 @@ bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
|
|||
return flowcontrol == FlowControl::Disabled;
|
||||
}
|
||||
|
||||
bool SerialImpl::getSingleWireMode() const
|
||||
{
|
||||
return _single_wire_mode;
|
||||
}
|
||||
|
||||
bool SerialImpl::setSingleWireMode()
|
||||
{
|
||||
#if defined(TIOCSSINGLEWIRE)
|
||||
|
||||
if (_open) {
|
||||
ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
|
||||
}
|
||||
|
||||
_single_wire_mode = true;
|
||||
return true;
|
||||
#else
|
||||
return false;
|
||||
#endif // TIOCSSINGLEWIRE
|
||||
}
|
||||
|
||||
bool SerialImpl::getSwapRxTxMode() const
|
||||
{
|
||||
return _swap_rx_tx_mode;
|
||||
}
|
||||
|
||||
bool SerialImpl::setSwapRxTxMode()
|
||||
{
|
||||
#if defined(TIOCSSWAP)
|
||||
|
||||
if (_open) {
|
||||
ioctl(_serial_fd, TIOCSSWAP, SER_SWAP_ENABLED);
|
||||
}
|
||||
|
||||
_swap_rx_tx_mode = true;
|
||||
return true;
|
||||
#else
|
||||
return false;
|
||||
#endif // TIOCSSWAP
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
|
|
|
@ -64,6 +64,8 @@ public:
|
|||
|
||||
ssize_t write(const void *buffer, size_t buffer_size);
|
||||
|
||||
void flush();
|
||||
|
||||
const char *getPort() const;
|
||||
|
||||
uint32_t getBaudrate() const;
|
||||
|
@ -81,6 +83,12 @@ public:
|
|||
FlowControl getFlowcontrol() const;
|
||||
bool setFlowcontrol(FlowControl flowcontrol);
|
||||
|
||||
bool getSingleWireMode() const;
|
||||
bool setSingleWireMode();
|
||||
|
||||
bool getSwapRxTxMode() const;
|
||||
bool setSwapRxTxMode();
|
||||
|
||||
private:
|
||||
|
||||
int _serial_fd{-1};
|
||||
|
@ -99,6 +107,8 @@ private:
|
|||
bool validateBaudrate(uint32_t baudrate);
|
||||
bool configure();
|
||||
|
||||
bool _single_wire_mode{false};
|
||||
bool _swap_rx_tx_mode{false};
|
||||
};
|
||||
|
||||
} // namespace device
|
||||
|
|
|
@ -46,6 +46,7 @@ target_link_libraries(px4_layer
|
|||
|
||||
add_library(px4_kernel_layer
|
||||
${KERNEL_SRCS}
|
||||
SerialImpl.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(px4_kernel_layer
|
||||
|
|
|
@ -64,6 +64,8 @@ public:
|
|||
|
||||
ssize_t write(const void *buffer, size_t buffer_size);
|
||||
|
||||
void flush();
|
||||
|
||||
const char *getPort() const;
|
||||
|
||||
uint32_t getBaudrate() const;
|
||||
|
@ -81,6 +83,12 @@ public:
|
|||
FlowControl getFlowcontrol() const;
|
||||
bool setFlowcontrol(FlowControl flowcontrol);
|
||||
|
||||
bool getSingleWireMode() const;
|
||||
bool setSingleWireMode();
|
||||
|
||||
bool getSwapRxTxMode() const;
|
||||
bool setSwapRxTxMode();
|
||||
|
||||
private:
|
||||
|
||||
int _serial_fd{-1};
|
||||
|
@ -98,6 +106,9 @@ private:
|
|||
|
||||
bool validateBaudrate(uint32_t baudrate);
|
||||
bool configure();
|
||||
|
||||
bool _single_wire_mode{false};
|
||||
bool _swap_rx_tx_mode{false};
|
||||
};
|
||||
|
||||
} // namespace device
|
||||
|
|
|
@ -208,6 +208,14 @@ bool SerialImpl::open()
|
|||
|
||||
_open = true;
|
||||
|
||||
if (_single_wire_mode) {
|
||||
setSingleWireMode();
|
||||
}
|
||||
|
||||
if (_swap_rx_tx_mode) {
|
||||
setSwapRxTxMode();
|
||||
}
|
||||
|
||||
return _open;
|
||||
}
|
||||
|
||||
|
@ -312,6 +320,13 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
|
|||
return written;
|
||||
}
|
||||
|
||||
void SerialImpl::flush()
|
||||
{
|
||||
if (_open) {
|
||||
tcflush(_serial_fd, TCIOFLUSH);
|
||||
}
|
||||
}
|
||||
|
||||
const char *SerialImpl::getPort() const
|
||||
{
|
||||
return _port;
|
||||
|
@ -384,4 +399,44 @@ bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
|
|||
return flowcontrol == FlowControl::Disabled;
|
||||
}
|
||||
|
||||
bool SerialImpl::getSingleWireMode() const
|
||||
{
|
||||
return _single_wire_mode;
|
||||
}
|
||||
|
||||
bool SerialImpl::setSingleWireMode()
|
||||
{
|
||||
#if defined(TIOCSSINGLEWIRE)
|
||||
|
||||
if (_open) {
|
||||
ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
|
||||
}
|
||||
|
||||
_single_wire_mode = true;
|
||||
return true;
|
||||
#else
|
||||
return false;
|
||||
#endif // TIOCSSINGLEWIRE
|
||||
}
|
||||
|
||||
bool SerialImpl::getSwapRxTxMode() const
|
||||
{
|
||||
return _swap_rx_tx_mode;
|
||||
}
|
||||
|
||||
bool SerialImpl::setSwapRxTxMode()
|
||||
{
|
||||
#if defined(TIOCSSWAP)
|
||||
|
||||
if (_open) {
|
||||
ioctl(_serial_fd, TIOCSSWAP, SER_SWAP_ENABLED);
|
||||
}
|
||||
|
||||
_swap_rx_tx_mode = true;
|
||||
return true;
|
||||
#else
|
||||
return false;
|
||||
#endif // TIOCSSWAP
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
|
|
|
@ -63,6 +63,8 @@ public:
|
|||
|
||||
ssize_t write(const void *buffer, size_t buffer_size);
|
||||
|
||||
void flush();
|
||||
|
||||
const char *getPort() const;
|
||||
bool setPort(const char *port);
|
||||
|
||||
|
@ -81,6 +83,12 @@ public:
|
|||
FlowControl getFlowcontrol() const;
|
||||
bool setFlowcontrol(FlowControl flowcontrol);
|
||||
|
||||
bool getSingleWireMode() const;
|
||||
bool setSingleWireMode();
|
||||
|
||||
bool getSwapRxTxMode() const;
|
||||
bool setSwapRxTxMode();
|
||||
|
||||
private:
|
||||
|
||||
int _serial_fd{-1};
|
||||
|
|
|
@ -251,6 +251,11 @@ ssize_t SerialImpl::write(const void *buffer, size_t buffer_size)
|
|||
return ret_write;
|
||||
}
|
||||
|
||||
void SerialImpl::flush()
|
||||
{
|
||||
// TODO: Flush not implemented yet on Qurt
|
||||
}
|
||||
|
||||
const char *SerialImpl::getPort() const
|
||||
{
|
||||
return _port;
|
||||
|
@ -323,4 +328,26 @@ bool SerialImpl::setFlowcontrol(FlowControl flowcontrol)
|
|||
return flowcontrol == FlowControl::Disabled;
|
||||
}
|
||||
|
||||
bool SerialImpl::getSingleWireMode() const
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
bool SerialImpl::setSingleWireMode()
|
||||
{
|
||||
// Qurt platform does not support single wire mode
|
||||
return false;
|
||||
}
|
||||
|
||||
bool SerialImpl::getSwapRxTxMode() const
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
bool SerialImpl::setSwapRxTxMode()
|
||||
{
|
||||
// Qurt platform does not support swap rx tx mode
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
|
|
|
@ -46,6 +46,4 @@ px4_add_module(
|
|||
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
rc
|
||||
)
|
||||
|
|
|
@ -161,6 +161,8 @@ static float MapF(const float x, const float in_min, const float in_max, const f
|
|||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||
}
|
||||
|
||||
#define CONSTRAIN_CHAN(x) ConstrainF(x, CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX)
|
||||
|
||||
static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet)
|
||||
{
|
||||
uint32_t raw_channels[CRSF_CHANNEL_COUNT];
|
||||
|
@ -169,25 +171,24 @@ static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPac
|
|||
new_packet->message_type = CRSF_MESSAGE_TYPE_RC_CHANNELS;
|
||||
|
||||
// Decode channel data
|
||||
raw_channels[0] = (data[0] | data[1] << 8) & 0x07FF;
|
||||
raw_channels[1] = (data[1] >> 3 | data[2] << 5) & 0x07FF;
|
||||
raw_channels[2] = (data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF;
|
||||
raw_channels[3] = (data[4] >> 1 | data[5] << 7) & 0x07FF;
|
||||
raw_channels[4] = (data[5] >> 4 | data[6] << 4) & 0x07FF;
|
||||
raw_channels[5] = (data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF;
|
||||
raw_channels[6] = (data[8] >> 2 | data[9] << 6) & 0x07FF;
|
||||
raw_channels[7] = (data[9] >> 5 | data[10] << 3) & 0x07FF;
|
||||
raw_channels[8] = (data[11] | data[12] << 8) & 0x07FF;
|
||||
raw_channels[9] = (data[12] >> 3 | data[13] << 5) & 0x07FF;
|
||||
raw_channels[10] = (data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF;
|
||||
raw_channels[11] = (data[15] >> 1 | data[16] << 7) & 0x07FF;
|
||||
raw_channels[12] = (data[16] >> 4 | data[17] << 4) & 0x07FF;
|
||||
raw_channels[13] = (data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF;
|
||||
raw_channels[14] = (data[19] >> 2 | data[20] << 6) & 0x07FF;
|
||||
raw_channels[15] = (data[20] >> 5 | data[21] << 3) & 0x07FF;
|
||||
raw_channels[0] = CONSTRAIN_CHAN((data[0] | data[1] << 8) & 0x07FF);
|
||||
raw_channels[1] = CONSTRAIN_CHAN((data[1] >> 3 | data[2] << 5) & 0x07FF);
|
||||
raw_channels[2] = CONSTRAIN_CHAN((data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF);
|
||||
raw_channels[3] = CONSTRAIN_CHAN((data[4] >> 1 | data[5] << 7) & 0x07FF);
|
||||
raw_channels[4] = CONSTRAIN_CHAN((data[5] >> 4 | data[6] << 4) & 0x07FF);
|
||||
raw_channels[5] = CONSTRAIN_CHAN((data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF);
|
||||
raw_channels[6] = CONSTRAIN_CHAN((data[8] >> 2 | data[9] << 6) & 0x07FF);
|
||||
raw_channels[7] = CONSTRAIN_CHAN((data[9] >> 5 | data[10] << 3) & 0x07FF);
|
||||
raw_channels[8] = CONSTRAIN_CHAN((data[11] | data[12] << 8) & 0x07FF);
|
||||
raw_channels[9] = CONSTRAIN_CHAN((data[12] >> 3 | data[13] << 5) & 0x07FF);
|
||||
raw_channels[10] = CONSTRAIN_CHAN((data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF);
|
||||
raw_channels[11] = CONSTRAIN_CHAN((data[15] >> 1 | data[16] << 7) & 0x07FF);
|
||||
raw_channels[12] = CONSTRAIN_CHAN((data[16] >> 4 | data[17] << 4) & 0x07FF);
|
||||
raw_channels[13] = CONSTRAIN_CHAN((data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF);
|
||||
raw_channels[14] = CONSTRAIN_CHAN((data[19] >> 2 | data[20] << 6) & 0x07FF);
|
||||
raw_channels[15] = CONSTRAIN_CHAN((data[20] >> 5 | data[21] << 3) & 0x07FF);
|
||||
|
||||
for (i = 0; i < CRSF_CHANNEL_COUNT; i++) {
|
||||
raw_channels[i] = ConstrainF(raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX);
|
||||
new_packet->channel_data.channels[i] = MapF((float)raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX,
|
||||
1000.0f, 2000.0f);
|
||||
}
|
||||
|
|
|
@ -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,37 +116,51 @@ 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;
|
||||
if (_uart == nullptr) {
|
||||
PX4_ERR("Error creating serial device %s", _device);
|
||||
px4_sleep(1);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
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 (! _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
|
||||
_uart->setSwapRxTxMode();
|
||||
}
|
||||
|
||||
if (board_rc_singlewire(_device)) {
|
||||
_is_singlewire = true;
|
||||
#if defined(TIOCSSINGLEWIRE)
|
||||
ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
|
||||
#endif // TIOCSSINGLEWIRE
|
||||
_uart->setSingleWireMode();
|
||||
}
|
||||
|
||||
PX4_INFO("Crsf serial opened sucessfully");
|
||||
|
@ -157,37 +169,21 @@ void CrsfRc::Run()
|
|||
PX4_INFO("Crsf serial is single wire. Telemetry disabled");
|
||||
}
|
||||
|
||||
tcflush(_rc_fd, TCIOFLUSH);
|
||||
_uart->flush();
|
||||
|
||||
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 +429,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 +446,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 +458,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 +477,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()
|
||||
|
|
|
@ -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};
|
||||
|
||||
|
|
Loading…
Reference in New Issue