mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-07 17:35:22 +08:00
Added support for fc_addon drivers
Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
committed by
Lorenz Meier
parent
dea4c55897
commit
6f8f8279b7
@@ -67,6 +67,12 @@ set(config_module_list
|
||||
drivers/pwm_out_rc_in
|
||||
drivers/qshell/qurt
|
||||
|
||||
#
|
||||
# FC_ADDON drivers
|
||||
#
|
||||
#platforms/qurt/fc_addon/rc_receiver
|
||||
#platforms/qurt/fc_addon/mpu_spi
|
||||
|
||||
#
|
||||
# Libraries
|
||||
#
|
||||
|
||||
@@ -1,101 +0,0 @@
|
||||
include(qurt/px4_impl_qurt)
|
||||
|
||||
if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
|
||||
message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
|
||||
else()
|
||||
set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
|
||||
endif()
|
||||
|
||||
set(CONFIG_SHMEM "1")
|
||||
|
||||
set(config_generate_parameters_scope ALL)
|
||||
|
||||
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
|
||||
|
||||
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
|
||||
|
||||
set(config_module_list
|
||||
#
|
||||
# Board support modules
|
||||
#
|
||||
drivers/device
|
||||
modules/sensors
|
||||
platforms/posix/drivers/df_mpu9250_wrapper
|
||||
platforms/posix/drivers/df_bmp280_wrapper
|
||||
platforms/posix/drivers/df_hmc5883_wrapper
|
||||
platforms/posix/drivers/df_trone_wrapper
|
||||
platforms/posix/drivers/df_isl29501_wrapper
|
||||
|
||||
#
|
||||
# System commands
|
||||
#
|
||||
systemcmds/param
|
||||
|
||||
#
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
#
|
||||
#modules/attitude_estimator_ekf
|
||||
modules/ekf_att_pos_estimator
|
||||
modules/attitude_estimator_q
|
||||
modules/position_estimator_inav
|
||||
modules/ekf2
|
||||
|
||||
#
|
||||
# Vehicle Control
|
||||
#
|
||||
modules/mc_att_control
|
||||
modules/mc_pos_control
|
||||
|
||||
#
|
||||
# Library modules
|
||||
#
|
||||
modules/param
|
||||
modules/systemlib
|
||||
modules/systemlib/mixer
|
||||
modules/uORB
|
||||
modules/commander
|
||||
modules/land_detector
|
||||
modules/load_mon
|
||||
|
||||
#
|
||||
# PX4 drivers
|
||||
#
|
||||
drivers/gps
|
||||
drivers/pwm_out_rc_in
|
||||
drivers/qshell/qurt
|
||||
|
||||
#
|
||||
# Libraries
|
||||
#
|
||||
lib/controllib
|
||||
lib/mathlib
|
||||
lib/mathlib/math/filter
|
||||
lib/geo
|
||||
lib/ecl
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
lib/terrain_estimation
|
||||
lib/runway_takeoff
|
||||
lib/tailsitter_recovery
|
||||
lib/DriverFramework/framework
|
||||
|
||||
#
|
||||
# QuRT port
|
||||
#
|
||||
platforms/common
|
||||
platforms/qurt/px4_layer
|
||||
platforms/posix/work_queue
|
||||
|
||||
#
|
||||
# sources for muorb over fastrpc
|
||||
#
|
||||
modules/muorb/adsp
|
||||
)
|
||||
|
||||
set(config_df_driver_list
|
||||
mpu9250
|
||||
bmp280
|
||||
hmc5883
|
||||
trone
|
||||
isl29501
|
||||
)
|
||||
@@ -0,0 +1,70 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2015 Mark Charlebois. 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
set(TOOLS_ERROR_MSG
|
||||
"The FC_ADDON must be installed and the environment variable FC_ADDON must be set"
|
||||
"(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)")
|
||||
|
||||
if ("$ENV{FC_ADDON}" STREQUAL "")
|
||||
message(FATAL_ERROR ${TOOLS_ERROR_MSG})
|
||||
else()
|
||||
set(FC_ADDON $ENV{FC_ADDON})
|
||||
endif()
|
||||
|
||||
add_library(libmpu9x50 SHARED IMPORTED GLOBAL)
|
||||
set_target_properties(libmpu9x50 PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libmpu9x50.so)
|
||||
|
||||
include_directories(
|
||||
${FC_ADDON}/flight_controller/hexagon/inc
|
||||
)
|
||||
|
||||
px4_add_module(
|
||||
MODULE platforms__qurt__fc_addon__mpu_spi
|
||||
MAIN mpu9x50
|
||||
STACK_MAIN 1200
|
||||
COMPILE_FLAGS
|
||||
-Os -Weffc++
|
||||
SRCS
|
||||
mpu9x50_main.cpp
|
||||
mpu9x50_params.c
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
|
||||
set(module_external_libraries
|
||||
${module_external_libraries}
|
||||
libmpu9x50
|
||||
CACHE INTERNAL "module_external_libraries"
|
||||
)
|
||||
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,89 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 Mark Charlebois. 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 mpu9x50_params.c
|
||||
*
|
||||
* Parameters defined by the mpu9x50 driver
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/**
|
||||
* IMU Low pass filter enum value for Gyro
|
||||
*
|
||||
* Acceptable values:
|
||||
*
|
||||
* MPU9X50_GYRO_LPF_250HZ = 0,
|
||||
* MPU9X50_GYRO_LPF_184HZ = 1,
|
||||
* MPU9X50_GYRO_LPF_92HZ = 2,
|
||||
* MPU9X50_GYRO_LPF_41HZ = 3,
|
||||
* MPU9X50_GYRO_LPF_20HZ = 4 (default),
|
||||
* MPU9X50_GYRO_LPF_10HZ = 5,
|
||||
* MPU9X50_GYRO_LPF_5HZ = 6,
|
||||
* MPU9X50_GYRO_LPF_3600HZ_NOLPF = 7,
|
||||
*
|
||||
* @group MPU9x50 Configuration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MPU_GYRO_LPF_ENUM, 4);
|
||||
|
||||
/**
|
||||
* IMU Low pass filter enum value for Accelerometer
|
||||
*
|
||||
* Acceptable values:
|
||||
*
|
||||
* MPU9X50_ACC_LPF_460HZ = 0,
|
||||
* MPU9X50_ACC_LPF_184HZ = 1,
|
||||
* MPU9X50_ACC_LPF_92HZ = 2,
|
||||
* MPU9X50_ACC_LPF_41HZ = 3,
|
||||
* MPU9X50_ACC_LPF_20HZ = 4 (default),
|
||||
* MPU9X50_ACC_LPF_10HZ = 5,
|
||||
* MPU9X50_ACC_LPF_5HZ = 6,
|
||||
* MPU9X50_ACC_LPF_460HZ_NOLPF = 7,
|
||||
*
|
||||
* @group MPU9x50 Configuration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MPU_ACC_LPF_ENUM, 4);
|
||||
|
||||
/**
|
||||
* IMU sample rate in Hz
|
||||
* acceptable values:
|
||||
* MPU9x50_SAMPLE_RATE_100HZ = 0,
|
||||
* MPU9x50_SAMPLE_RATE_200HZ,
|
||||
* MPU9x50_SAMPLE_RATE_500HZ, (default)
|
||||
* MPU9x50_SAMPLE_RATE_1000HZ,
|
||||
*
|
||||
* @group MPU9x50 Configuration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MPU_SAMPLE_RATE_ENUM, 2);
|
||||
@@ -0,0 +1,71 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2016 Mark Charlebois. 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 AtlFlight 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
set(TOOLS_ERROR_MSG
|
||||
"The FC_ADDON must be installed and the environment variable FC_ADDON must be set"
|
||||
"(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)")
|
||||
|
||||
if ("$ENV{FC_ADDON}" STREQUAL "")
|
||||
message(FATAL_ERROR ${TOOLS_ERROR_MSG})
|
||||
else()
|
||||
set(FC_ADDON $ENV{FC_ADDON})
|
||||
endif()
|
||||
|
||||
include_directories(
|
||||
${FC_ADDON}/flight_controller/hexagon/inc
|
||||
)
|
||||
|
||||
add_library(librc_receiver SHARED IMPORTED GLOBAL)
|
||||
set_target_properties(librc_receiver PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/librc_receiver.so)
|
||||
|
||||
px4_add_module(
|
||||
MODULE platforms__qurt__fc_addon__rc_receiver
|
||||
MAIN rc_receiver
|
||||
STACK_MAIN 1200
|
||||
COMPILE_FLAGS
|
||||
-Os -Weffc++
|
||||
SRCS
|
||||
rc_receiver_main.cpp
|
||||
rc_receiver_params.c
|
||||
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
|
||||
set(module_external_libraries
|
||||
${module_external_libraries}
|
||||
librc_receiver
|
||||
CACHE INTERNAL "module_external_libraries"
|
||||
)
|
||||
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
@@ -0,0 +1,347 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 Mark Charlebois. 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 <px4_getopt.h>
|
||||
#include <px4_tasks.h>
|
||||
#include <px4_log.h>
|
||||
#include <string.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#include <semaphore.h>
|
||||
#include "rc_receiver_api.h"
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
/** driver 'main' command */
|
||||
extern "C" { __EXPORT int rc_receiver_main(int argc, char *argv[]); }
|
||||
|
||||
#define MAX_LEN_DEV_PATH 32
|
||||
|
||||
namespace rc_receiver
|
||||
{
|
||||
|
||||
/** Threshold value to detect rc receiver signal lost in millisecond */
|
||||
#define SIGNAL_LOST_THRESHOLD_MS 500
|
||||
|
||||
/** serial device path that rc receiver is connected to */
|
||||
static char _device[MAX_LEN_DEV_PATH];
|
||||
|
||||
/** flag indicating if rc_receiver module is running */
|
||||
static bool _is_running = false;
|
||||
|
||||
/** flag indicating if task thread should exit */
|
||||
static bool _task_should_exit = false;
|
||||
|
||||
/** handle to the task main thread */
|
||||
static px4_task_t _task_handle = -1;
|
||||
|
||||
/** RC receiver type */
|
||||
static enum RC_RECEIVER_TYPES _rc_receiver_type;
|
||||
|
||||
/** RC input channel value array accomondating up to RC_INPUT_MAX_CHANNELS */
|
||||
static uint16_t rc_inputs[input_rc_s::RC_INPUT_MAX_CHANNELS];
|
||||
|
||||
/** rc_input uorb topic publication handle */
|
||||
static orb_advert_t _input_rc_pub = nullptr;
|
||||
|
||||
/** rc_input uorb topic data */
|
||||
static struct input_rc_s _rc_in;
|
||||
|
||||
/**< parameter update subscription */
|
||||
static int _params_sub;
|
||||
|
||||
struct {
|
||||
param_t rc_receiver_type;
|
||||
} _params_handles; /**< parameter handles */
|
||||
|
||||
/** Print out the usage information */
|
||||
static void usage();
|
||||
|
||||
/** mpu9x50 start measurement */
|
||||
static void start();
|
||||
|
||||
/** mpu9x50 stop measurement */
|
||||
static void stop();
|
||||
|
||||
/** task main trampoline function */
|
||||
static void task_main_trampoline(int argc, char *argv[]);
|
||||
|
||||
/** mpu9x50 measurement thread primary entry point */
|
||||
static void task_main(int argc, char *argv[]);
|
||||
|
||||
/** update all parameters */
|
||||
static void parameters_update();
|
||||
|
||||
/** poll parameter update */
|
||||
static void parameter_update_poll();
|
||||
|
||||
void parameters_update()
|
||||
{
|
||||
PX4_DEBUG("rc_receiver_parameters_update");
|
||||
float v;
|
||||
|
||||
// accel params
|
||||
if (param_get(_params_handles.rc_receiver_type, &v) == 0) {
|
||||
_rc_receiver_type = (enum RC_RECEIVER_TYPES)v;
|
||||
PX4_DEBUG("rc_receiver_parameters_update rc_receiver_type %f", v);
|
||||
}
|
||||
}
|
||||
|
||||
void parameters_init()
|
||||
{
|
||||
_params_handles.rc_receiver_type = param_find("RC_RECEIVER_TYPE");
|
||||
|
||||
parameters_update();
|
||||
}
|
||||
|
||||
void parameter_update_poll()
|
||||
{
|
||||
bool updated;
|
||||
|
||||
/* Check if parameters have changed */
|
||||
orb_check(_params_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
struct parameter_update_s param_update;
|
||||
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);
|
||||
parameters_update();
|
||||
}
|
||||
}
|
||||
|
||||
void start()
|
||||
{
|
||||
ASSERT(_task_handle == -1);
|
||||
|
||||
/* start the task */
|
||||
_task_handle = px4_task_spawn_cmd("rc_receiver_main",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX,
|
||||
1500,
|
||||
(px4_main_t)&task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
if (_task_handle < 0) {
|
||||
warn("task start failed");
|
||||
return;
|
||||
}
|
||||
|
||||
_is_running = true;
|
||||
}
|
||||
|
||||
void stop()
|
||||
{
|
||||
// TODO - set thread exit signal to terminate the task main thread
|
||||
|
||||
_is_running = false;
|
||||
_task_handle = -1;
|
||||
}
|
||||
|
||||
void task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
PX4_WARN("task_main_trampoline");
|
||||
task_main(argc, argv);
|
||||
}
|
||||
|
||||
void task_main(int argc, char *argv[])
|
||||
{
|
||||
PX4_WARN("enter rc_receiver task_main");
|
||||
uint32_t fd;
|
||||
|
||||
// clear the rc_input report for topic advertise
|
||||
memset(&_rc_in, 0, sizeof(struct input_rc_s));
|
||||
|
||||
_input_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc_in);
|
||||
|
||||
if (_input_rc_pub == nullptr) {
|
||||
PX4_WARN("failed to advertise input_rc uorb topic. Quit!");
|
||||
return ;
|
||||
}
|
||||
|
||||
// subscribe to parameter_update topic
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
// Open the RC receiver device on the specified serial port
|
||||
fd = rc_receiver_open(_rc_receiver_type, _device);
|
||||
|
||||
if (fd <= 0) {
|
||||
PX4_WARN("failed to open rc receiver type %d dev %s. Quit!",
|
||||
_rc_receiver_type, _device);
|
||||
return ;
|
||||
}
|
||||
|
||||
// Continuously receive RC packet from serial device, until task is signaled
|
||||
// to exit
|
||||
uint32_t num_channels;
|
||||
uint64_t ts = hrt_absolute_time();
|
||||
int ret;
|
||||
int counter = 0;
|
||||
|
||||
_rc_in.timestamp_last_signal = ts;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
// poll parameter update
|
||||
parameter_update_poll();
|
||||
|
||||
// read RC packet from serial device in blocking mode.
|
||||
num_channels = input_rc_s::RC_INPUT_MAX_CHANNELS;
|
||||
|
||||
ret = rc_receiver_get_packet(fd, rc_inputs, &num_channels);
|
||||
ts = hrt_absolute_time();
|
||||
|
||||
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_QURT;
|
||||
|
||||
if (ret < 0) {
|
||||
// enum RC_RECEIVER_ERRORS error_code = rc_receiver_get_last_error(fd);
|
||||
// PX4_WARN("RC packet receiving timed out. error code %d", error_code);
|
||||
|
||||
uint64_t time_diff_us = ts - _rc_in.timestamp_last_signal;
|
||||
|
||||
if (time_diff_us > SIGNAL_LOST_THRESHOLD_MS * 1000) {
|
||||
_rc_in.rc_lost = true;
|
||||
|
||||
if (++counter == 500) {
|
||||
PX4_WARN("RC signal lost for %u ms", time_diff_us / 1000);
|
||||
counter = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// populate the input_rc_s structure
|
||||
if (ret == 0) {
|
||||
_rc_in.timestamp_publication = ts;
|
||||
_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
|
||||
_rc_in.channel_count = num_channels;
|
||||
|
||||
// TODO - need to add support for RSSI, failsafe mode
|
||||
_rc_in.rssi = RC_INPUT_RSSI_MAX;
|
||||
_rc_in.rc_failsafe = false;
|
||||
_rc_in.rc_lost = false;
|
||||
_rc_in.rc_lost_frame_count = 0;
|
||||
_rc_in.rc_total_frame_count = 1;
|
||||
}
|
||||
|
||||
for (uint32_t i = 0; i < num_channels; i++) {
|
||||
// Scale the Spektrum DSM value to ppm encoding. This is for the
|
||||
// consistency with PX4 which internally translates DSM to PPM.
|
||||
// See modules/px4iofirmware/dsm.c for details
|
||||
// NOTE: rc_receiver spektrum driver outputs the data in 10bit DSM
|
||||
// format, so we need to double the channel value before the scaling
|
||||
_rc_in.values[i] = ((((int)(rc_inputs[i]*2) - 1024) * 1000) / 1700) + 1500;
|
||||
}
|
||||
|
||||
orb_publish(ORB_ID(input_rc), _input_rc_pub, &_rc_in);
|
||||
}
|
||||
|
||||
rc_receiver_close(fd);
|
||||
}
|
||||
|
||||
void usage()
|
||||
{
|
||||
PX4_WARN("missing command: try 'start', 'stop', 'status'");
|
||||
PX4_WARN("options:");
|
||||
PX4_WARN(" -D device");
|
||||
}
|
||||
|
||||
} // namespace rc_receiver
|
||||
|
||||
int rc_receiver_main(int argc, char *argv[])
|
||||
{
|
||||
const char *device = NULL;
|
||||
int ch;
|
||||
int myoptind = 1;
|
||||
const char *myoptarg = NULL;
|
||||
|
||||
/* jump over start/off/etc and look at options first */
|
||||
while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'D':
|
||||
device = myoptarg;
|
||||
break;
|
||||
|
||||
default:
|
||||
rc_receiver::usage();
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Check on required arguments
|
||||
if (device == NULL || strlen(device) == 0) {
|
||||
rc_receiver::usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
memset(rc_receiver::_device, 0, MAX_LEN_DEV_PATH);
|
||||
strncpy(rc_receiver::_device, device, strlen(device));
|
||||
|
||||
const char *verb = argv[myoptind];
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(verb, "start")) {
|
||||
if (rc_receiver::_is_running) {
|
||||
PX4_WARN("rc_receiver already running");
|
||||
return 1;
|
||||
}
|
||||
|
||||
rc_receiver::start();
|
||||
|
||||
} else if (!strcmp(verb, "stop")) {
|
||||
if (rc_receiver::_is_running) {
|
||||
PX4_WARN("rc_receiver is not running");
|
||||
return 1;
|
||||
}
|
||||
|
||||
rc_receiver::stop();
|
||||
|
||||
} else if (!strcmp(verb, "status")) {
|
||||
PX4_WARN("rc_receiver is %s", rc_receiver::_is_running ? "running" : "stopped");
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
rc_receiver::usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 Mark Charlebois. 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 rc_receiver_params.c
|
||||
*
|
||||
* Parameters defined by the rc_receiver driver
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/**
|
||||
* RC receiver type
|
||||
*
|
||||
* Acceptable values:
|
||||
*
|
||||
* - RC_RECEIVER_SPEKTRUM = 1,
|
||||
* - RC_RECEIVER_LEMONRX = 2,
|
||||
*
|
||||
* @group RC Receiver Configuration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_RECEIVER_TYPE, 1);
|
||||
Reference in New Issue
Block a user