forked from Archive/PX4-Autopilot
drivers/rc/crsf_rc: create new standalone CRSF driver
This commit is contained in:
chris1seto
GitHub
parent
b910c1dcf4
commit
b682b30ab1
|
|
@ -34,6 +34,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
|||
CONFIG_DRIVERS_PWM_INPUT=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_DRIVERS_PX4IO=y
|
||||
CONFIG_COMMON_RC=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_ROBOCLAW=y
|
||||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
|
|
|
|||
|
|
@ -38,6 +38,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA238=y
|
|||
CONFIG_DRIVERS_PWM_INPUT=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_DRIVERS_PX4IO=y
|
||||
CONFIG_COMMON_RC=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_ROBOCLAW=y
|
||||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
|
|
|
|||
|
|
@ -0,0 +1,9 @@
|
|||
menu "RC"
|
||||
menuconfig COMMON_RC
|
||||
bool "Common RC"
|
||||
default n
|
||||
select DRIVERS_RC_CRSF_RC
|
||||
---help---
|
||||
Enable default set of magnetometer drivers
|
||||
rsource "*/Kconfig"
|
||||
endmenu
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2022 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.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE drivers__rc__crsf_rc
|
||||
MAIN crsf_rc
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
crsf.cpp
|
||||
crsf.h
|
||||
CrsfRc.cpp
|
||||
CrsfRc.hpp
|
||||
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
rc
|
||||
)
|
||||
|
|
@ -0,0 +1,360 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "CrsfRc.hpp"
|
||||
|
||||
#include <poll.h>
|
||||
#include <termios.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
CrsfRc::CrsfRc(const char *device) :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device))
|
||||
{
|
||||
if (device) {
|
||||
strncpy(_device, device, sizeof(_device) - 1);
|
||||
_device[sizeof(_device) - 1] = '\0';
|
||||
}
|
||||
}
|
||||
|
||||
CrsfRc::~CrsfRc()
|
||||
{
|
||||
perf_free(_cycle_interval_perf);
|
||||
perf_free(_publish_interval_perf);
|
||||
}
|
||||
|
||||
int CrsfRc::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
bool error_flag = false;
|
||||
|
||||
int myoptind = 1;
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
const char *device_name = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'd':
|
||||
device_name = myoptarg;
|
||||
break;
|
||||
|
||||
case '?':
|
||||
error_flag = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_WARN("unrecognized flag");
|
||||
error_flag = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (error_flag) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (device_name && (access(device_name, R_OK | W_OK) == 0)) {
|
||||
CrsfRc *instance = new CrsfRc(device_name);
|
||||
|
||||
if (instance == nullptr) {
|
||||
PX4_ERR("alloc failed");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
instance->ScheduleNow();
|
||||
|
||||
return PX4_OK;
|
||||
|
||||
} else {
|
||||
if (device_name) {
|
||||
PX4_ERR("invalid device (-d) %s", device_name);
|
||||
|
||||
} else {
|
||||
PX4_ERR("valid device required");
|
||||
}
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void CrsfRc::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
::close(_rc_fd);
|
||||
_rc_fd = -1;
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
if (_rc_fd < 0) {
|
||||
_rc_fd = ::open(_device, O_RDWR | O_NONBLOCK);
|
||||
|
||||
if (_rc_fd >= 0) {
|
||||
// no parity, one stop bit
|
||||
struct termios t {};
|
||||
tcgetattr(_rc_fd, &t);
|
||||
cfsetspeed(&t, CRSF_BAUDRATE);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
tcsetattr(_rc_fd, TCSANOW, &t);
|
||||
}
|
||||
}
|
||||
|
||||
// poll with 3 second timeout
|
||||
pollfd fds[1];
|
||||
fds[0].fd = _rc_fd;
|
||||
fds[0].events = POLLIN;
|
||||
int ret = ::poll(fds, 1, 3000);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_ERR("poll error");
|
||||
// try again with delay
|
||||
ScheduleDelayed(500_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);
|
||||
|
||||
if (new_bytes > 0) {
|
||||
_bytes_rx += new_bytes;
|
||||
|
||||
// parse new data
|
||||
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
|
||||
uint16_t raw_rc_count = 0;
|
||||
bool rc_updated = crsf::crsf_parse(time_now_us, &_rcs_buf[0], new_bytes, &raw_rc_values[0], &raw_rc_count,
|
||||
input_rc_s::RC_INPUT_MAX_CHANNELS);
|
||||
|
||||
if (rc_updated) {
|
||||
if (!_rc_locked) {
|
||||
_rc_locked = true;
|
||||
PX4_INFO("RC input locked");
|
||||
}
|
||||
|
||||
// we have a new CRSF frame. Publish it.
|
||||
input_rc_s input_rc{};
|
||||
input_rc.timestamp_last_signal = time_now_us;
|
||||
input_rc.channel_count = math::max(raw_rc_count, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS);
|
||||
input_rc.rssi = -1; // TODO
|
||||
input_rc.rc_lost = (raw_rc_count == 0);
|
||||
input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
|
||||
|
||||
for (int i = 0; i < input_rc.channel_count; i++) {
|
||||
input_rc.values[i] = raw_rc_values[i];
|
||||
}
|
||||
|
||||
input_rc.timestamp = hrt_absolute_time();
|
||||
_input_rc_pub.publish(input_rc);
|
||||
perf_count(_publish_interval_perf);
|
||||
_rc_valid = input_rc.timestamp;
|
||||
|
||||
if (_param_rc_crsf_tel_en.get() && (input_rc.timestamp > _telemetry_update_last + 100_ms)) {
|
||||
switch (_next_type) {
|
||||
case 0:
|
||||
battery_status_s battery_status;
|
||||
|
||||
if (_battery_status_sub.update(&battery_status)) {
|
||||
uint16_t voltage = battery_status.voltage_filtered_v * 10;
|
||||
uint16_t current = battery_status.current_filtered_a * 10;
|
||||
int fuel = battery_status.discharged_mah;
|
||||
uint8_t remaining = battery_status.remaining * 100;
|
||||
crsf::crsf_send_telemetry_battery(_rc_fd, voltage, current, fuel, remaining);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 1:
|
||||
vehicle_gps_position_s vehicle_gps_position;
|
||||
|
||||
if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
|
||||
int32_t latitude = vehicle_gps_position.lat;
|
||||
int32_t longitude = vehicle_gps_position.lon;
|
||||
uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f;
|
||||
uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f;
|
||||
uint16_t altitude = vehicle_gps_position.alt + 1000;
|
||||
uint8_t num_satellites = vehicle_gps_position.satellites_used;
|
||||
crsf::crsf_send_telemetry_gps(_rc_fd, latitude, longitude, groundspeed, gps_heading, altitude, num_satellites);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 2:
|
||||
vehicle_attitude_s vehicle_attitude;
|
||||
|
||||
if (_vehicle_attitude_sub.update(&vehicle_attitude)) {
|
||||
matrix::Eulerf attitude = matrix::Quatf(vehicle_attitude.q);
|
||||
int16_t pitch = attitude(1) * 1e4f;
|
||||
int16_t roll = attitude(0) * 1e4f;
|
||||
int16_t yaw = attitude(2) * 1e4f;
|
||||
crsf::crsf_send_telemetry_attitude(_rc_fd, pitch, roll, yaw);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 3:
|
||||
vehicle_status_s vehicle_status;
|
||||
|
||||
if (_vehicle_status_sub.update(&vehicle_status)) {
|
||||
const char *flight_mode = "(unknown)";
|
||||
|
||||
switch (vehicle_status.nav_state) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
|
||||
flight_mode = "Manual";
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
|
||||
flight_mode = "Altitude";
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
|
||||
flight_mode = "Position";
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
flight_mode = "Return";
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
|
||||
flight_mode = "Mission";
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
|
||||
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
|
||||
flight_mode = "Auto";
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
|
||||
flight_mode = "Failure";
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ACRO:
|
||||
flight_mode = "Acro";
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
|
||||
flight_mode = "Terminate";
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
|
||||
flight_mode = "Offboard";
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_STAB:
|
||||
flight_mode = "Stabilized";
|
||||
break;
|
||||
}
|
||||
|
||||
crsf::crsf_send_telemetry_flight_mode(_rc_fd, flight_mode);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
_telemetry_update_last = input_rc.timestamp;
|
||||
_next_type = (_next_type + 1) % num_data_types;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!_rc_locked && (hrt_elapsed_time(&_rc_valid) > 5_s)) {
|
||||
::close(_rc_fd);
|
||||
_rc_fd = -1;
|
||||
_rc_locked = false;
|
||||
ScheduleDelayed(200_ms);
|
||||
|
||||
} else {
|
||||
ScheduleDelayed(4_ms);
|
||||
}
|
||||
}
|
||||
|
||||
int CrsfRc::print_status()
|
||||
{
|
||||
if (_device[0] != '\0') {
|
||||
PX4_INFO("UART device: %s", _device);
|
||||
PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx);
|
||||
}
|
||||
|
||||
PX4_INFO("RC state: %s", _rc_locked ? "found" : "searching for signal");
|
||||
|
||||
if (_rc_locked) {
|
||||
PX4_INFO("Telemetry: %s", _param_rc_crsf_tel_en.get() ? "yes" : "no");
|
||||
}
|
||||
|
||||
perf_print_counter(_cycle_interval_perf);
|
||||
perf_print_counter(_publish_interval_perf);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int CrsfRc::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int CrsfRc::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
This module does the CRSF RC input parsing.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
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_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int crsf_rc_main(int argc, char *argv[])
|
||||
{
|
||||
return CrsfRc::main(argc, argv);
|
||||
}
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "crsf.h" // old parser TODO:
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
|
||||
// telemetry
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
class CrsfRc : public ModuleBase<CrsfRc>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
CrsfRc(const char *device);
|
||||
~CrsfRc() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
hrt_abstime _rc_valid{0};
|
||||
bool _rc_locked{false};
|
||||
|
||||
uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
|
||||
|
||||
int _rc_fd{-1};
|
||||
char _device[20] {}; ///< device / serial port path
|
||||
|
||||
static constexpr size_t RC_MAX_BUFFER_SIZE{64};
|
||||
uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
|
||||
uint32_t _bytes_rx{0};
|
||||
|
||||
// telemetry
|
||||
hrt_abstime _telemetry_update_last{0};
|
||||
static constexpr int num_data_types{4}; ///< number of different telemetry data types
|
||||
int _next_type{0};
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||
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)};
|
||||
|
||||
perf_counter_t _cycle_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
|
||||
perf_counter_t _publish_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamBool<px4::params::RC_CRSF_TEL_EN>) _param_rc_crsf_tel_en
|
||||
)
|
||||
};
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
menuconfig DRIVERS_RC_CRSF_RC
|
||||
bool "crsf_rc"
|
||||
default n
|
||||
---help---
|
||||
Enable support for crsf rc
|
||||
|
|
@ -0,0 +1,507 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018-2022 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#if 0 // enable non-verbose debugging
|
||||
#define CRSF_DEBUG PX4_WARN
|
||||
#else
|
||||
#define CRSF_DEBUG(...)
|
||||
#endif
|
||||
|
||||
#if 0 // verbose debugging. Careful when enabling: it leads to too much output, causing dropped bytes
|
||||
#define CRSF_VERBOSE PX4_WARN
|
||||
#else
|
||||
#define CRSF_VERBOSE(...)
|
||||
#endif
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <termios.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "crsf.h"
|
||||
|
||||
namespace crsf
|
||||
{
|
||||
|
||||
#define CRSF_FRAME_SIZE_MAX 30 // the actual maximum length is 64, but we're only interested in RC channels and want to minimize buffer size
|
||||
#define CRSF_PAYLOAD_SIZE_MAX (CRSF_FRAME_SIZE_MAX-4)
|
||||
|
||||
struct crsf_frame_header_t {
|
||||
uint8_t device_address; ///< @see crsf_address_t
|
||||
uint8_t length; ///< length of crsf_frame_t (including CRC) minus sizeof(crsf_frame_header_t)
|
||||
};
|
||||
|
||||
struct crsf_frame_t {
|
||||
crsf_frame_header_t header;
|
||||
uint8_t type; ///< @see crsf_frame_type_t
|
||||
uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; ///< payload data including 1 byte CRC at end
|
||||
};
|
||||
|
||||
#define MIN(a,b) (((a)<(b))?(a):(b))
|
||||
#define MAX(a,b) (((a)>(b))?(a):(b))
|
||||
|
||||
#define CRSF_SYNC_BYTE 0xC8
|
||||
|
||||
enum class crsf_frame_type_t : uint8_t {
|
||||
gps = 0x02,
|
||||
battery_sensor = 0x08,
|
||||
link_statistics = 0x14,
|
||||
rc_channels_packed = 0x16,
|
||||
attitude = 0x1E,
|
||||
flight_mode = 0x21,
|
||||
|
||||
// Extended Header Frames, range: 0x28 to 0x96
|
||||
device_ping = 0x28,
|
||||
device_info = 0x29,
|
||||
parameter_settings_entry = 0x2B,
|
||||
parameter_read = 0x2C,
|
||||
parameter_write = 0x2D,
|
||||
command = 0x32
|
||||
};
|
||||
|
||||
enum class crsf_payload_size_t : uint8_t {
|
||||
gps = 15,
|
||||
battery_sensor = 8,
|
||||
link_statistics = 10,
|
||||
rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
|
||||
attitude = 6,
|
||||
};
|
||||
|
||||
|
||||
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
|
||||
};
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct crsf_payload_RC_channels_packed_t {
|
||||
// 176 bits of data (11 bits per channel * 16 channels) = 22 bytes
|
||||
unsigned chan0 : 11;
|
||||
unsigned chan1 : 11;
|
||||
unsigned chan2 : 11;
|
||||
unsigned chan3 : 11;
|
||||
unsigned chan4 : 11;
|
||||
unsigned chan5 : 11;
|
||||
unsigned chan6 : 11;
|
||||
unsigned chan7 : 11;
|
||||
unsigned chan8 : 11;
|
||||
unsigned chan9 : 11;
|
||||
unsigned chan10 : 11;
|
||||
unsigned chan11 : 11;
|
||||
unsigned chan12 : 11;
|
||||
unsigned chan13 : 11;
|
||||
unsigned chan14 : 11;
|
||||
unsigned chan15 : 11;
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
enum class crsf_parser_state_t : uint8_t {
|
||||
unsynced = 0,
|
||||
synced
|
||||
};
|
||||
|
||||
static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
|
||||
{
|
||||
crc ^= a;
|
||||
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
if (crc & 0x80) {
|
||||
crc = (crc << 1) ^ 0xD5;
|
||||
|
||||
} else {
|
||||
crc = crc << 1;
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
|
||||
for (int i = 0; i < len; ++i) {
|
||||
crc = crc8_dvb_s2(crc, buf[i]);
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
static crsf_frame_t crsf_frame{};
|
||||
static unsigned current_frame_position = 0;
|
||||
static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced;
|
||||
|
||||
/**
|
||||
* parse the current crsf_frame buffer
|
||||
*/
|
||||
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels);
|
||||
|
||||
uint8_t crsf_frame_CRC(const crsf_frame_t &frame);
|
||||
|
||||
/**
|
||||
* Convert from RC to PWM value
|
||||
* @param chan_value channel value in [172, 1811]
|
||||
* @return PWM channel value in [1000, 2000]
|
||||
*/
|
||||
static uint16_t convert_channel_value(unsigned chan_value);
|
||||
|
||||
|
||||
bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values,
|
||||
uint16_t *num_values, uint16_t max_channels)
|
||||
{
|
||||
bool ret = false;
|
||||
uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
|
||||
|
||||
while (len > 0) {
|
||||
|
||||
// fill in the crsf_buffer, as much as we can
|
||||
const unsigned current_len = MIN(len, sizeof(crsf_frame_t) - current_frame_position);
|
||||
memcpy(crsf_frame_ptr + current_frame_position, frame, current_len);
|
||||
current_frame_position += current_len;
|
||||
|
||||
// protection to guarantee parsing progress
|
||||
if (current_len == 0) {
|
||||
CRSF_DEBUG("========== parser bug: no progress (%i) ===========", len);
|
||||
|
||||
for (unsigned i = 0; i < current_frame_position; ++i) {
|
||||
CRSF_DEBUG("crsf_frame_ptr[%i]: 0x%x", i, (int)crsf_frame_ptr[i]);
|
||||
}
|
||||
|
||||
// reset the parser
|
||||
current_frame_position = 0;
|
||||
parser_state = crsf_parser_state_t::unsynced;
|
||||
return false;
|
||||
}
|
||||
|
||||
len -= current_len;
|
||||
frame += current_len;
|
||||
|
||||
if (crsf_parse_buffer(values, num_values, max_channels)) {
|
||||
ret = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint8_t crsf_frame_CRC(const crsf_frame_t &frame)
|
||||
{
|
||||
// CRC includes type and payload
|
||||
uint8_t crc = crc8_dvb_s2(0, frame.type);
|
||||
|
||||
for (int i = 0; i < frame.header.length - 2; ++i) {
|
||||
crc = crc8_dvb_s2(crc, frame.payload[i]);
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
static uint16_t convert_channel_value(unsigned chan_value)
|
||||
{
|
||||
/*
|
||||
* RC PWM
|
||||
* min 172 -> 988us
|
||||
* mid 992 -> 1500us
|
||||
* max 1811 -> 2012us
|
||||
*/
|
||||
static constexpr float scale = (2012.f - 988.f) / (1811.f - 172.f);
|
||||
static constexpr float offset = 988.f - 172.f * scale;
|
||||
return (scale * chan_value) + offset;
|
||||
}
|
||||
|
||||
static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels)
|
||||
{
|
||||
uint8_t *crsf_frame_ptr = (uint8_t *)&crsf_frame;
|
||||
|
||||
if (parser_state == crsf_parser_state_t::unsynced) {
|
||||
// there is no sync byte, try to find an RC packet by searching for a matching frame length and type
|
||||
for (unsigned i = 1; i < current_frame_position - 1; ++i) {
|
||||
if (crsf_frame_ptr[i] == (uint8_t)crsf_payload_size_t::rc_channels + 2 &&
|
||||
crsf_frame_ptr[i + 1] == (uint8_t)crsf_frame_type_t::rc_channels_packed) {
|
||||
parser_state = crsf_parser_state_t::synced;
|
||||
unsigned frame_offset = i - 1;
|
||||
CRSF_VERBOSE("RC channels found at offset %i", frame_offset);
|
||||
|
||||
// move the rest of the buffer to the beginning
|
||||
if (frame_offset != 0) {
|
||||
memmove(crsf_frame_ptr, crsf_frame_ptr + frame_offset, current_frame_position - frame_offset);
|
||||
current_frame_position -= frame_offset;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (parser_state != crsf_parser_state_t::synced) {
|
||||
if (current_frame_position >= sizeof(crsf_frame_t)) {
|
||||
// discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start)
|
||||
current_frame_position = 3;
|
||||
|
||||
for (unsigned i = 0; i < current_frame_position; ++i) {
|
||||
crsf_frame_ptr[i] = crsf_frame_ptr[sizeof(crsf_frame_t) - current_frame_position + i];
|
||||
}
|
||||
|
||||
CRSF_VERBOSE("Discarding buffer");
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
if (current_frame_position < 3) {
|
||||
// wait until we have the header & type
|
||||
return false;
|
||||
}
|
||||
|
||||
// Now we have at least the header and the type
|
||||
|
||||
const unsigned current_frame_length = crsf_frame.header.length + sizeof(crsf_frame_header_t);
|
||||
|
||||
if (current_frame_length > sizeof(crsf_frame_t) || current_frame_length < 4) {
|
||||
// frame too long or bogus -> discard everything and go into unsynced state
|
||||
current_frame_position = 0;
|
||||
parser_state = crsf_parser_state_t::unsynced;
|
||||
CRSF_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, crsf_frame.type);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (current_frame_position < current_frame_length) {
|
||||
// we don't have the full frame yet -> wait for more data
|
||||
CRSF_VERBOSE("waiting for more data (%i < %i)", current_frame_position, current_frame_length);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ret = false;
|
||||
|
||||
// Now we have the full frame
|
||||
|
||||
if (crsf_frame.type == (uint8_t)crsf_frame_type_t::rc_channels_packed &&
|
||||
crsf_frame.header.length == (uint8_t)crsf_payload_size_t::rc_channels + 2) {
|
||||
const uint8_t crc = crsf_frame.payload[crsf_frame.header.length - 2];
|
||||
|
||||
if (crc == crsf_frame_CRC(crsf_frame)) {
|
||||
const crsf_payload_RC_channels_packed_t *const rc_channels =
|
||||
(crsf_payload_RC_channels_packed_t *)&crsf_frame.payload;
|
||||
*num_values = MIN(max_channels, 16);
|
||||
|
||||
if (max_channels > 0) { values[0] = convert_channel_value(rc_channels->chan0); }
|
||||
|
||||
if (max_channels > 1) { values[1] = convert_channel_value(rc_channels->chan1); }
|
||||
|
||||
if (max_channels > 2) { values[2] = convert_channel_value(rc_channels->chan2); }
|
||||
|
||||
if (max_channels > 3) { values[3] = convert_channel_value(rc_channels->chan3); }
|
||||
|
||||
if (max_channels > 4) { values[4] = convert_channel_value(rc_channels->chan4); }
|
||||
|
||||
if (max_channels > 5) { values[5] = convert_channel_value(rc_channels->chan5); }
|
||||
|
||||
if (max_channels > 6) { values[6] = convert_channel_value(rc_channels->chan6); }
|
||||
|
||||
if (max_channels > 7) { values[7] = convert_channel_value(rc_channels->chan7); }
|
||||
|
||||
if (max_channels > 8) { values[8] = convert_channel_value(rc_channels->chan8); }
|
||||
|
||||
if (max_channels > 9) { values[9] = convert_channel_value(rc_channels->chan9); }
|
||||
|
||||
if (max_channels > 10) { values[10] = convert_channel_value(rc_channels->chan10); }
|
||||
|
||||
if (max_channels > 11) { values[11] = convert_channel_value(rc_channels->chan11); }
|
||||
|
||||
if (max_channels > 12) { values[12] = convert_channel_value(rc_channels->chan12); }
|
||||
|
||||
if (max_channels > 13) { values[13] = convert_channel_value(rc_channels->chan13); }
|
||||
|
||||
if (max_channels > 14) { values[14] = convert_channel_value(rc_channels->chan14); }
|
||||
|
||||
if (max_channels > 15) { values[15] = convert_channel_value(rc_channels->chan15); }
|
||||
|
||||
CRSF_VERBOSE("Got Channels");
|
||||
|
||||
ret = true;
|
||||
|
||||
} else {
|
||||
CRSF_DEBUG("CRC check failed");
|
||||
}
|
||||
|
||||
} else {
|
||||
CRSF_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, crsf_frame.type);
|
||||
// We could check the CRC here and reset the parser into unsynced state if it fails.
|
||||
// But in practise it's robust even without that.
|
||||
}
|
||||
|
||||
// Either reset or move the rest of the buffer
|
||||
if (current_frame_position > current_frame_length) {
|
||||
CRSF_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length);
|
||||
memmove(crsf_frame_ptr, crsf_frame_ptr + current_frame_length, current_frame_position - current_frame_length);
|
||||
current_frame_position -= current_frame_length;
|
||||
|
||||
} else {
|
||||
current_frame_position = 0;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* write an uint8_t value to a buffer at a given offset and increment the offset
|
||||
*/
|
||||
static inline void write_uint8_t(uint8_t *buf, int &offset, uint8_t value)
|
||||
{
|
||||
buf[offset++] = value;
|
||||
}
|
||||
/**
|
||||
* write an uint16_t value to a buffer at a given offset and increment the offset
|
||||
*/
|
||||
static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value)
|
||||
{
|
||||
// Big endian
|
||||
buf[offset] = value >> 8;
|
||||
buf[offset + 1] = value & 0xff;
|
||||
offset += 2;
|
||||
}
|
||||
/**
|
||||
* write an uint24_t value to a buffer at a given offset and increment the offset
|
||||
*/
|
||||
static inline void write_uint24_t(uint8_t *buf, int &offset, int value)
|
||||
{
|
||||
// Big endian
|
||||
buf[offset] = value >> 16;
|
||||
buf[offset + 1] = (value >> 8) & 0xff;
|
||||
buf[offset + 2] = value & 0xff;
|
||||
offset += 3;
|
||||
}
|
||||
|
||||
/**
|
||||
* write an int32_t value to a buffer at a given offset and increment the offset
|
||||
*/
|
||||
static inline void write_int32_t(uint8_t *buf, int &offset, int32_t value)
|
||||
{
|
||||
// Big endian
|
||||
buf[offset] = value >> 24;
|
||||
buf[offset + 1] = (value >> 16) & 0xff;
|
||||
buf[offset + 2] = (value >> 8) & 0xff;
|
||||
buf[offset + 3] = value & 0xff;
|
||||
offset += 4;
|
||||
}
|
||||
|
||||
static inline void write_frame_header(uint8_t *buf, int &offset, crsf_frame_type_t type, uint8_t payload_size)
|
||||
{
|
||||
write_uint8_t(buf, offset, CRSF_SYNC_BYTE); // this got changed from the address to the sync byte
|
||||
write_uint8_t(buf, offset, payload_size + 2);
|
||||
write_uint8_t(buf, offset, (uint8_t)type);
|
||||
}
|
||||
static inline void write_frame_crc(uint8_t *buf, int &offset, int buf_size)
|
||||
{
|
||||
// CRC does not include the address and length
|
||||
write_uint8_t(buf, offset, crc8_dvb_s2_buf(buf + 2, buf_size - 3));
|
||||
|
||||
// check correctness of buffer size (only needed during development)
|
||||
//if (buf_size != offset) { PX4_ERR("frame size mismatch (%i != %i)", buf_size, offset); }
|
||||
}
|
||||
|
||||
bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining)
|
||||
{
|
||||
uint8_t buf[(uint8_t)crsf_payload_size_t::battery_sensor + 4];
|
||||
int offset = 0;
|
||||
write_frame_header(buf, offset, crsf_frame_type_t::battery_sensor, (uint8_t)crsf_payload_size_t::battery_sensor);
|
||||
write_uint16_t(buf, offset, voltage);
|
||||
write_uint16_t(buf, offset, current);
|
||||
write_uint24_t(buf, offset, fuel);
|
||||
write_uint8_t(buf, offset, remaining);
|
||||
write_frame_crc(buf, offset, sizeof(buf));
|
||||
return write(uart_fd, buf, offset) == offset;
|
||||
}
|
||||
|
||||
bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed,
|
||||
uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites)
|
||||
{
|
||||
uint8_t buf[(uint8_t)crsf_payload_size_t::gps + 4];
|
||||
int offset = 0;
|
||||
write_frame_header(buf, offset, crsf_frame_type_t::gps, (uint8_t)crsf_payload_size_t::gps);
|
||||
write_int32_t(buf, offset, latitude);
|
||||
write_int32_t(buf, offset, longitude);
|
||||
write_uint16_t(buf, offset, groundspeed);
|
||||
write_uint16_t(buf, offset, gps_heading);
|
||||
write_uint16_t(buf, offset, altitude);
|
||||
write_uint8_t(buf, offset, num_satellites);
|
||||
write_frame_crc(buf, offset, sizeof(buf));
|
||||
return write(uart_fd, buf, offset) == offset;
|
||||
}
|
||||
|
||||
bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw)
|
||||
{
|
||||
uint8_t buf[(uint8_t)crsf_payload_size_t::attitude + 4];
|
||||
int offset = 0;
|
||||
write_frame_header(buf, offset, crsf_frame_type_t::attitude, (uint8_t)crsf_payload_size_t::attitude);
|
||||
write_uint16_t(buf, offset, pitch);
|
||||
write_uint16_t(buf, offset, roll);
|
||||
write_uint16_t(buf, offset, yaw);
|
||||
write_frame_crc(buf, offset, sizeof(buf));
|
||||
return write(uart_fd, buf, offset) == offset;
|
||||
}
|
||||
|
||||
bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode)
|
||||
{
|
||||
const int max_length = 16;
|
||||
int length = strlen(flight_mode) + 1;
|
||||
|
||||
if (length > max_length) {
|
||||
length = max_length;
|
||||
}
|
||||
|
||||
uint8_t buf[max_length + 4];
|
||||
int offset = 0;
|
||||
write_frame_header(buf, offset, crsf_frame_type_t::flight_mode, length);
|
||||
memcpy(buf + offset, flight_mode, length);
|
||||
offset += length;
|
||||
buf[offset - 1] = 0; // ensure null-terminated string
|
||||
write_frame_crc(buf, offset, length + 4);
|
||||
return write(uart_fd, buf, offset) == offset;
|
||||
}
|
||||
|
||||
}; // namespace crsf
|
||||
|
|
@ -0,0 +1,112 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 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 crsf.h
|
||||
*
|
||||
* RC protocol definition for CSRF (TBS Crossfire).
|
||||
* It is an uninverted protocol at 420000 baudrate.
|
||||
*
|
||||
* RC channels come in at 150Hz.
|
||||
*
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
namespace crsf
|
||||
{
|
||||
|
||||
#define CRSF_BAUDRATE 420000
|
||||
|
||||
/**
|
||||
* Parse the CRSF protocol and extract RC channel data.
|
||||
*
|
||||
* @param now current time
|
||||
* @param frame data to parse
|
||||
* @param len length of frame
|
||||
* @param values output channel values, each in range [1000, 2000]
|
||||
* @param num_values set to the number of parsed channels in values
|
||||
* @param max_channels maximum length of values
|
||||
* @return true if channels successfully decoded
|
||||
*/
|
||||
bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, uint16_t *num_values,
|
||||
uint16_t max_channels);
|
||||
|
||||
/**
|
||||
* Send telemetry battery information
|
||||
* @param uart_fd UART file descriptor
|
||||
* @param voltage Voltage [0.1V]
|
||||
* @param current Current [0.1A]
|
||||
* @param fuel drawn mAh
|
||||
* @param remaining battery remaining [%]
|
||||
* @return true on success
|
||||
*/
|
||||
bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, int fuel, uint8_t remaining);
|
||||
|
||||
/**
|
||||
* Send telemetry GPS information
|
||||
* @param uart_fd UART file descriptor
|
||||
* @param latitude latitude [degree * 1e7]
|
||||
* @param longitude longitude [degree * 1e7]
|
||||
* @param groundspeed Ground speed [km/h * 10]
|
||||
* @param gps_heading GPS heading [degree * 100]
|
||||
* @param altitude Altitude [meters + 1000m offset]
|
||||
* @param num_satellites number of satellites used
|
||||
* @return true on success
|
||||
*/
|
||||
bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed,
|
||||
uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites);
|
||||
|
||||
/**
|
||||
* Send telemetry Attitude information
|
||||
* @param uart_fd UART file descriptor
|
||||
* @param pitch Pitch angle [rad * 1e4]
|
||||
* @param roll Roll angle [rad * 1e4]
|
||||
* @param yaw Yaw angle [rad * 1e4]
|
||||
* @return true on success
|
||||
*/
|
||||
bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw);
|
||||
|
||||
/**
|
||||
* Send telemetry Flight Mode information
|
||||
* @param uart_fd UART file descriptor
|
||||
* @param flight_mode Flight Mode string (max length = 15)
|
||||
* @return true on success
|
||||
*/
|
||||
bool crsf_send_telemetry_flight_mode(int uart_fd, const char *flight_mode);
|
||||
|
||||
}; // namespace crsf
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
module_name: CRSF RC Input Driver
|
||||
serial_config:
|
||||
- command: "crsf_rc start -d ${SERIAL_DEV}"
|
||||
port_config_param:
|
||||
name: RC_CRSF_PRT_CFG
|
||||
group: Serial
|
||||
#default: RC
|
||||
#depends_on_port: RC
|
||||
description_extended: |
|
||||
Crossfire RC (CRSF) driver.
|
||||
|
||||
parameters:
|
||||
- group: RC
|
||||
definitions:
|
||||
RC_CRSF_TEL_EN:
|
||||
description:
|
||||
short: Crossfire RC telemetry enable
|
||||
long: |
|
||||
Crossfire telemetry enable
|
||||
|
||||
type: boolean
|
||||
default: [0]
|
||||
Loading…
Reference in New Issue