Skip to content

Commit

Permalink
crsf_rc: add bind command
Browse files Browse the repository at this point in the history
  • Loading branch information
benjinne committed Jun 18, 2024
1 parent 9c83f84 commit 301f969
Show file tree
Hide file tree
Showing 2 changed files with 95 additions and 5 deletions.
68 changes: 63 additions & 5 deletions src/drivers/rc/crsf_rc/CrsfRc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,11 +37,6 @@

#include <fcntl.h>

#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_status.h>

using namespace time_literals;

#define CRSF_BAUDRATE 420000
Expand Down Expand Up @@ -182,6 +177,40 @@ void CrsfRc::Run()
const hrt_abstime time_now_us = hrt_absolute_time();
perf_count_interval(_cycle_interval_perf, time_now_us);

if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;

if (_vehicle_status_sub.copy(&vehicle_status)) {
_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
}
}

/* vehicle command */
vehicle_command_s vcmd;

if (_vehicle_cmd_sub.update(&vcmd)) {
// Check for a pairing command
if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {

uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;

if (!_armed) {
BindCRSF();
cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
}

// publish acknowledgement
vehicle_command_ack_s command_ack{};
command_ack.command = vcmd.command;
command_ack.result = cmd_ret;
command_ack.target_system = vcmd.source_system;
command_ack.target_component = vcmd.source_component;
command_ack.timestamp = hrt_absolute_time();
uORB::Publication<vehicle_command_ack_s> vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
vehicle_command_ack_pub.publish(command_ack);
}
}

// Read all available data from the serial RC input UART
int new_bytes = _uart->readAtLeast(&_rcs_buf[0], RC_MAX_BUFFER_SIZE, 1, 100);

Expand Down Expand Up @@ -480,6 +509,23 @@ bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode)
return _uart->write((void *) buf, (size_t) offset);
}

bool CrsfRc::BindCRSF()
{
uint8_t bindFrame[] = {
(uint8_t)crsf_sync_t::sync,
0x07, // frame length
(uint8_t)crsf_frame_type_t::command,
(uint8_t)crsf_address_t::crsf_receiver,
(uint8_t)crsf_address_t::flight_controller,
(uint8_t)crsf_sub_command_t::subcmd_rx,
(uint8_t)crsf_sub_command_t::subcmd_rx_bind,
0x9E, // Command CRC8
0xE8, // Packet CRC8
};

return _uart->write((void *) bindFrame, 9);
}

int CrsfRc::print_status()
{
if (_device[0] != '\0') {
Expand Down Expand Up @@ -509,6 +555,17 @@ int CrsfRc::print_status()

int CrsfRc::custom_command(int argc, char *argv[])
{
const char *verb = argv[0];

if (!strcmp(verb, "bind")) {
uORB::Publication<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
vehicle_command_s vcmd{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
vcmd.timestamp = hrt_absolute_time();
vehicle_command_pub.publish(vcmd);
return 0;
}

return print_usage("unknown command");
}

Expand All @@ -528,6 +585,7 @@ This module parses the CRSF RC uplink protocol and generates CRSF downlink telem
PRINT_MODULE_USAGE_NAME("crsf_rc", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a bind command (not available on singlewire)");

PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

Expand Down
32 changes: 32 additions & 0 deletions src/drivers/rc/crsf_rc/CrsfRc.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,8 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>

using namespace device;

Expand Down Expand Up @@ -90,10 +92,13 @@ class CrsfRc : public ModuleBase<CrsfRc>, public ModuleParams, public px4::Sched

bool SendTelemetryFlightMode(const char *flight_mode);

bool BindCRSF();

Serial *_uart = nullptr; ///< UART interface to RC

char _device[20] {}; ///< device / serial port path
bool _is_singlewire{false};
bool _armed{false};

static constexpr size_t RC_MAX_BUFFER_SIZE{64};
uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
Expand All @@ -111,6 +116,7 @@ class CrsfRc : public ModuleBase<CrsfRc>, public ModuleParams, public px4::Sched
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};

enum class crsf_frame_type_t : uint8_t {
gps = 0x02,
Expand All @@ -137,6 +143,32 @@ class CrsfRc : public ModuleBase<CrsfRc>, public ModuleParams, public px4::Sched
attitude = 6,
};

enum class crsf_sync_t : uint8_t {
sync = 0xC8
};

enum class crsf_sub_command_t : uint8_t {
subcmd_rx = 0x10,
subcmd_general = 0x0A,
subcmd_rx_bind = 0x01,
};

enum class crsf_address_t : uint8_t {
broadcast = 0x00,
usb = 0x10,
tbs_core_pnp_pro = 0x80,
reserved1 = 0x8A,
current_sensor = 0xC0,
gps = 0xC2,
tbs_blackbox = 0xC4,
flight_controller = 0xC8,
reserved2 = 0xCA,
race_tag = 0xCC,
radio_transmitter = 0xEA,
crsf_receiver = 0xEC,
crsf_transmitter = 0xEE
} ;

void WriteFrameHeader(uint8_t *buf, int &offset, const crsf_frame_type_t type, const uint8_t payload_size);
void WriteFrameCrc(uint8_t *buf, int &offset, const int buf_size);

Expand Down

0 comments on commit 301f969

Please sign in to comment.