From c0fcffae76973bea053b924eb55a65dc0084994a Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 4 Dec 2016 17:23:40 +0100
Subject: [PATCH] spektrum_rc: make it compile
---
cmake/configs/qurt_sdflight_default.cmake | 3 +-
.../drivers/df_spektrum_rc/df_spektrum_rc.cpp | 87 +++++++++++++++++--
2 files changed, 83 insertions(+), 7 deletions(-)
diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake
index f71f248c44..381b293688 100644
--- a/cmake/configs/qurt_sdflight_default.cmake
+++ b/cmake/configs/qurt_sdflight_default.cmake
@@ -69,9 +69,10 @@ set(config_module_list
#
drivers/gps
#drivers/pwm_out_rc_in
+ platforms/posix/drivers/df_spektrum_rc
drivers/qshell/qurt
drivers/snapdragon_pwm_out
-
+
#
# Libraries
#
diff --git a/src/platforms/posix/drivers/df_spektrum_rc/df_spektrum_rc.cpp b/src/platforms/posix/drivers/df_spektrum_rc/df_spektrum_rc.cpp
index 753b9addbc..d7293bca6a 100644
--- a/src/platforms/posix/drivers/df_spektrum_rc/df_spektrum_rc.cpp
+++ b/src/platforms/posix/drivers/df_spektrum_rc/df_spektrum_rc.cpp
@@ -51,8 +51,12 @@
#include <lib/rc/dsm.h>
#include <drivers/drv_rc_input.h>
+#include <drivers/drv_hrt.h>
-#define SPEKTRUM_UART_DEVICE_PATH /dev/serialABC
+#include <uORB/topics/input_rc.h>
+
+#define SPEKTRUM_UART_DEVICE_PATH "/dev/serialABC"
+#define SBUS_BUFFER_SIZE 128
extern "C" { __EXPORT int df_spektrum_rc_main(int argc, char *argv[]); }
@@ -63,17 +67,24 @@ volatile bool _task_should_exit = false; // flag indicating if bebop esc control
static bool _is_running = false; // flag indicating if bebop esc app is running
static px4_task_t _task_handle = -1; // handle to the task main thread
+input_rc_s _rc_in;
+float _analog_rc_rssi_volt;
+bool _analog_rc_rssi_stable;
+
int start();
int stop();
int info();
void usage();
void task_main(int argc, char *argv[]);
+void fill_rc_in(uint16_t raw_rc_count,
+ uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS],
+ hrt_abstime now, bool frame_drop, bool failsafe,
+ unsigned frame_drops, int rssi = -1);
void task_main(int argc, char *argv[])
{
// publications
orb_advert_t rc_pub = nullptr;
- int rc_orb_class_instance = -1;
uint8_t _rcs_buf[50];
// important to keep these buffers out of the stack
@@ -117,15 +128,17 @@ void task_main(int argc, char *argv[])
if (newbytes > 0) {
+ hrt_abstime now = hrt_absolute_time();
+
// parse new data
- rc_updated = dsm_parse(_cycle_timestamp, &_rcs_buf[0], newbytes, &raw_rc_values[0], &raw_rc_count,
- &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
+ bool rc_updated = dsm_parse(now, &_rcs_buf[0], newbytes, &raw_rc_values[0], &raw_rc_count,
+ &dsm_11_bit, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
if (rc_updated) {
// we have a new DSM frame. Publish it.
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
- fill_rc_in(raw_rc_count, raw_rc_values, _cycle_timestamp,
+ fill_rc_in(raw_rc_count, raw_rc_values, now,
false, false, frame_drops);
if (rc_pub == nullptr) {
@@ -139,12 +152,74 @@ void task_main(int argc, char *argv[])
}
}
- orb_deadvertise(_rc_pub);
+ orb_unadvertise(rc_pub);
_is_running = false;
}
+void
+fill_rc_in(uint16_t raw_rc_count,
+ uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS],
+ hrt_abstime now, bool frame_drop, bool failsafe,
+ unsigned frame_drops, int rssi)
+{
+ // fill rc_in struct for publishing
+ _rc_in.channel_count = raw_rc_count;
+
+ if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+ _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+ }
+
+ unsigned valid_chans = 0;
+
+ for (unsigned i = 0; i < _rc_in.channel_count; i++) {
+ _rc_in.values[i] = raw_rc_values[i];
+
+ if (raw_rc_values[i] != UINT16_MAX) {
+ valid_chans++;
+ }
+ }
+
+ _rc_in.timestamp = now;
+ _rc_in.timestamp_last_signal = _rc_in.timestamp;
+ _rc_in.rc_ppm_frame_length = 0;
+
+ /* fake rssi if no value was provided */
+ if (rssi == -1) {
+
+ /* set RSSI if analog RSSI input is present */
+ if (_analog_rc_rssi_stable) {
+ float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
+
+ if (rssi_analog > 100.0f) {
+ rssi_analog = 100.0f;
+ }
+
+ if (rssi_analog < 0.0f) {
+ rssi_analog = 0.0f;
+ }
+
+ _rc_in.rssi = rssi_analog;
+
+ } else {
+ _rc_in.rssi = 255;
+ }
+
+ } else {
+ _rc_in.rssi = rssi;
+ }
+
+ if (valid_chans == 0) {
+ _rc_in.rssi = 0;
+ }
+
+ _rc_in.rc_failsafe = failsafe;
+ _rc_in.rc_lost = (valid_chans == 0);
+ _rc_in.rc_lost_frame_count = frame_drops;
+ _rc_in.rc_total_frame_count = 0;
+}
+
int start()
{
// Start the task to handle RC
--
GitLab