dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

DriverFramework purge 

The bulk of this change was tightly coupled and needed to be deleted in one pass. Some of the smaller changes were things that broke as a result of the initial purge and subsequently fixed by further eradicating unnecessary platform differences. Finally, I deleted any dead code I came across in the related files I touched while going through everything.

 - DriverFramework (src/lib/DriverFramework submodule) completely removed
 - added dspal submodule in qurt platform (was brought in via DriverFramework)
 - all df wrapper drivers removed
 - all boards using df wrapper drivers updated to use in tree equivalents
 - unused empty arch/board.h on posix and qurt removed
 - unused IOCTLs removed (pub block, priv, etc)
 - Integrator delete methods only used from df wrapper drivers
 - commander: sensor calibration use "NuttX version" everywhere for now
 - sensors: update to px4_{open, read, close} instead of DevMgr wrapper (adc open for analog differential pressure)
 - battery_status: update to px4_{open, read, close} instead of DevMgr wrapper (adc open for analog differential pressure)
 - cdev cleanup conflicting typedefs and names with actual OS (pollevent_t, etc)
 - load_mon and top remove from linux boards (unused)
 - delete unused PX4_MAIN_FUNCTION
 - delete unused getreg32 macro
 - delete unused SIOCDEVPRIVATE define
 - named each platform tasks consistently
 - posix list_devices and list_topics removed (list_files now shows all virtual files)
This commit is contained in:
Daniel Agar 2020-01-09 11:00:40 -05:00
parent 04ba05f5a0
commit de4f594937
165 changed files with 285 additions and 8108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.gitmodules vendored
View File

@ -26,10 +26,6 @@
path = src/lib/matrix
url = https://github.com/PX4/Matrix.git
branch = master
[submodule "src/lib/DriverFramework"]
path = src/lib/DriverFramework
url = https://github.com/PX4/DriverFramework.git
branch = master
[submodule "src/lib/ecl"]
path = src/lib/ecl
url = https://github.com/PX4/ecl.git
@ -58,3 +54,6 @@
path = cmake/configs/uavcan_board_ident
url = https://github.com/PX4/uavcan_board_ident.git
branch = master
[submodule "platforms/qurt/dspal"]
path = platforms/qurt/dspal
url = https://github.com/ATLFlight/dspal.git

20
CMakeLists.txt
View File

@ -138,7 +138,6 @@ set(CONFIG "px4_sitl_default" CACHE STRING "desired configuration")
include(px4_add_module)
set(config_module_list)
set(config_df_driver_list)
include(px4_config)
include(px4_add_board)
@ -311,25 +310,6 @@ add_subdirectory(msg EXCLUDE_FROM_ALL)
px4_generate_airframes_xml(BOARD ${PX4_BOARD})
#=============================================================================
# DriverFramework
#
px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework")
set(OS ${PX4_PLATFORM})
add_subdirectory(src/lib/DriverFramework/framework)
# List the DriverFramework drivers
if (DEFINED config_df_driver_list)
message("DF Drivers: ${config_df_driver_list}")
endif()
set(df_driver_libs)
foreach(driver ${config_df_driver_list})
add_subdirectory(src/lib/DriverFramework/drivers/${driver})
list(APPEND df_driver_libs df_${driver})
message("Adding DF driver: ${driver}")
endforeach()
#=============================================================================
# external projects
#

3
Documentation/Doxyfile.in
View File

@ -828,8 +828,7 @@ RECURSIVE = YES
# Note that relative paths are relative to the directory from which doxygen is
# run.
EXCLUDE = @CMAKE_SOURCE_DIR@/src/lib/DriverFramework \
@CMAKE_SOURCE_DIR@/src/modules/uavcan/libuavcan \
EXCLUDE = @CMAKE_SOURCE_DIR@/src/modules/uavcan/libuavcan \
@CMAKE_SOURCE_DIR@/src/modules/micrortps_bridge/micro-CDR \
@CMAKE_SOURCE_DIR@/src/examples \
@CMAKE_SOURCE_DIR@/src/templates

2
Tools/astyle/files_to_check_code_style.sh
View File

@ -10,9 +10,9 @@ fi
exec find boards msg src platforms \
-path msg/templates/urtps -prune -o \
-path platforms/nuttx/NuttX -prune -o \
-path platforms/qurt/dspal -prune -o \
-path src/drivers/uavcan/libuavcan -prune -o \
-path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \
-path src/lib/DriverFramework -prune -o \
-path src/lib/ecl -prune -o \
-path src/lib/matrix -prune -o \
-path src/lib/systemlib/uthash -prune -o \

6
boards/aerotenna/ocpoc/default.cmake
View File

@ -34,8 +34,6 @@ px4_add_board(
pwm_out_sim
rc_input
#telemetry # all available telemetry drivers
DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers
mpu9250
MODULES
airspeed_selector
attitude_estimator_q
@ -49,7 +47,7 @@ px4_add_board(
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
#load_mon
local_position_estimator
logger
mavlink
@ -77,7 +75,7 @@ px4_add_board(
sd_bench
shutdown
tests # tests and test runner
top
#top
topic_listener
tune_control
ver

7
boards/atlflight/eagle/default.cmake
View File

@ -32,9 +32,6 @@ add_compile_options($<$<COMPILE_LANGUAGE:CXX>:-std=gnu++11>)
add_definitions(
-D__PX4_POSIX_EAGLE
-D__PX4_LINUX
# For DriverFramework
-D__DF_LINUX
)
px4_add_board(
@ -71,7 +68,7 @@ px4_add_board(
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
#load_mon
local_position_estimator
logger
mavlink
@ -108,7 +105,7 @@ px4_add_board(
sd_bench
shutdown
#tests # tests and test runner
top
#top
topic_listener
tune_control
ver

7
boards/atlflight/eagle/qurt.cmake
View File

@ -47,15 +47,10 @@ px4_add_board(
barometer/bmp280
gps
imu/mpu9250
magnetometer/hmc5883
#magnetometer/hmc5883
qshell/qurt
snapdragon_pwm_out
spektrum_rc
DF_DRIVERS
isl29501
ltc2946
mpu9250
trone
MODULES
airspeed_selector
attitude_estimator_q

7
boards/atlflight/excelsior/default.cmake
View File

@ -32,9 +32,6 @@ add_compile_options($<$<COMPILE_LANGUAGE:CXX>:-std=gnu++11>)
add_definitions(
-D__PX4_POSIX_EXCELSIOR
-D__PX4_LINUX
# For DriverFramework
-D__DF_LINUX
)
px4_add_board(
@ -71,7 +68,7 @@ px4_add_board(
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
#load_mon
local_position_estimator
logger
mavlink
@ -108,7 +105,7 @@ px4_add_board(
sd_bench
shutdown
#tests # tests and test runner
top
#top
topic_listener
tune_control
ver

5
boards/atlflight/excelsior/qurt.cmake
View File

@ -51,11 +51,6 @@ px4_add_board(
qshell/qurt
snapdragon_pwm_out
spektrum_rc
DF_DRIVERS
isl29501
ltc2946
mpu9250
trone
MODULES
airspeed_selector
attitude_estimator_q

6
boards/beaglebone/blue/default.cmake
View File

@ -26,8 +26,6 @@ px4_add_board(
pwm_out_sim
rc_input
#telemetry # all available telemetry drivers
DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers
mpu9250
MODULES
airspeed_selector
attitude_estimator_q
@ -41,7 +39,7 @@ px4_add_board(
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
#load_mon
local_position_estimator
logger
mavlink
@ -70,7 +68,7 @@ px4_add_board(
sd_bench
shutdown
tests # tests and test runner
top
#top
topic_listener
tune_control
ver

4
boards/beaglebone/blue/src/init.cpp
View File

@ -49,7 +49,6 @@ int rc_init(void)
#ifdef __RC_V0_3
return rc_initialize();
#else
#ifdef __DF_BBBLUE
if (rc_get_state() == RUNNING) { return 0; }
@ -111,7 +110,6 @@ int rc_init(void)
//i2c, barometer and mpu will be initialized later
rc_set_state(RUNNING);
#endif
return 0;
#endif
@ -123,7 +121,6 @@ void rc_cleaning(void)
#ifdef __RC_V0_3
rc_cleanup(); return ;
#else
#ifdef __DF_BBBLUE
if (rc_get_state() == EXITING) { return; }
@ -136,5 +133,4 @@ void rc_cleaning(void)
rc_remove_pid_file();
#endif
#endif
}

2
boards/emlid/navio2/cmake/upload.cmake
View File

@ -31,7 +31,7 @@
#
############################################################################
if($ENV{AUTOPILOT_HOST})
if(DEFINED ENV{AUTOPILOT_HOST})
set(AUTOPILOT_HOST $ENV{AUTOPILOT_HOST})
else()
set(AUTOPILOT_HOST "navio")

9
boards/emlid/navio2/default.cmake
View File

@ -28,11 +28,6 @@ px4_add_board(
pwm_out_sim
rc_input
#telemetry # all available telemetry drivers
DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers
isl29501
lsm9ds1
mpu9250
trone
MODULES
airspeed_selector
attitude_estimator_q
@ -46,7 +41,7 @@ px4_add_board(
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
#load_mon
local_position_estimator
logger
mavlink
@ -75,7 +70,7 @@ px4_add_board(
sd_bench
shutdown
tests # tests and test runner
top
#top
topic_listener
tune_control
ver

4
boards/px4/raspberrypi/default.cmake
View File

@ -39,7 +39,7 @@ px4_add_board(
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
#load_mon
local_position_estimator
logger
mavlink
@ -68,7 +68,7 @@ px4_add_board(
sd_bench
shutdown
tests # tests and test runner
top
#top
topic_listener
tune_control
ver

4
boards/px4/sitl/default.cmake
View File

@ -31,7 +31,7 @@ px4_add_board(
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
#load_mon
local_position_estimator
logger
mavlink
@ -65,7 +65,7 @@ px4_add_board(
sd_bench
shutdown
tests # tests and test runner
top
#top
topic_listener
tune_control
ver

4
boards/px4/sitl/rtps.cmake
View File

@ -32,7 +32,7 @@ px4_add_board(
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
#load_mon
local_position_estimator
logger
mavlink
@ -67,7 +67,7 @@ px4_add_board(
sd_bench
shutdown
tests # tests and test runner
top
#top
topic_listener
tune_control
ver

4
boards/px4/sitl/test.cmake
View File

@ -31,7 +31,7 @@ px4_add_board(
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
#load_mon
local_position_estimator
logger
mavlink
@ -65,7 +65,7 @@ px4_add_board(
sd_bench
shutdown
tests # tests and test runner
top
#top
topic_listener
tune_control
ver

16
cmake/px4_add_board.cmake
View File

@ -55,7 +55,6 @@
# [ SYSTEMCMDS <list> ]
# [ EXAMPLES <list> ]
# [ SERIAL_PORTS <list> ]
# [ DF_DRIVERS <list> ]
# [ CONSTRAINED_FLASH ]
# [ TESTING ]
# )
@ -77,7 +76,6 @@
# SYSTEMCMDS : list of system commands to build for this board (relative to src/systemcmds)
# EXAMPLES : list of example modules to build for this board (relative to src/examples)
# SERIAL_PORTS : mapping of user configurable serial ports and param facing name
# DF_DRIVERS : list of DriverFramework device drivers (includes DriverFramework driver and wrapper)
# CONSTRAINED_FLASH : flag to enable constrained flash options (eg limit init script status text)
# TESTING : flag to enable automatic inclusion of PX4 testing modules
#
@ -148,7 +146,6 @@ function(px4_add_board)
SYSTEMCMDS
EXAMPLES
SERIAL_PORTS
DF_DRIVERS
OPTIONS
BUILD_BOOTLOADER
CONSTRAINED_FLASH
@ -264,19 +261,6 @@ function(px4_add_board)
endforeach()
endif()
# DriverFramework drivers
if(DF_DRIVERS)
set(config_df_driver_list)
foreach(driver ${DF_DRIVERS})
list(APPEND config_df_driver_list ${driver})
if(EXISTS "${PX4_SOURCE_DIR}/src/drivers/driver_framework_wrapper/df_${driver}_wrapper")
list(APPEND config_module_list drivers/driver_framework_wrapper/df_${driver}_wrapper)
endif()
endforeach()
set(config_df_driver_list ${config_df_driver_list} PARENT_SCOPE)
endif()
# add board config directory src to build modules
file(RELATIVE_PATH board_support_src_rel ${PX4_SOURCE_DIR}/src ${PX4_BOARD_DIR})
list(APPEND config_module_list ${board_support_src_rel}/src)

1
cmake/px4_add_common_flags.cmake
View File

@ -177,7 +177,6 @@ function(px4_add_common_flags)
${PX4_SOURCE_DIR}/src
${PX4_SOURCE_DIR}/src/include
${PX4_SOURCE_DIR}/src/lib
${PX4_SOURCE_DIR}/src/lib/DriverFramework/framework/include
${PX4_SOURCE_DIR}/src/lib/matrix
${PX4_SOURCE_DIR}/src/modules
)

2
cmake/px4_add_module.cmake
View File

@ -132,7 +132,7 @@ function(px4_add_module)
endforeach()
endif()
elseif(DYNAMIC AND MAIN AND (${OS} STREQUAL "posix"))
elseif(DYNAMIC AND MAIN AND (${PX4_PLATFORM} STREQUAL "posix"))
add_library(${MODULE} SHARED ${SRCS})
target_compile_definitions(${MODULE} PRIVATE ${MAIN}_main=px4_module_main)
set_target_properties(${MODULE} PROPERTIES

49
platforms/common/apps.cpp.in
View File

@ -19,25 +19,17 @@ ${builtin_apps_decl_string}
int shutdown_main(int argc, char *argv[]);
int list_tasks_main(int argc, char *argv[]);
int list_files_main(int argc, char *argv[]);
int list_devices_main(int argc, char *argv[]);
int list_topics_main(int argc, char *argv[]);
int sleep_main(int argc, char *argv[]);
int wait_for_topic(int argc, char *argv[]);
}
extern void px4_show_devices(void);
void init_app_map(apps_map_type &apps)
{
${builtin_apps_string}
apps["shutdown"] = shutdown_main;
apps["list_tasks"] = list_tasks_main;
apps["list_files"] = list_files_main;
apps["list_devices"] = list_devices_main;
apps["list_topics"] = list_topics_main;
apps["sleep"] = sleep_main;
apps["wait_for_topic"] = wait_for_topic;
}
void list_builtins(apps_map_type &apps)
@ -60,18 +52,6 @@ int list_tasks_main(int argc, char *argv[])
return 0;
}
int list_devices_main(int argc, char *argv[])
{
px4_show_devices();
return 0;
}
int list_topics_main(int argc, char *argv[])
{
px4_show_topics();
return 0;
}
int list_files_main(int argc, char *argv[])
{
px4_show_files();
@ -90,32 +70,3 @@ int sleep_main(int argc, char *argv[])
px4_usleep(usecs);
return 0;
}
#include "uORB/uORB.h"
int wait_for_topic(int argc, char *argv[])
{
if (argc < 2 || argc > 3) {
printf("Usage: wait_for_topic <topic> [timeout_sec]\n");
return 1;
}
struct orb_metadata meta = { .o_name = argv[1],
.o_size = 0,
.o_size_no_padding = 0,
.o_fields = nullptr };
unsigned int timeout = (argc == 3) ? (unsigned int)atoi(argv[2]) : 0;
unsigned int elapsed = 0;
printf("Waiting for topic %s timeout %u\n", argv[1], timeout);
while (orb_exists(&meta, 0) != 0 && (timeout && (elapsed < timeout)))
{
px4_sleep(1);
elapsed += 1;
}
return 0;
}

17
platforms/common/include/px4_platform_common/defines.h
View File

@ -54,19 +54,6 @@ constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); }
constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); }
#endif /* __cplusplus */
#if defined(__PX4_NUTTX) || defined(__PX4_POSIX)
/****************************************************************************
* Building for NuttX or POSIX.
****************************************************************************/
/* Main entry point */
#define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[])
#else // defined(__PX4_NUTTX) || defined(__PX4_POSIX)
/****************************************************************************/
#error "No target OS defined"
#endif
#if defined(__PX4_NUTTX)
/****************************************************************************
* NuttX specific defines.
@ -99,9 +86,6 @@ constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); }
// NuttX _IOC is equivalent to Linux _IO
#define _PX4_IOC(x,y) _IO(x,y)
/* FIXME - Used to satisfy build */
#define getreg32(a) (*(volatile uint32_t *)(a))
#define USEC_PER_TICK (1000000/PX4_TICKS_PER_SEC)
#define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK)
@ -111,7 +95,6 @@ constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); }
# include "dspal_math.h"
# define PX4_ROOTFSDIR "."
# define PX4_TICKS_PER_SEC 1000L
# define SIOCDEVPRIVATE 999999
#else // __PX4_QURT

30
platforms/common/include/px4_platform_common/posix.h
View File

@ -44,7 +44,6 @@
#include <stdint.h>
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include <stdint.h>
#if defined(__PX4_QURT)
@ -60,7 +59,10 @@
#ifdef __PX4_NUTTX
#include <poll.h>
typedef struct pollfd px4_pollfd_struct_t;
typedef pollevent_t px4_pollevent_t;
#if defined(__cplusplus)
#define _GLOBAL ::
@ -73,7 +75,6 @@ typedef struct pollfd px4_pollfd_struct_t;
#define px4_write _GLOBAL write
#define px4_read _GLOBAL read
#define px4_poll _GLOBAL poll
#define px4_fsync _GLOBAL fsync
#define px4_access _GLOBAL access
#define px4_getpid _GLOBAL getpid
@ -83,34 +84,31 @@ typedef struct pollfd px4_pollfd_struct_t;
#define PX4_STACK_OVERHEAD (1024 * 11)
/**
* Terminates the calling process immediately.
* @return 0 on success, 1 on error
*/
#define px4_exit(status) ({return status;})
__BEGIN_DECLS
typedef short pollevent_t;
typedef short px4_pollevent_t;
typedef struct {
/* This part of the struct is POSIX-like */
int fd; /* The descriptor being polled */
pollevent_t events; /* The input event flags */
pollevent_t revents; /* The output event flags */
px4_pollevent_t events; /* The input event flags */
px4_pollevent_t revents; /* The output event flags */
/* Required for PX4 compatibility */
px4_sem_t *sem; /* Pointer to semaphore used to post output event */
void *priv; /* For use by drivers */
} px4_pollfd_struct_t;
#ifndef POLLIN
#define POLLIN (0x01)
#endif
__EXPORT int px4_open(const char *path, int flags, ...);
__EXPORT int px4_close(int fd);
__EXPORT ssize_t px4_read(int fd, void *buffer, size_t buflen);
__EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen);
__EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg);
__EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout);
__EXPORT int px4_fsync(int fd);
__EXPORT int px4_poll(px4_pollfd_struct_t *fds, unsigned int nfds, int timeout);
__EXPORT int px4_access(const char *pathname, int mode);
__EXPORT px4_task_t px4_getpid(void);
@ -127,13 +125,7 @@ __END_DECLS
#define PX4_STACK_ADJUSTED(_s) (_s * (__SIZEOF_POINTER__ >> 2) + PX4_STACK_OVERHEAD)
__BEGIN_DECLS
extern int px4_errno;
__EXPORT void px4_show_devices(void);
__EXPORT void px4_show_files(void);
__EXPORT const char *px4_get_device_names(unsigned int *handle);
__EXPORT void px4_show_topics(void);
__EXPORT const char *px4_get_topic_names(unsigned int *handle);
__END_DECLS

1
platforms/common/shutdown.cpp
View File

@ -94,6 +94,7 @@ int px4_register_shutdown_hook(shutdown_hook_t hook)
{
return -EINVAL;
}
int px4_unregister_shutdown_hook(shutdown_hook_t hook)
{
return -EINVAL;

1
platforms/nuttx/cmake/px4_impl_os.cmake
View File

@ -69,7 +69,6 @@ function(px4_os_add_flags)
add_definitions(
-D__PX4_NUTTX
-D__DF_NUTTX
-D_SYS_CDEFS_H_ # skip toolchain's <sys/cdefs.h>
-D_SYS_REENT_H_ # skip toolchain's <sys/reent.h>

2
platforms/nuttx/src/px4/common/CMakeLists.txt
View File

@ -40,7 +40,7 @@ if (NOT ${PX4_BOARD} MATCHES "px4_io")
board_fat_dma_alloc.c
console_buffer.cpp
gpio.c
px4_nuttx_tasks.c
tasks.cpp
px4_nuttx_impl.cpp
px4_init.cpp
)

1
platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h
View File

@ -34,7 +34,6 @@
__BEGIN_DECLS
#include <nuttx/spi/spi.h>
/* For historical reasons (NuttX STM32 numbering) PX4 bus numbering is 1 based
* All PX4 code, including, board code is written to assuming 1 based numbering.

10
platforms/nuttx/src/px4/common/px4_nuttx_tasks.c → platforms/nuttx/src/px4/common/tasks.cpp
View File

@ -33,11 +33,13 @@
****************************************************************************/
/**
* @file px4_nuttx_tasks.c
* @file tasks.cpp
* Implementation of existing task API for NuttX
*/
#include <nuttx/config.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/tasks.h>
#include <sys/wait.h>
#include <stdbool.h>
@ -50,10 +52,6 @@
#include <errno.h>
#include <stdbool.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/tasks.h>
void
px4_systemreset(bool to_bootloader)
{

5
platforms/posix/CMakeLists.txt
View File

@ -42,7 +42,6 @@ if (("${PX4_BOARD}" MATCHES "atlflight_eagle") OR ("${PX4_BOARD}" MATCHES "atlfl
px4_layer
parameters
${module_libraries}
${df_driver_libs}
${FASTRPC_ARM_LIBS}
pthread m rt
-Wl,--end-group
@ -57,8 +56,6 @@ else()
target_link_libraries(px4
PRIVATE
${module_libraries}
${df_driver_libs}
df_driver_framework
pthread m
# horrible circular dependencies that need to be teased apart
@ -109,8 +106,6 @@ endif()
if("${PX4_BOARD}" MATCHES "beaglebone_blue")
target_link_libraries(px4 PRIVATE robotics_cape)
add_dependencies(df_driver_framework librobotcontrol)
add_dependencies(df_mpu9250 librobotcontrol)
elseif("${PX4_BOARD}" MATCHES "emlid_navio2")
target_link_libraries(px4 PRIVATE atomic)

36
platforms/posix/cmake/px4_impl_os.cmake
View File

@ -223,10 +223,7 @@ function(px4_os_add_flags)
if ("${PX4_BOARD}" MATCHES "sitl")
if(UNIX AND APPLE)
add_definitions(
-D__PX4_DARWIN
-D__DF_DARWIN
)
add_definitions(-D__PX4_DARWIN)
if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS 8.0)
message(FATAL_ERROR "PX4 Firmware requires XCode 8 or newer on Mac OS. Version installed on this system: ${CMAKE_CXX_COMPILER_VERSION}")
@ -251,46 +248,21 @@ function(px4_os_add_flags)
-U__CUSTOM_FILE_IO__
)
else()
add_definitions(
-D__PX4_LINUX
-D__DF_LINUX
)
add_definitions(-D__PX4_LINUX)
endif()
elseif (("${PX4_BOARD}" MATCHES "navio2") OR ("${PX4_BOARD}" MATCHES "raspberrypi"))
#TODO: move to board support
add_definitions(
-D__PX4_LINUX
# For DriverFramework
-D__DF_LINUX
-D__DF_RPI
)
add_definitions(-D__PX4_LINUX)
elseif ("${PX4_BOARD}" MATCHES "aerotenna_ocpoc")
#TODO: move to board support
add_definitions(
-D__PX4_LINUX
-D__PX4_POSIX_OCPOC # TODO: remove
# For DriverFramework
-D__DF_LINUX
-D__DF_OCPOC
)
add_definitions(-D__PX4_LINUX)
elseif ("${PX4_BOARD}" MATCHES "beaglebone_blue")
#TODO: move to board support
add_definitions(
-D__PX4_LINUX
-D__PX4_POSIX_BBBLUE # TODO: remove
# For DriverFramework
-D__DF_LINUX
-D__DF_BBBLUE
-DRC_AUTOPILOT_EXT # Enable extensions in Robotics Cape Library, TODO: remove
)

0
platforms/posix/include/arch/board/board.h
View File

2
platforms/posix/src/px4/common/CMakeLists.txt
View File

@ -50,7 +50,7 @@ endif()
add_library(px4_layer
px4_posix_impl.cpp
px4_posix_tasks.cpp
tasks.cpp
px4_sem.cpp
px4_init.cpp
lib_crc32.c

3
platforms/posix/src/px4/common/main.cpp
View File

@ -71,7 +71,6 @@
#include <px4_platform_common/posix.h>
#include "apps.h"
#include "DriverFramework.hpp"
#include "px4_daemon/client.h"
#include "px4_daemon/server.h"
#include "px4_daemon/pxh.h"
@ -281,8 +280,6 @@ int main(int argc, char **argv)
return ret;
}
DriverFramework::Framework::initialize();
px4::init_once();
px4::init(argc, argv, "px4");

1
platforms/posix/src/px4/common/px4_posix_impl.cpp
View File

@ -68,7 +68,6 @@ void init_once();
void init_once()
{
_shell_task_id = pthread_self();
//printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id);
work_queues_init();
hrt_work_queue_init();

0
platforms/posix/src/px4/common/px4_posix_tasks.cpp → platforms/posix/src/px4/common/tasks.cpp
View File

1
platforms/posix/src/px4/common/test_stubs/CMakeLists.txt
View File

@ -33,7 +33,6 @@
set(SRCS
stub_daemon.cpp
stub_devmgr.cpp
stub_parameter.cpp
)

10
platforms/posix/src/px4/common/test_stubs/stub_devmgr.cpp
View File

@ -1,10 +0,0 @@
#include "stub_devmgr.h"
namespace DriverFramework
{
int DevMgr::getNextDeviceName(unsigned int &index, const char **instancename)
{
return stub_getNextDeviceName_callback(index, instancename);
}
}

8
platforms/posix/src/px4/common/test_stubs/stub_devmgr.h
View File

@ -1,8 +0,0 @@
#pragma once
#include "DevMgr.hpp"
#include <functional>
std::function<int(unsigned int &, const char **)> stub_getNextDeviceName_callback = [](unsigned int &,
const char **) {return 0;};

8
platforms/qurt/CMakeLists.txt
View File

@ -1,5 +1,7 @@
list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon")
px4_add_git_submodule(TARGET git_dspal PATH "${PX4_SOURCE_DIR}/platforms/qurt/dspal")
list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/platforms/qurt/dspal/cmake_hexagon")
include(toolchain/Toolchain-qurt)
include(fastrpc)
include(qurt_lib)
@ -33,7 +35,7 @@ if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
${PX4_BINARY_DIR}/platforms/qurt/px4muorb_skel.c
)
target_link_libraries(px4 PRIVATE ${module_libraries} ${df_driver_libs})
target_link_libraries(px4 PRIVATE ${module_libraries})
else()
# Generate the DSP lib and stubs but not the apps side executable
@ -45,8 +47,6 @@ else()
LINK_LIBS
modules__muorb__adsp
${module_libraries}
${df_driver_libs}
df_driver_framework
m
)

8
platforms/qurt/cmake/px4_impl_os.cmake
View File

@ -108,13 +108,11 @@ endfunction()
#
function(px4_os_add_flags)
set(DSPAL_ROOT src/lib/DriverFramework/dspal)
set(DSPAL_ROOT platforms/qurt/dspal)
include_directories(
${DSPAL_ROOT}/include
${DSPAL_ROOT}/mpu_spi/inc
${DSPAL_ROOT}/sys
${DSPAL_ROOT}/sys/sys
${DSPAL_ROOT}/uart_esc/inc
platforms/posix/include
platforms/qurt/include
@ -123,8 +121,6 @@ function(px4_os_add_flags)
add_definitions(
-D__PX4_POSIX
-D__PX4_QURT
-D__DF_QURT # For DriverFramework
)
add_compile_options(
@ -139,8 +135,6 @@ function(px4_os_add_flags)
set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS)
set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS)
set(DF_TARGET "qurt" CACHE STRING "DriverFramework target" FORCE)
endfunction()
#=============================================================================

1
platforms/qurt/dspal Submodule

@ -0,0 +1 @@
Subproject commit 0322a4e345e48ea28cb1cee14a33033cdaf0b16a

0
platforms/qurt/include/arch/board/board.h
View File

40
platforms/qurt/include/poll.h
View File

@ -1,40 +0,0 @@
/****************************************************************************
* include/poll.h
*
* Copyright (C) 2008-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 NuttX 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
typedef unsigned int nfds_t;
#define POLLIN (0x01)

3
platforms/qurt/include/sys/ioctl.h
View File

@ -1,3 +0,0 @@
#pragma once
#define _IO(x,y) (x+y)

2
platforms/qurt/src/px4/common/CMakeLists.txt
View File

@ -38,7 +38,7 @@ include_directories(${HEXAGON_8074_INCLUDES})
set(QURT_LAYER_SRCS
px4_qurt_impl.cpp
px4_qurt_tasks.cpp
tasks.cpp
lib_crc32.c
drv_hrt.cpp
qurt_stubs.c

2
platforms/qurt/src/px4/common/commands_hil.c
View File

@ -93,10 +93,8 @@ const char *get_commands()
"param set MAV_TYPE 2\n"
"mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"
"list_devices\n"
"list_files\n"
"list_tasks\n"
"list_topics\n"
"sleep 10\n"
"list_tasks\n"
"sleep 10\n"

2
platforms/qurt/src/px4/common/main.cpp
View File

@ -52,7 +52,6 @@
#include "get_commands.h"
#include "apps.h"
#include "DriverFramework.hpp"
#define MAX_ARGS 8 // max number of whitespace separated args after app name
@ -196,7 +195,6 @@ int dspal_entry(int argc, char *argv[])
PX4_INFO("In dspal_entry");
apps_map_type apps;
init_app_map(apps);
DriverFramework::Framework::initialize();
px4::init_once();
px4::init(argc, (char **)argv, "px4");
process_commands(apps, get_commands());

0
platforms/qurt/src/px4/common/px4_qurt_tasks.cpp → platforms/qurt/src/px4/common/tasks.cpp
View File

2
posix-configs/bbblue/px4.config
View File

@ -50,7 +50,7 @@ dataman start
bmp280 -I start
df_mpu9250_wrapper start
mpu9250 start
# options: -R rotation
gps start -d /dev/ttyS2 -s -p ubx

2
posix-configs/bbblue/px4_fw.config
View File

@ -50,7 +50,7 @@ dataman start
bmp280 -I start
df_mpu9250_wrapper start
mpu9250 start
# options: -R rotation
gps start -d /dev/ttyS2 -s -p ubx

14
posix-configs/eagle/200qx/px4.config
View File

@ -14,17 +14,11 @@ param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.08
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.001
param set ATT_W_MAG 0.00
param set SENS_BOARD_ROT 0
param set RC_RECEIVER_TYPE 1
param set UART_ESC_MODEL 2
param set UART_ESC_BAUD 250000
param set UART_ESC_MOTOR1 4
param set UART_ESC_MOTOR2 2
param set UART_ESC_MOTOR3 1
param set UART_ESC_MOTOR4 3
sleep 1
df_mpu9250_wrapper start
mpu9250 start
bmp280 start
gps start -d /dev/tty-4
rc_update start
@ -38,7 +32,5 @@ mc_rate_control start
uart_esc start -D /dev/tty-2
spektrum_rc start -d /dev/tty-1
sleep 1
list_devices
list_files
list_tasks
list_topics

12
posix-configs/eagle/210qc/px4.config
View File

@ -16,15 +16,9 @@ param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.001
param set ATT_W_MAG 0.00
param set SENS_BOARD_ROT 0
param set RC_RECEIVER_TYPE 1
param set UART_ESC_MODEL 2
param set UART_ESC_BAUD 250000
param set UART_ESC_MOTOR1 4
param set UART_ESC_MOTOR2 2
param set UART_ESC_MOTOR3 1
param set UART_ESC_MOTOR4 3
sleep 1
df_mpu9250_wrapper start
mpu9250 start
bmp280 start
gps start -d /dev/tty-4
rc_update start
@ -38,7 +32,5 @@ mc_rate_control start
uart_esc start -D /dev/tty-2
spektrum_rc start -d /dev/tty-1
sleep 1
list_devices
list_files
list_tasks
list_topics

5
posix-configs/eagle/flight/px4.config
View File

@ -6,11 +6,8 @@ gps start -d /dev/tty-4
param set MAV_TYPE 2
sleep 1
hmc5883 start
df_mpu9250_wrapper start_without_mag
mpu9250 start_without_mag
bmp280 start
df_trone_wrapper start
#df_ltc2946_wrapper start # (currently not working on all boards...)
#df_isl29501_wrapper start
sleep 1
rc_update start
sensors start

32
posix-configs/eagle/hil/px4.config
View File

@ -1,42 +1,16 @@
uorb start
qshell start
param set CAL_GYRO0_ID 2293760
param set CAL_ACC0_ID 1310720
param set CAL_ACC1_ID 1376256
param set CAL_MAG0_ID 196608
param set SYS_AUTOSTART 4001
param set SYS_AUTOCONFIG 1
param set MAV_TYPE 2
param set RC1_MAX 2015
param set RC1_MIN 996
param set RC1_TRIM 1502
param set RC1_REV -1
param set RC2_MAX 2016
param set RC2_MIN 995
param set RC2_TRIM 1500
param set RC3_MAX 2003
param set RC3_MIN 992
param set RC3_TRIM 992
param set RC4_MAX 2011
param set RC4_MIN 997
param set RC4_TRIM 1504
param set RC4_REV -1
param set RC6_MAX 2016
param set RC6_MIN 992
param set RC6_TRIM 1504
param set RC_CHAN_CNT 8
param set RC_MAP_MODE_SW 5
param set RC_MAP_POSCTL_SW 7
param set RC_MAP_RETURN_SW 8
param set MC_YAW_P 1.5
param set MC_PITCH_P 3.0
param set MC_ROLL_P 3.0
param set MC_YAWRATE_P 0.2
param set MC_PITCHRATE_P 0.03
param set MC_ROLLRATE_P 0.03
param set ATT_W_ACC 0.0002
param set ATT_W_MAG 0.002
param set ATT_W_GYRO_BIAS 0.05
sleep 1
rc_update start
commander start -hil
@ -49,10 +23,8 @@ land_detector start multicopter
sleep 1
pwm_out_sim start
mixer load /dev/pwm_output0 /startup/quad_x.main.mix
list_devices
list_files
list_tasks
list_topics
sleep 10
list_tasks
sleep 10

108
posix-configs/eagle/init/rcS
View File

@ -20,15 +20,10 @@ then
qshell gps start -d /dev/tty-4
qshell hmc5883 start
qshell df_trone_wrapper start
#qshell df_isl29501_wrapper start
# Qualcomm 200qx microheli platform
elif param compare SYS_AUTOSTART 4200
then
param set CAL_GYRO0_ID 100
param set CAL_ACC0_ID 100
param set CAL_MAG0_ID 101
param set MAV_TYPE 2
param set MC_YAW_P 7.0
param set MC_YAWRATE_P 0.08
@ -42,56 +37,11 @@ then
param set MC_ROLLRATE_P 0.08
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.0001
param set RC_MAP_THROTTLE 1
param set RC_MAP_ROLL 2
param set RC_MAP_PITCH 3
param set RC_MAP_YAW 4
param set RC_MAP_MODE_SW 5
param set RC_MAP_POSCTL_SW 6
param set RC1_MAX 1900
param set RC1_MIN 1099
param set RC1_TRIM 1099
param set RC1_REV 1
param set RC2_MAX 1900
param set RC2_MIN 1099
param set RC2_TRIM 1500
param set RC2_REV -1
param set RC3_MAX 1900
param set RC3_MIN 1099
param set RC3_TRIM 1500
param set RC3_REV 1
param set RC4_MAX 1900
param set RC4_MIN 1099
param set RC4_TRIM 1500
param set RC4_REV -1
param set RC5_MAX 1900
param set RC5_MIN 1099
param set RC5_TRIM 1500
param set RC5_REV 1
param set RC6_MAX 1900
param set RC6_MIN 1099
param set RC6_TRIM 1099
param set RC6_REV 1
param set ATT_W_MAG 0.00
param set PE_MAG_NOISE 1.0f
param set SENS_BOARD_ROT 0
param set RC_RECEIVER_TYPE 1
param set UART_ESC_MODEL 2
param set UART_ESC_BAUDRTE 250000
param set UART_ESC_MOTOR1 4
param set UART_ESC_MOTOR2 2
param set UART_ESC_MOTOR3 1
param set UART_ESC_MOTOR4 3
qshell uart_esc start -D /dev/tty-2
qshell rc_receiver start -D /dev/tty-1
# Qualcomm internal 210qc platform
elif param compare SYS_AUTOSTART 4210
then
param set CAL_GYRO0_ID 100
param set CAL_ACC0_ID 100
param set CAL_MAG0_ID 101
param set MAV_TYPE 2
param set MC_YAW_P 12
param set MC_YAWRATE_P 0.08
@ -105,64 +55,8 @@ then
param set MC_ROLLRATE_P 0.08
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.001
param set RC_MAP_THROTTLE 1
param set RC_MAP_ROLL 2
param set RC_MAP_PITCH 3
param set RC_MAP_YAW 4
param set RC_MAP_MODE_SW 5
param set RC_MAP_POSCTL_SW 6
param set RC1_MAX 1900
param set RC1_MIN 1099
param set RC1_TRIM 1099
param set RC1_REV 1
param set RC2_MAX 1900
param set RC2_MIN 1099
param set RC2_TRIM 1500
param set RC2_REV -1
param set RC3_MAX 1900
param set RC3_MIN 1099
param set RC3_TRIM 1500
param set RC3_REV 1
param set RC4_MAX 1900
param set RC4_MIN 1099
param set RC4_TRIM 1500
param set RC4_REV -1
param set RC5_MAX 1900
param set RC5_MIN 1099
param set RC5_TRIM 1500
param set RC5_REV 1
param set RC6_MAX 1900
param set RC6_MIN 1099
param set RC6_TRIM 1099
param set RC6_REV 1
param set ATT_W_MAG 0.00
param set PE_MAG_NOISE 1.0f
param set SENS_BOARD_ROT 0
param set CAL_GYRO0_XOFF -0.0028
param set CAL_GYRO0_YOFF -0.0047
param set CAL_GYRO0_ZOFF -0.0034
param set CAL_MAG0_XOFF 0.0000
param set CAL_MAG0_YOFF 0.0000
param set CAL_MAG0_ZOFF 0.0000
param set CAL_MAG0_XSCALE 1.0000
param set CAL_MAG0_YSCALE 1.0000
param set CAL_MAG0_ZSCALE 1.0000
param set CAL_ACC0_XOFF -0.0941
param set CAL_ACC0_YOFF 0.1168
param set CAL_ACC0_ZOFF -0.0398
param set CAL_ACC0_XSCALE 1.0004
param set CAL_ACC0_YSCALE 0.9974
param set CAL_ACC0_ZSCALE 0.9951
param set RC_RECEIVER_TYPE 1
param set UART_ESC_MODEL 2
param set UART_ESC_BAUDRTE 250000
param set UART_ESC_MOTOR1 4
param set UART_ESC_MOTOR2 2
param set UART_ESC_MOTOR3 1
param set UART_ESC_MOTOR4 3
uart_esc start -D /dev/tty-2
rc_receiver start -D /dev/tty-1
else
echo "NO AIRFRAME CHOSEN!"
echo "Please set parameter to select airframe:"
@ -171,7 +65,7 @@ else
fi
qshell df_mpu9250_wrapper start
qshell mpu9250 start
qshell bmp280 start
qshell rc_update start
qshell sensors start

61
posix-configs/excelsior/px4.config
View File

@ -1,8 +1,5 @@
uorb start
qshell start
param set CAL_GYRO0_ID 100
param set CAL_ACC0_ID 100
param set CAL_MAG0_ID 101
param set SYS_AUTOSTART 4001
param set MAV_TYPE 2
param set MC_YAW_P 12
@ -17,62 +14,10 @@ param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.08
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.001
param set RC_MAP_THROTTLE 1
param set RC_MAP_ROLL 2
param set RC_MAP_PITCH 3
param set RC_MAP_YAW 4
param set RC_MAP_MODE_SW 5
param set RC_MAP_POSCTL_SW 6
param set RC1_MAX 1900
param set RC1_MIN 1099
param set RC1_TRIM 1099
param set RC1_REV 1
param set RC2_MAX 1900
param set RC2_MIN 1099
param set RC2_TRIM 1500
param set RC2_REV -1
param set RC3_MAX 1900
param set RC3_MIN 1099
param set RC3_TRIM 1500
param set RC3_REV 1
param set RC4_MAX 1900
param set RC4_MIN 1099
param set RC4_TRIM 1500
param set RC4_REV -1
param set RC5_MAX 1900
param set RC5_MIN 1099
param set RC5_TRIM 1500
param set RC5_REV 1
param set RC6_MAX 1900
param set RC6_MIN 1099
param set RC6_TRIM 1099
param set RC6_REV 1
param set ATT_W_MAG 0.00
param set SENS_BOARD_ROT 4
param set CAL_GYRO0_XOFF -0.0028
param set CAL_GYRO0_YOFF -0.0047
param set CAL_GYRO0_ZOFF -0.0034
param set CAL_MAG0_XOFF 0.0000
param set CAL_MAG0_YOFF 0.0000
param set CAL_MAG0_ZOFF 0.0000
param set CAL_MAG0_XSCALE 1.0000
param set CAL_MAG0_YSCALE 1.0000
param set CAL_MAG0_ZSCALE 1.0000
param set CAL_ACC0_XOFF -0.0941
param set CAL_ACC0_YOFF 0.1168
param set CAL_ACC0_ZOFF -0.0398
param set CAL_ACC0_XSCALE 1.0004
param set CAL_ACC0_YSCALE 0.9974
param set CAL_ACC0_ZSCALE 0.9951
param set RC_RECEIVER_TYPE 1
param set UART_ESC_MODEL 2
param set UART_ESC_BAUD 250000
param set UART_ESC_MOTOR1 4
param set UART_ESC_MOTOR2 2
param set UART_ESC_MOTOR3 1
param set UART_ESC_MOTOR4 3
sleep 1
df_mpu9250_wrapper start
mpu9250 start
bmp280 start
rc_update start
sensors start
@ -85,7 +30,5 @@ mc_rate_control start
uart_esc start -D /dev/tty-1
spektrum_rc start -d /dev/tty-101
sleep 1
list_devices
list_files
list_tasks
list_topics

3
posix-configs/ocpoc/px4.config
View File

@ -15,9 +15,10 @@ param set MAV_BROADCAST 1
param set MAV_TYPE 2
param set MAV_SYS_ID 1
df_mpu9250_wrapper start_without_mag -R 10
mpu9250 -R 10 start
hmc5883 start
ms5611 start
rgbled start -b 1
adc start

8
posix-configs/rpi/px4.config
View File

@ -19,11 +19,11 @@ param set BAT_CNT_V_CURR 0.001
dataman start
df_lsm9ds1_wrapper start -R 4
#lsm9ds1 start -R 4
#lsm9ds1_mag start -R 4
#df_mpu9250_wrapper start -R 10
mpu9250 -R 8 start
lsm9ds1 -R 4 start
lsm9ds1_mag -R 4 start
ms5611 start
navio_rgbled start
adc start

8
posix-configs/rpi/px4_fw.config
View File

@ -19,11 +19,11 @@ param set BAT_CNT_V_CURR 0.001
dataman start
df_lsm9ds1_wrapper start -R 4
#lsm9ds1 start -R 4
#lsm9ds1_mag start -R 4
#df_mpu9250_wrapper start -R 10
mpu9250 -R 8 start
lsm9ds1 -R 4 start
lsm9ds1_mag -R 4 start
ms5611 start
navio_rgbled start
adc start

8
posix-configs/rpi/px4_test.config
View File

@ -19,11 +19,11 @@ param set BAT_CNT_V_CURR 0.001
dataman start
df_lsm9ds1_wrapper start -R 4
#lsm9ds1 start -R 4
#lsm9ds1_mag start -R 4
#df_mpu9250_wrapper start -R 10
mpu9250 -R 8 start
lsm9ds1 -R 4 start
lsm9ds1_mag -R 4 start
ms5611 start
navio_rgbled start
adc start

45
src/drivers/driver_framework_wrapper/df_ak8963_wrapper/CMakeLists.txt
View File

@ -1,45 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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_directories(../../../lib/DriverFramework/drivers)
px4_add_module(
MODULE platforms__posix__drivers__df_ak8963_wrapper
MAIN df_ak8963_wrapper
SRCS
df_ak8963_wrapper.cpp
DEPENDS
git_driverframework
df_driver_framework
df_ak8963
)

459
src/drivers/driver_framework_wrapper/df_ak8963_wrapper/df_ak8963_wrapper.cpp
View File

@ -1,459 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 df_ak8963_wrapper.cpp
* Lightweight driver to access the AK8963 of the DriverFramework.
*/
#include <px4_platform_common/px4_config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_platform_common/getopt.h>
#include <errno.h>
#include <lib/parameters/param.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/parameter_update.h>
#include <board_config.h>
#include <lib/conversion/rotation.h>
#include <ak8963/AK8963.hpp>
#include <DevMgr.hpp>
extern "C" { __EXPORT int df_ak8963_wrapper_main(int argc, char *argv[]); }
using namespace DriverFramework;
class DfAK8963Wrapper : public AK8963
{
public:
DfAK8963Wrapper(enum Rotation rotation);
~DfAK8963Wrapper();
/**
* Start automatic measurement.
*
* @return 0 on success
*/
int start();
/**
* Stop automatic measurement.
*
* @return 0 on success
*/
int stop();
void print_calibration();
private:
int _publish(struct mag_sensor_data &data);
void _update_mag_calibration();
orb_advert_t _mag_topic;
int _param_update_sub;
struct mag_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
} _mag_calibration;
matrix::Dcmf _rotation_matrix;
int _mag_orb_class_instance;
perf_counter_t _mag_sample_perf;
};
DfAK8963Wrapper::DfAK8963Wrapper(enum Rotation rotation) :
AK8963(MAG_DEVICE_PATH),
_mag_topic(nullptr),
_param_update_sub(-1),
_mag_calibration{},
_mag_orb_class_instance(-1),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
{
// Set sane default calibration values
_mag_calibration.x_scale = 1.0f;
_mag_calibration.y_scale = 1.0f;
_mag_calibration.z_scale = 1.0f;
_mag_calibration.x_offset = 0.0f;
_mag_calibration.y_offset = 0.0f;
_mag_calibration.z_offset = 0.0f;
// Get sensor rotation matrix
_rotation_matrix = get_rot_matrix(rotation);
}
DfAK8963Wrapper::~DfAK8963Wrapper()
{
perf_free(_mag_sample_perf);
}
int DfAK8963Wrapper::start()
{
/* Subscribe to param update topic. */
if (_param_update_sub < 0) {
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
}
/* Init device and start sensor. */
int ret = init();
if (ret != 0) {
PX4_ERR("AK8963 init fail: %d", ret);
return ret;
}
ret = AK8963::start();
if (ret != 0) {
PX4_ERR("AK8963 start fail: %d", ret);
return ret;
}
/* Force getting the calibration values. */
_update_mag_calibration();
return 0;
}
int DfAK8963Wrapper::stop()
{
/* Stop sensor. */
int ret = AK8963::stop();
if (ret != 0) {
PX4_ERR("AK8963 stop fail: %d", ret);
return ret;
}
return 0;
}
void DfAK8963Wrapper::_update_mag_calibration()
{
// TODO: replace magic number
for (unsigned i = 0; i < 3; ++i) {
// TODO: remove printfs and add error counter
char str[30];
(void)sprintf(str, "CAL_MAG%u_ID", i);
int32_t device_id;
int res = param_get(param_find(str), &device_id);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
continue;
}
if ((uint32_t)device_id != m_id.dev_id) {
continue;
}
(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
res = param_get(param_find(str), &_mag_calibration.x_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
res = param_get(param_find(str), &_mag_calibration.y_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
res = param_get(param_find(str), &_mag_calibration.z_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_XOFF", i);
res = param_get(param_find(str), &_mag_calibration.x_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YOFF", i);
res = param_get(param_find(str), &_mag_calibration.y_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
res = param_get(param_find(str), &_mag_calibration.z_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
}
}
void DfAK8963Wrapper::print_calibration()
{
PX4_INFO("calibration x_offset: %f", (double)_mag_calibration.x_offset);
PX4_INFO("calibration x_scale: %f", (double)_mag_calibration.x_scale);
PX4_INFO("calibration y_offset: %f", (double)_mag_calibration.y_offset);
PX4_INFO("calibration y_scale: %f", (double)_mag_calibration.y_scale);
PX4_INFO("calibration z_offset: %f", (double)_mag_calibration.z_offset);
PX4_INFO("calibration z_scale: %f", (double)_mag_calibration.z_scale);
}
int DfAK8963Wrapper::_publish(struct mag_sensor_data &data)
{
/* Check if calibration values are still up-to-date. */
bool updated;
orb_check(_param_update_sub, &updated);
if (updated) {
parameter_update_s parameter_update;
orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
_update_mag_calibration();
}
/* Publish mag first. */
perf_begin(_mag_sample_perf);
sensor_mag_s mag_report = {};
mag_report.timestamp = hrt_absolute_time();
mag_report.is_external = true;
// TODO: remove these (or get the values)
mag_report.x_raw = 0;
mag_report.y_raw = 0;
mag_report.z_raw = 0;
matrix::Vector3f mag_val(data.field_x_ga, data.field_y_ga, data.field_z_ga);
// apply sensor rotation on the accel measurement
mag_val = _rotation_matrix * mag_val;
mag_report.x = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
mag_report.y = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
mag_report.z = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;
// TODO: get these right
//mag_report.scaling = -1.0f;
mag_report.device_id = m_id.dev_id;
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {
if (_mag_topic == nullptr) {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
&_mag_orb_class_instance, ORB_PRIO_HIGH);
} else {
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
}
}
perf_end(_mag_sample_perf);
return 0;
};
namespace df_ak8963_wrapper
{
DfAK8963Wrapper *g_dev = nullptr;
int start(enum Rotation rotation);
int stop();
int info();
void usage();
int start(enum Rotation rotation)
{
g_dev = new DfAK8963Wrapper(rotation);
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfAK8963Wrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfAK8963Wrapper start failed");
return ret;
}
// Open the MAG sensor
DevHandle h;
DevMgr::getHandle(MAG_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
MAG_DEVICE_PATH, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
return 0;
}
int stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
int ret = g_dev->stop();
if (ret != 0) {
PX4_ERR("driver could not be stopped");
return ret;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_DEBUG("state @ %p", g_dev);
g_dev->print_calibration();
return 0;
}
void
usage()
{
PX4_INFO("Usage: df_ak8963_wrapper 'start', 'info', 'stop'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
}
} // namespace df_ak8963_wrapper
int
df_ak8963_wrapper_main(int argc, char *argv[])
{
int ch;
enum Rotation rotation = ROTATION_NONE;
int ret = 0;
int myoptind = 1;
const char *myoptarg = NULL;
/* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
break;
default:
df_ak8963_wrapper::usage();
return 0;
}
}
if (argc <= 1) {
df_ak8963_wrapper::usage();
return 1;
}
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ret = df_ak8963_wrapper::start(rotation);
}
else if (!strcmp(verb, "stop")) {
ret = df_ak8963_wrapper::stop();
}
else if (!strcmp(verb, "info")) {
ret = df_ak8963_wrapper::info();
}
else {
df_ak8963_wrapper::usage();
return 1;
}
return ret;
}

45
src/drivers/driver_framework_wrapper/df_isl29501_wrapper/CMakeLists.txt
View File

@ -1,45 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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_directories(../../../lib/DriverFramework/drivers)
px4_add_module(
MODULE platforms__posix__drivers__df_isl29501_wrapper
MAIN df_isl29501_wrapper
SRCS
df_isl29501_wrapper.cpp
DEPENDS
git_driverframework
df_driver_framework
df_isl29501
)

378
src/drivers/driver_framework_wrapper/df_isl29501_wrapper/df_isl29501_wrapper.cpp
View File

@ -1,378 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 df_isl29501_wrapper.cpp
* Driver to access the ISL29501 of the DriverFramework.
*
* @author Zach Lovett <zach.lovett@3drobotics.com>
* @author Siddharth B Purohit <sid@3drobotics.com>
*/
#include <px4_platform_common/px4_config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_platform_common/getopt.h>
#include <errno.h>
#include <string>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_range_finder.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
#include <isl29501/isl29501.hpp>
#include <DevMgr.hpp>
extern "C" { __EXPORT int df_isl29501_wrapper_main(int argc, char *argv[]); }
using namespace DriverFramework;
class DfISL29501Wrapper : public ISL29501
{
public:
DfISL29501Wrapper();
~DfISL29501Wrapper();
/**
* Start automatic measurement.
*
* @return 0 on success
*/
int start();
/**
* Stop automatic measurement.
*
* @return 0 on success
*/
int stop();
private:
int _publish(struct range_sensor_data &data);
orb_advert_t _range_topic;
int _orb_class_instance;
// perf_counter_t _range_sample_perf;
};
DfISL29501Wrapper::DfISL29501Wrapper(/*enum Rotation rotation*/) :
ISL29501(ISL_DEVICE_PATH),
_range_topic(nullptr),
_orb_class_instance(-1)
{
}
DfISL29501Wrapper::~DfISL29501Wrapper()
{
}
int DfISL29501Wrapper::start()
{
int ret;
ret = ISL29501::init();
if (ret != 0) {
PX4_ERR("ISL init fail: %d", ret);
return ret;
}
ret = ISL29501::start();
if (ret != 0) {
PX4_ERR("ISL start fail: %d", ret);
return ret;
}
return 0;
}
int DfISL29501Wrapper::stop()
{
/* Stop sensor. */
int ret = ISL29501::stop();
if (ret != 0) {
PX4_ERR("ISL stop fail: %d", ret);
return ret;
}
return 0;
}
int DfISL29501Wrapper::_publish(struct range_sensor_data &data)
{
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
d.timestamp = hrt_absolute_time();
d.min_distance = float(ISL_MIN_DISTANCE); /* m */
d.max_distance = float(ISL_MAX_DISTANCE); /* m */
d.current_distance = float(data.dist);
d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
d.id = 0; // TODO set proper ID
d.variance = 0.0f;
d.signal_quality = -1;
if (_range_topic == nullptr) {
_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
&_orb_class_instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
}
return 0;
};
namespace df_isl29501_wrapper
{
DfISL29501Wrapper *g_dev = nullptr;
int start();
int stop();
int info();
int probe();
int calibration();
void usage();
int start()
{
PX4_ERR("start");
g_dev = new DfISL29501Wrapper();
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfISL29501Wrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfISL29501Wrapper start failed");
return ret;
}
// Open the range sensor
DevHandle h;
DevMgr::getHandle(ISL_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
ISL_DEVICE_PATH, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
return 0;
}
int stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
int ret = g_dev->stop();
if (ret != 0) {
PX4_ERR("driver could not be stopped");
return ret;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_DEBUG("state @ %p", g_dev);
return 0;
}
/**
* Who am i
*/
int
probe()
{
int ret;
if (g_dev == nullptr) {
ret = start();
if (ret) {
PX4_ERR("Failed to start");
return ret;
}
}
ret = g_dev->probe();
if (ret) {
PX4_ERR("Failed to probe");
return ret;
}
PX4_DEBUG("state @ %p", g_dev);
return 0;
}
/**
* Calibration
* runs calibration routine for ISL29501
* TODO: implement calibration user interface and parameter system to store calib
* Note: Currently only serves debugging purpose, user is required to manually
* set offset inside code.
*/
int
calibration()
{
int ret;
if (g_dev == nullptr) {
ret = start();
if (ret) {
PX4_ERR("Failed to start");
return ret;
}
}
ret = g_dev->calibration();
if (ret) {
PX4_ERR("Failed to calibrate");
return ret;
}
PX4_DEBUG("state @ %p", g_dev);
return 0;
}
void
usage()
{
PX4_WARN("Usage: df_isl_wrapper 'start', 'info', 'stop', 'calib', 'probe'");
}
} // namespace df_isl_wrapper
int
df_isl29501_wrapper_main(int argc, char *argv[])
{
int ret = 0;
int myoptind = 1;
if (argc <= 1) {
df_isl29501_wrapper::usage();
return 1;
}
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ret = df_isl29501_wrapper::start(/*rotation*/);
}
else if (!strcmp(verb, "stop")) {
ret = df_isl29501_wrapper::stop();
}
else if (!strcmp(verb, "info")) {
ret = df_isl29501_wrapper::info();
}
else if (!strcmp(verb, "probe")) {
ret = df_isl29501_wrapper::probe();
}
else if (!strcmp(verb, "calib")) {
df_isl29501_wrapper::calibration();
return 1;
}
else {
df_isl29501_wrapper::usage();
return 1;
}
return ret;
}

45
src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/CMakeLists.txt
View File

@ -1,45 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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_directories(../../../lib/DriverFramework/drivers)
px4_add_module(
MODULE platforms__posix__drivers__df_lsm9ds1_wrapper
MAIN df_lsm9ds1_wrapper
SRCS
df_lsm9ds1_wrapper.cpp
DEPENDS
git_driverframework
df_driver_framework
df_lsm9ds1
)

854
src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp
View File

@ -1,854 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 df_lsm9ds1_wrapper.cpp
* Lightweight driver to access the MPU9250 of the DriverFramework.
*
* @author Miguel Arroyo <miguel@arroyo.me>
*/
#include <px4_platform_common/px4_config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_platform_common/getopt.h>
#include <errno.h>
#include <systemlib/err.h>
#include <lib/parameters/param.h>
#include <perf/perf_counter.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/device/integrator.h>
#include <lib/conversion/rotation.h>
#include <uORB/topics/parameter_update.h>
#include <lsm9ds1/LSM9DS1.hpp>
#include <DevMgr.hpp>
// We don't want to auto publish, therefore set this to 0.
#define LSM9DS1_NEVER_AUTOPUBLISH_US 0
extern "C" { __EXPORT int df_lsm9ds1_wrapper_main(int argc, char *argv[]); }
using namespace DriverFramework;
class DfLsm9ds1Wrapper : public LSM9DS1
{
public:
DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation);
~DfLsm9ds1Wrapper();
/**
* Start automatic measurement.
*
* @return 0 on success
*/
int start();
/**
* Stop automatic measurement.
*
* @return 0 on success
*/
int stop();
/**
* Print some debug info.
*/
void info();
private:
int _publish(struct imu_sensor_data &data);
void _update_accel_calibration();
void _update_gyro_calibration();
void _update_mag_calibration();
orb_advert_t _accel_topic;
orb_advert_t _gyro_topic;
orb_advert_t _mag_topic;
orb_advert_t _mavlink_log_pub;
int _param_update_sub;
struct accel_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
} _accel_calibration;
struct gyro_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
} _gyro_calibration;
struct mag_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
} _mag_calibration;
matrix::Dcmf _rotation_matrix;
int _accel_orb_class_instance;
int _gyro_orb_class_instance;
int _mag_orb_class_instance;
Integrator _accel_int;
Integrator _gyro_int;
unsigned _publish_count;
perf_counter_t _read_counter;
perf_counter_t _error_counter;
perf_counter_t _fifo_overflow_counter;
perf_counter_t _fifo_corruption_counter;
perf_counter_t _gyro_range_hit_counter;
perf_counter_t _accel_range_hit_counter;
perf_counter_t _publish_perf;
hrt_abstime _last_accel_range_hit_time;
uint64_t _last_accel_range_hit_count;
bool _mag_enabled;
};
DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) :
LSM9DS1(IMU_DEVICE_ACC_GYRO, IMU_DEVICE_MAG),
_accel_topic(nullptr),
_gyro_topic(nullptr),
_mag_topic(nullptr),
_mavlink_log_pub(nullptr),
_param_update_sub(-1),
_accel_calibration{},
_gyro_calibration{},
_mag_calibration{},
_accel_orb_class_instance(-1),
_gyro_orb_class_instance(-1),
_mag_orb_class_instance(-1),
_accel_int(LSM9DS1_NEVER_AUTOPUBLISH_US, false),
_gyro_int(LSM9DS1_NEVER_AUTOPUBLISH_US, true),
_publish_count(0),
_read_counter(perf_alloc(PC_COUNT, "lsm9ds1_reads")),
_error_counter(perf_alloc(PC_COUNT, "lsm9ds1_errors")),
_fifo_overflow_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_overflows")),
_fifo_corruption_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_corruptions")),
_gyro_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_gyro_range_hits")),
_accel_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_accel_range_hits")),
_publish_perf(perf_alloc(PC_ELAPSED, "lsm9ds1_publish")),
_last_accel_range_hit_time(0),
_last_accel_range_hit_count(0),
_mag_enabled(mag_enabled)
{
// Set sane default calibration values
_accel_calibration.x_scale = 1.0f;
_accel_calibration.y_scale = 1.0f;
_accel_calibration.z_scale = 1.0f;
_accel_calibration.x_offset = 0.0f;
_accel_calibration.y_offset = 0.0f;
_accel_calibration.z_offset = 0.0f;
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
if (_mag_enabled) {
_mag_calibration.x_scale = 1.0f;
_mag_calibration.y_scale = 1.0f;
_mag_calibration.z_scale = 1.0f;
_mag_calibration.x_offset = 0.0f;
_mag_calibration.y_offset = 0.0f;
_mag_calibration.z_offset = 0.0f;
}
// Get sensor rotation matrix
_rotation_matrix = get_rot_matrix(rotation);
}
DfLsm9ds1Wrapper::~DfLsm9ds1Wrapper()
{
perf_free(_read_counter);
perf_free(_error_counter);
perf_free(_fifo_overflow_counter);
perf_free(_fifo_corruption_counter);
perf_free(_gyro_range_hit_counter);
perf_free(_accel_range_hit_counter);
perf_free(_publish_perf);
}
int DfLsm9ds1Wrapper::start()
{
// TODO: don't publish garbage here
sensor_accel_s accel_report = {};
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
&_accel_orb_class_instance, ORB_PRIO_DEFAULT);
if (_accel_topic == nullptr) {
PX4_ERR("sensor_accel advert fail");
return -1;
}
// TODO: don't publish garbage here
sensor_gyro_s gyro_report = {};
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
&_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
if (_gyro_topic == nullptr) {
PX4_ERR("sensor_gyro advert fail");
return -1;
}
if (_mag_enabled) {
sensor_mag_s mag_report = {};
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
&_mag_orb_class_instance, ORB_PRIO_DEFAULT);
if (_mag_topic == nullptr) {
PX4_ERR("sensor_mag advert fail");
return -1;
}
}
/* Subscribe to param update topic. */
if (_param_update_sub < 0) {
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
}
/* Init device and start sensor. */
int ret = init();
if (ret != 0) {
PX4_ERR("LSM9DS1 init fail: %d", ret);
return ret;
}
ret = LSM9DS1::start();
if (ret != 0) {
PX4_ERR("LSM9DS1 start fail: %d", ret);
return ret;
}
PX4_DEBUG("LSM9DS1 device id is: %d", m_id.dev_id);
/* Force getting the calibration values. */
_update_accel_calibration();
_update_gyro_calibration();
_update_mag_calibration();
return 0;
}
int DfLsm9ds1Wrapper::stop()
{
/* Stop sensor. */
int ret = LSM9DS1::stop();
if (ret != 0) {
PX4_ERR("LSM9DS1 stop fail: %d", ret);
return ret;
}
return 0;
}
void DfLsm9ds1Wrapper::info()
{
perf_print_counter(_read_counter);
perf_print_counter(_error_counter);
perf_print_counter(_fifo_overflow_counter);
perf_print_counter(_fifo_corruption_counter);
perf_print_counter(_gyro_range_hit_counter);
perf_print_counter(_accel_range_hit_counter);
perf_print_counter(_publish_perf);
}
void DfLsm9ds1Wrapper::_update_gyro_calibration()
{
// TODO: replace magic number
for (unsigned i = 0; i < 3; ++i) {
// TODO: remove printfs and add error counter
char str[30];
(void)sprintf(str, "CAL_GYRO%u_ID", i);
int32_t device_id;
int res = param_get(param_find(str), &device_id);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
continue;
}
if ((uint32_t)device_id != m_id.dev_id) {
continue;
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
res = param_get(param_find(str), &_gyro_calibration.x_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
res = param_get(param_find(str), &_gyro_calibration.y_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
res = param_get(param_find(str), &_gyro_calibration.z_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
// We got calibration values, let's exit.
return;
}
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
}
void DfLsm9ds1Wrapper::_update_accel_calibration()
{
// TODO: replace magic number
for (unsigned i = 0; i < 3; ++i) {
// TODO: remove printfs and add error counter
char str[30];
(void)sprintf(str, "CAL_ACC%u_ID", i);
int32_t device_id;
int res = param_get(param_find(str), &device_id);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
continue;
}
if ((uint32_t)device_id != m_id.dev_id) {
continue;
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
res = param_get(param_find(str), &_accel_calibration.x_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
res = param_get(param_find(str), &_accel_calibration.y_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
res = param_get(param_find(str), &_accel_calibration.z_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
res = param_get(param_find(str), &_accel_calibration.x_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
res = param_get(param_find(str), &_accel_calibration.y_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
res = param_get(param_find(str), &_accel_calibration.z_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
// We got calibration values, let's exit.
return;
}
// Set sane default calibration values
_accel_calibration.x_scale = 1.0f;
_accel_calibration.y_scale = 1.0f;
_accel_calibration.z_scale = 1.0f;
_accel_calibration.x_offset = 0.0f;
_accel_calibration.y_offset = 0.0f;
_accel_calibration.z_offset = 0.0f;
}
void DfLsm9ds1Wrapper::_update_mag_calibration()
{
if (!_mag_enabled) {
return;
}
// TODO: replace magic number
for (unsigned i = 0; i < 3; ++i) {
// TODO: remove printfs and add error counter
char str[30];
(void)sprintf(str, "CAL_MAG%u_ID", i);
int32_t device_id;
int res = param_get(param_find(str), &device_id);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
continue;
}
if ((uint32_t)device_id != m_id.dev_id) {
continue;
}
(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
res = param_get(param_find(str), &_mag_calibration.x_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
res = param_get(param_find(str), &_mag_calibration.y_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
res = param_get(param_find(str), &_mag_calibration.z_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_XOFF", i);
res = param_get(param_find(str), &_mag_calibration.x_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YOFF", i);
res = param_get(param_find(str), &_mag_calibration.y_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
res = param_get(param_find(str), &_mag_calibration.z_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
// We got calibration values, let's exit.
return;
}
// Set sane default calibration values
_mag_calibration.x_scale = 1.0f;
_mag_calibration.y_scale = 1.0f;
_mag_calibration.z_scale = 1.0f;
_mag_calibration.x_offset = 0.0f;
_mag_calibration.y_offset = 0.0f;
_mag_calibration.z_offset = 0.0f;
}
int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
{
/* Check if calibration values are still up-to-date. */
bool updated;
orb_check(_param_update_sub, &updated);
if (updated) {
parameter_update_s parameter_update;
orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
_update_accel_calibration();
_update_gyro_calibration();
}
matrix::Vector3f vec_integrated_unused;
uint32_t integral_dt_unused;
matrix::Vector3f accel_val(data.accel_m_s2_x,
data.accel_m_s2_y,
data.accel_m_s2_z);
// apply sensor rotation on the accel measurement
accel_val = _rotation_matrix * accel_val;
// Apply calibration after rotation
accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale;
accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale;
accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale;
_accel_int.put_with_interval(data.fifo_sample_interval_us,
accel_val,
vec_integrated_unused,
integral_dt_unused);
matrix::Vector3f gyro_val(data.gyro_rad_s_x,
data.gyro_rad_s_y,
data.gyro_rad_s_z);
// apply sensor rotation on the gyro measurement
gyro_val = _rotation_matrix * gyro_val;
// Apply calibration after rotation
gyro_val(0) = gyro_val(0) - _gyro_calibration.x_offset;
gyro_val(1) = gyro_val(1) - _gyro_calibration.y_offset;
gyro_val(2) = gyro_val(2) - _gyro_calibration.z_offset;
_gyro_int.put_with_interval(data.fifo_sample_interval_us,
gyro_val,
vec_integrated_unused,
integral_dt_unused);
// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
// Therefore, only publish every forth time.
++_publish_count;
if (_publish_count < 4) {
return 0;
}
_publish_count = 0;
// Update all the counters.
perf_set_count(_read_counter, data.read_counter);
perf_set_count(_error_counter, data.error_counter);
perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
perf_begin(_publish_perf);
sensor_accel_s accel_report = {};
sensor_gyro_s gyro_report = {};
sensor_mag_s mag_report = {};
accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
mag_report.timestamp = accel_report.timestamp;
mag_report.is_external = false;
// TODO: get these right
gyro_report.scaling = -1.0f;
gyro_report.device_id = m_id.dev_id;
accel_report.scaling = -1.0f;
accel_report.device_id = m_id.dev_id;
if (_mag_enabled) {
mag_report.scaling = -1.0f;
mag_report.device_id = m_id.dev_id;
}
// TODO: remove these (or get the values)
gyro_report.x_raw = 0;
gyro_report.y_raw = 0;
gyro_report.z_raw = 0;
accel_report.x_raw = 0;
accel_report.y_raw = 0;
accel_report.z_raw = 0;
if (_mag_enabled) {
mag_report.x_raw = 0;
mag_report.y_raw = 0;
mag_report.z_raw = 0;
}
matrix::Vector3f gyro_val_filt;
matrix::Vector3f accel_val_filt;
// Read and reset.
matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
// Use the filtered (by integration) values to get smoother / less noisy data.
gyro_report.x = gyro_val_filt(0);
gyro_report.y = gyro_val_filt(1);
gyro_report.z = gyro_val_filt(2);
accel_report.x = accel_val_filt(0);
accel_report.y = accel_val_filt(1);
accel_report.z = accel_val_filt(2);
if (_mag_enabled) {
matrix::Vector3f mag_val(data.mag_ga_x,
data.mag_ga_y,
data.mag_ga_z);
mag_val = _rotation_matrix * mag_val;
mag_val(0) = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
mag_val(1) = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
mag_val(2) = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;
mag_report.x = mag_val(0);
mag_report.y = mag_val(1);
mag_report.z = mag_val(2);
}
gyro_report.x_integral = gyro_val_integ(0);
gyro_report.y_integral = gyro_val_integ(1);
gyro_report.z_integral = gyro_val_integ(2);
accel_report.x_integral = accel_val_integ(0);
accel_report.y_integral = accel_val_integ(1);
accel_report.z_integral = accel_val_integ(2);
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {
if (_gyro_topic != nullptr) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
}
if (_accel_topic != nullptr) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
if (_mag_topic != nullptr) {
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
}
// Report if there are high vibrations, every 10 times it happens.
const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
// Report every 5s.
const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
if (threshold_reached && due_to_report) {
mavlink_log_critical(&_mavlink_log_pub,
"High accelerations, range exceeded %llu times",
data.accel_range_hit_counter);
_last_accel_range_hit_time = hrt_absolute_time();
_last_accel_range_hit_count = data.accel_range_hit_counter;
}
}
perf_end(_publish_perf);
// TODO: check the return codes of this function
return 0;
};
namespace df_lsm9ds1_wrapper
{
DfLsm9ds1Wrapper *g_dev = nullptr;
int start(bool mag_enabled, enum Rotation rotation);
int stop();
int info();
void usage();
int start(bool mag_enabled, enum Rotation rotation)
{
g_dev = new DfLsm9ds1Wrapper(mag_enabled, rotation);
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfLsm9ds1Wrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfLsm9ds1Wrapper start failed");
return ret;
}
// Open the IMU sensor
DevHandle h;
DevMgr::getHandle(IMU_DEVICE_ACC_GYRO, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
IMU_DEVICE_ACC_GYRO, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
DevMgr::getHandle(IMU_DEVICE_MAG, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
IMU_DEVICE_MAG, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
return 0;
}
int stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
int ret = g_dev->stop();
if (ret != 0) {
PX4_ERR("driver could not be stopped");
return ret;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_INFO("state @ %p", g_dev);
g_dev->info();
return 0;
}
void
usage()
{
PX4_INFO("Usage: df_lsm9ds1_wrapper 'start', 'info', 'stop', 'start_without_mag'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
}
} // namespace df_lsm9ds1_wrapper
int
df_lsm9ds1_wrapper_main(int argc, char *argv[])
{
int ch;
enum Rotation rotation = ROTATION_NONE;
int ret = 0;
int myoptind = 1;
const char *myoptarg = NULL;
/* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
break;
default:
df_lsm9ds1_wrapper::usage();
return 0;
}
}
if (argc <= 1) {
df_lsm9ds1_wrapper::usage();
return 1;
}
const char *verb = argv[myoptind];
if (!strcmp(verb, "start_without_mag")) {
ret = df_lsm9ds1_wrapper::start(false, rotation);
}
else if (!strcmp(verb, "start")) {
ret = df_lsm9ds1_wrapper::start(true, rotation);
}
else if (!strcmp(verb, "stop")) {
ret = df_lsm9ds1_wrapper::stop();
}
else if (!strcmp(verb, "info")) {
ret = df_lsm9ds1_wrapper::info();
}
else {
df_lsm9ds1_wrapper::usage();
return 1;
}
return ret;
}

46
src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt
View File

@ -1,46 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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_directories(../../../lib/DriverFramework/drivers)
px4_add_module(
MODULE platforms__posix__drivers__df_ltc2946_wrapper
MAIN df_ltc2946_wrapper
SRCS
df_ltc2946_wrapper.cpp
DEPENDS
git_driverframework
df_driver_framework
df_ltc2946
battery
)

257
src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp
View File

@ -1,257 +0,0 @@
/****************************************************************************
*
* 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 df_ltc2946_wrapper.cpp
* Driver to access the LTC2946 of the DriverFramework.
*
* @author Christoph Tobler <christoph@px4.io>
*/
#include <string>
#include <px4_platform_common/px4_config.h>
#include <systemlib/err.h>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <ltc2946/LTC2946.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
extern "C" { __EXPORT int df_ltc2946_wrapper_main(int argc, char *argv[]); }
using namespace DriverFramework;
class DfLtc2946Wrapper : public LTC2946
{
public:
DfLtc2946Wrapper();
~DfLtc2946Wrapper();
int start();
int stop();
private:
int _publish(const struct ltc2946_sensor_data &data);
Battery _battery{1, nullptr};
int _actuator_ctrl_0_sub{-1};
int _vcontrol_mode_sub{-1};
};
DfLtc2946Wrapper::DfLtc2946Wrapper() :
LTC2946(LTC2946_DEVICE_PATH)
{
_battery.reset();
// subscriptions
_actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
}
DfLtc2946Wrapper::~DfLtc2946Wrapper()
{
orb_unsubscribe(_actuator_ctrl_0_sub);
orb_unsubscribe(_vcontrol_mode_sub);
}
int DfLtc2946Wrapper::start()
{
int ret;
/* Init device and start sensor. */
ret = init();
//
if (ret != 0) {
PX4_ERR("LTC2946 init fail: %d", ret);
return ret;
}
ret = LTC2946::start();
if (ret != 0) {
PX4_ERR("LTC2946 start fail: %d", ret);
return ret;
}
return 0;
}
int DfLtc2946Wrapper::stop()
{
/* Stop sensor. */
int ret = LTC2946::stop();
if (ret != 0) {
PX4_ERR("LTC2946 stop fail: %d", ret);
return ret;
}
return 0;
}
int DfLtc2946Wrapper::_publish(const struct ltc2946_sensor_data &data)
{
hrt_abstime t = hrt_absolute_time();
bool connected = data.battery_voltage_V > BOARD_ADC_OPEN_CIRCUIT_V;
actuator_controls_s ctrl;
orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
vehicle_control_mode_s vcontrol_mode;
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
_battery.updateBatteryStatus(t, data.battery_voltage_V, data.battery_current_A,
connected, true, 1,
ctrl.control[actuator_controls_s::INDEX_THROTTLE],
true);
return 0;
}
namespace df_ltc2946_wrapper
{
DfLtc2946Wrapper *g_dev = nullptr;
int start();
int stop();
int info();
void usage();
int start()
{
PX4_WARN("starting LTC2946");
g_dev = new DfLtc2946Wrapper();
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfLtc2946Wrapper object");
return -1;
} else {
PX4_INFO("started LTC2946");
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfLtc2946Wrapper start failed");
return ret;
}
return 0;
}
int stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
int ret = g_dev->stop();
if (ret != 0) {
PX4_ERR("driver could not be stopped");
return ret;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_DEBUG("state @ %p", g_dev);
return 0;
}
void
usage()
{
PX4_WARN("Usage: df_ltc2946_wrapper 'start', 'info', 'stop'");
}
} // namespace df_ltc2946_wrapper
int
df_ltc2946_wrapper_main(int argc, char *argv[])
{
int ret = 0;
if (argc <= 1) {
df_ltc2946_wrapper::usage();
return 1;
}
const char *verb = argv[1];
if (!strcmp(verb, "start")) {
ret = df_ltc2946_wrapper::start();
}
else if (!strcmp(verb, "stop")) {
ret = df_ltc2946_wrapper::stop();
}
else if (!strcmp(verb, "info")) {
ret = df_ltc2946_wrapper::info();
}
else {
df_ltc2946_wrapper::usage();
return 1;
}
return ret;
}

45
src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/CMakeLists.txt
View File

@ -1,45 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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_directories(../../../lib/DriverFramework/drivers)
px4_add_module(
MODULE platforms__posix__drivers__df_mpu6050_wrapper
MAIN df_mpu6050_wrapper
SRCS
df_mpu6050_wrapper.cpp
DEPENDS
git_driverframework
df_driver_framework
df_mpu6050
)

687
src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp
View File

@ -1,687 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 df_mpu6050_wrapper.cpp
* Lightweight driver to access the MPU6050 of the DriverFramework.
*/
#include <px4_platform_common/px4_config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_platform_common/getopt.h>
#include <errno.h>
#include <systemlib/err.h>
#include <lib/parameters/param.h>
#include <perf/perf_counter.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/device/integrator.h>
#include <lib/conversion/rotation.h>
#include <uORB/topics/parameter_update.h>
#include <mpu6050/MPU6050.hpp>
#include <DevMgr.hpp>
// We don't want to auto publish, therefore set this to 0.
#define MPU6050_NEVER_AUTOPUBLISH_US 0
extern "C" { __EXPORT int df_mpu6050_wrapper_main(int argc, char *argv[]); }
using namespace DriverFramework;
class DfMPU6050Wrapper : public MPU6050
{
public:
DfMPU6050Wrapper(enum Rotation rotation);
~DfMPU6050Wrapper();
/**
* Start automatic measurement.
*
* @return 0 on success
*/
int start();
/**
* Stop automatic measurement.
*
* @return 0 on success
*/
int stop();
/**
* Print some debug info.
*/
void info();
private:
int _publish(struct imu_sensor_data &data);
void _update_accel_calibration();
void _update_gyro_calibration();
orb_advert_t _accel_topic;
orb_advert_t _gyro_topic;
orb_advert_t _mavlink_log_pub;
int _param_update_sub;
struct accel_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
} _accel_calibration;
struct gyro_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
} _gyro_calibration;
matrix::Dcmf _rotation_matrix;
int _accel_orb_class_instance;
int _gyro_orb_class_instance;
Integrator _accel_int;
Integrator _gyro_int;
unsigned _publish_count;
perf_counter_t _read_counter;
perf_counter_t _error_counter;
perf_counter_t _fifo_overflow_counter;
perf_counter_t _fifo_corruption_counter;
perf_counter_t _gyro_range_hit_counter;
perf_counter_t _accel_range_hit_counter;
perf_counter_t _publish_perf;
hrt_abstime _last_accel_range_hit_time;
uint64_t _last_accel_range_hit_count;
};
DfMPU6050Wrapper::DfMPU6050Wrapper(enum Rotation rotation) :
MPU6050(IMU_DEVICE_PATH),
_accel_topic(nullptr),
_gyro_topic(nullptr),
_mavlink_log_pub(nullptr),
_param_update_sub(-1),
_accel_calibration{},
_gyro_calibration{},
_accel_orb_class_instance(-1),
_gyro_orb_class_instance(-1),
_accel_int(MPU6050_NEVER_AUTOPUBLISH_US, false),
_gyro_int(MPU6050_NEVER_AUTOPUBLISH_US, true),
_publish_count(0),
_read_counter(perf_alloc(PC_COUNT, "mpu6050_reads")),
_error_counter(perf_alloc(PC_COUNT, "mpu6050_errors")),
_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_overflows")),
_fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_corruptions")),
_gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_gyro_range_hits")),
_accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_accel_range_hits")),
_publish_perf(perf_alloc(PC_ELAPSED, "mpu6050_publish")),
_last_accel_range_hit_time(0),
_last_accel_range_hit_count(0)
{
// Set sane default calibration values
_accel_calibration.x_scale = 1.0f;
_accel_calibration.y_scale = 1.0f;
_accel_calibration.z_scale = 1.0f;
_accel_calibration.x_offset = 0.0f;
_accel_calibration.y_offset = 0.0f;
_accel_calibration.z_offset = 0.0f;
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
// Get sensor rotation matrix
_rotation_matrix = get_rot_matrix(rotation);
}
DfMPU6050Wrapper::~DfMPU6050Wrapper()
{
perf_free(_read_counter);
perf_free(_error_counter);
perf_free(_fifo_overflow_counter);
perf_free(_fifo_corruption_counter);
perf_free(_gyro_range_hit_counter);
perf_free(_accel_range_hit_counter);
perf_free(_publish_perf);
}
int DfMPU6050Wrapper::start()
{
/* Subscribe to param update topic. */
if (_param_update_sub < 0) {
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
}
/* Init device and start sensor. */
int ret = init();
if (ret != 0) {
PX4_ERR("MPU6050 init fail: %d", ret);
return ret;
}
ret = MPU6050::start();
if (ret != 0) {
PX4_ERR("MPU6050 start fail: %d", ret);
return ret;
}
PX4_DEBUG("MPU6050 device id is: %d", m_id.dev_id);
/* Force getting the calibration values. */
_update_accel_calibration();
_update_gyro_calibration();
return 0;
}
int DfMPU6050Wrapper::stop()
{
/* Stop sensor. */
int ret = MPU6050::stop();
if (ret != 0) {
PX4_ERR("MPU6050 stop fail: %d", ret);
return ret;
}
return 0;
}
void DfMPU6050Wrapper::info()
{
perf_print_counter(_read_counter);
perf_print_counter(_error_counter);
perf_print_counter(_fifo_overflow_counter);
perf_print_counter(_fifo_corruption_counter);
perf_print_counter(_gyro_range_hit_counter);
perf_print_counter(_accel_range_hit_counter);
perf_print_counter(_publish_perf);
}
void DfMPU6050Wrapper::_update_gyro_calibration()
{
// TODO: replace magic number
for (unsigned i = 0; i < 3; ++i) {
// TODO: remove printfs and add error counter
char str[30];
(void)sprintf(str, "CAL_GYRO%u_ID", i);
int32_t device_id;
int res = param_get(param_find(str), &device_id);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
continue;
}
if ((uint32_t)device_id != m_id.dev_id) {
continue;
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
res = param_get(param_find(str), &_gyro_calibration.x_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
res = param_get(param_find(str), &_gyro_calibration.y_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
res = param_get(param_find(str), &_gyro_calibration.z_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
// We got calibration values, let's exit.
return;
}
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
}
void DfMPU6050Wrapper::_update_accel_calibration()
{
// TODO: replace magic number
for (unsigned i = 0; i < 3; ++i) {
// TODO: remove printfs and add error counter
char str[30];
(void)sprintf(str, "CAL_ACC%u_ID", i);
int32_t device_id;
int res = param_get(param_find(str), &device_id);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
continue;
}
if ((uint32_t)device_id != m_id.dev_id) {
continue;
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
res = param_get(param_find(str), &_accel_calibration.x_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
res = param_get(param_find(str), &_accel_calibration.y_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
res = param_get(param_find(str), &_accel_calibration.z_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
res = param_get(param_find(str), &_accel_calibration.x_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
res = param_get(param_find(str), &_accel_calibration.y_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
res = param_get(param_find(str), &_accel_calibration.z_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
// We got calibration values, let's exit.
return;
}
// Set sane default calibration values
_accel_calibration.x_scale = 1.0f;
_accel_calibration.y_scale = 1.0f;
_accel_calibration.z_scale = 1.0f;
_accel_calibration.x_offset = 0.0f;
_accel_calibration.y_offset = 0.0f;
_accel_calibration.z_offset = 0.0f;
}
int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data)
{
/* Check if calibration values are still up-to-date. */
bool updated;
orb_check(_param_update_sub, &updated);
if (updated) {
parameter_update_s parameter_update;
orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
_update_accel_calibration();
_update_gyro_calibration();
}
uint64_t now = hrt_absolute_time();
matrix::Vector3f vec_integrated_unused;
uint32_t integral_dt_unused;
matrix::Vector3f accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z);
// apply sensor rotation on the accel measurement
accel_val = _rotation_matrix * accel_val;
accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale;
accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale;
accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale;
_accel_int.put(now,
accel_val,
vec_integrated_unused,
integral_dt_unused);
matrix::Vector3f gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z);
// apply sensor rotation on the gyro measurement
gyro_val = _rotation_matrix * gyro_val;
gyro_val(0) = gyro_val(0) - _gyro_calibration.x_offset;
gyro_val(1) = gyro_val(1) - _gyro_calibration.y_offset;
gyro_val(2) = gyro_val(2) - _gyro_calibration.z_offset;
_gyro_int.put(now,
gyro_val,
vec_integrated_unused,
integral_dt_unused);
// If we are not receiving the last sample from the FIFO buffer yet, let's stop here
// and wait for more packets.
if (!data.is_last_fifo_sample) {
return 0;
}
// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
// Therefore, only publish every forth time.
++_publish_count;
if (_publish_count < 4) {
return 0;
}
_publish_count = 0;
// Update all the counters.
perf_set_count(_read_counter, data.read_counter);
perf_set_count(_error_counter, data.error_counter);
perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
perf_begin(_publish_perf);
sensor_accel_s accel_report = {};
sensor_gyro_s gyro_report = {};
accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
// TODO: get these right
gyro_report.scaling = -1.0f;
gyro_report.device_id = m_id.dev_id;
accel_report.scaling = -1.0f;
accel_report.device_id = m_id.dev_id;
// TODO: remove these (or get the values)
gyro_report.x_raw = 0;
gyro_report.y_raw = 0;
gyro_report.z_raw = 0;
accel_report.x_raw = 0;
accel_report.y_raw = 0;
accel_report.z_raw = 0;
matrix::Vector3f gyro_val_filt;
matrix::Vector3f accel_val_filt;
// Read and reset.
matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
// Use the filtered (by integration) values to get smoother / less noisy data.
gyro_report.x = gyro_val_filt(0);
gyro_report.y = gyro_val_filt(1);
gyro_report.z = gyro_val_filt(2);
accel_report.x = accel_val_filt(0);
accel_report.y = accel_val_filt(1);
accel_report.z = accel_val_filt(2);
gyro_report.x_integral = gyro_val_integ(0);
gyro_report.y_integral = gyro_val_integ(1);
gyro_report.z_integral = gyro_val_integ(2);
accel_report.x_integral = accel_val_integ(0);
accel_report.y_integral = accel_val_integ(1);
accel_report.z_integral = accel_val_integ(2);
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {
if (_gyro_topic == nullptr) {
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
&_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
}
if (_accel_topic == nullptr) {
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
&_accel_orb_class_instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
// Report if there are high vibrations, every 10 times it happens.
const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
// Report every 5s.
const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
if (threshold_reached && due_to_report) {
mavlink_log_critical(&_mavlink_log_pub,
"High accelerations, range exceeded %llu times",
data.accel_range_hit_counter);
_last_accel_range_hit_time = hrt_absolute_time();
_last_accel_range_hit_count = data.accel_range_hit_counter;
}
}
perf_end(_publish_perf);
// TODO: check the return codes of this function
return 0;
};
namespace df_mpu6050_wrapper
{
DfMPU6050Wrapper *g_dev = nullptr;
int start(enum Rotation rotation);
int stop();
int info();
void usage();
int start(enum Rotation rotation)
{
g_dev = new DfMPU6050Wrapper(rotation);
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfMPU6050Wrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfMPU6050Wrapper start failed");
return ret;
}
// Open the IMU sensor
DevHandle h;
DevMgr::getHandle(IMU_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
IMU_DEVICE_PATH, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
return 0;
}
int stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
int ret = g_dev->stop();
if (ret != 0) {
PX4_ERR("driver could not be stopped");
return ret;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_INFO("state @ %p", g_dev);
g_dev->info();
return 0;
}
void
usage()
{
PX4_INFO("Usage: df_mpu6050_wrapper 'start', 'info', 'stop'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
}
} // namespace df_mpu6050_wrapper
int
df_mpu6050_wrapper_main(int argc, char *argv[])
{
int ch;
enum Rotation rotation = ROTATION_NONE;
int ret = 0;
int myoptind = 1;
const char *myoptarg = NULL;
/* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
break;
default:
df_mpu6050_wrapper::usage();
return 0;
}
}
if (argc <= 1) {
df_mpu6050_wrapper::usage();
return 1;
}
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ret = df_mpu6050_wrapper::start(rotation);
}
else if (!strcmp(verb, "stop")) {
ret = df_mpu6050_wrapper::stop();
}
else if (!strcmp(verb, "info")) {
ret = df_mpu6050_wrapper::info();
}
else {
df_mpu6050_wrapper::usage();
return 1;
}
return ret;
}

45
src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/CMakeLists.txt
View File

@ -1,45 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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_directories(../../../lib/DriverFramework/drivers)
px4_add_module(
MODULE platforms__posix__drivers__df_mpu9250_wrapper
MAIN df_mpu9250_wrapper
SRCS
df_mpu9250_wrapper.cpp
DEPENDS
git_driverframework
df_driver_framework
df_mpu9250
)

897
src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
View File

@ -1,897 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 df_mpu9250_wrapper.cpp
* Lightweight driver to access the MPU9250 of the DriverFramework.
*
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_platform_common/px4_config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_platform_common/getopt.h>
#include <errno.h>
#include <lib/parameters/param.h>
#include <systemlib/err.h>
#include <perf/perf_counter.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/device/integrator.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#include <uORB/topics/parameter_update.h>
#include <mpu9250/MPU9250.hpp>
#include <DevMgr.hpp>
#define MPU9250_ACCEL_DEFAULT_RATE 1000
#define MPU9250_GYRO_DEFAULT_RATE 1000
#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_PUB_RATE 280
extern "C" { __EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[]); }
using namespace DriverFramework;
class DfMpu9250Wrapper : public MPU9250
{
public:
DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation);
~DfMpu9250Wrapper();
/**
* Start automatic measurement.
*
* @return 0 on success
*/
int start();
/**
* Stop automatic measurement.
*
* @return 0 on success
*/
int stop();
/**
* Print some debug info.
*/
void info();
private:
int _publish(struct imu_sensor_data &data);
void _update_accel_calibration();
void _update_gyro_calibration();
void _update_mag_calibration();
orb_advert_t _accel_topic;
orb_advert_t _gyro_topic;
orb_advert_t _mag_topic;
orb_advert_t _mavlink_log_pub;
int _param_update_sub;
struct accel_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
} _accel_calibration;
struct gyro_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
} _gyro_calibration;
struct mag_calibration_s {
float x_offset;
float x_scale;
float y_offset;
float y_scale;
float z_offset;
float z_scale;
} _mag_calibration;
int _accel_orb_class_instance;
int _gyro_orb_class_instance;
int _mag_orb_class_instance;
Integrator _accel_int;
Integrator _gyro_int;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
perf_counter_t _read_counter;
perf_counter_t _error_counter;
perf_counter_t _fifo_overflow_counter;
perf_counter_t _fifo_corruption_counter;
perf_counter_t _gyro_range_hit_counter;
perf_counter_t _accel_range_hit_counter;
perf_counter_t _mag_fifo_overflow_counter;
perf_counter_t _publish_perf;
hrt_abstime _last_accel_range_hit_time;
uint64_t _last_accel_range_hit_count;
bool _mag_enabled;
enum Rotation _rotation;
};
DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation) :
MPU9250(IMU_DEVICE_PATH, mag_enabled),
_accel_topic(nullptr),
_gyro_topic(nullptr),
_mag_topic(nullptr),
_mavlink_log_pub(nullptr),
_param_update_sub(-1),
_accel_calibration{},
_gyro_calibration{},
_mag_calibration{},
_accel_orb_class_instance(-1),
_gyro_orb_class_instance(-1),
_mag_orb_class_instance(-1),
_accel_int(1000000 / MPU9250_PUB_RATE, false),
_gyro_int(1000000 / MPU9250_PUB_RATE, true),
_accel_filter_x(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")),
_error_counter(perf_alloc(PC_COUNT, "mpu9250_errors")),
_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_overflows")),
_fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_corruptions")),
_gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_gyro_range_hits")),
_accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_accel_range_hits")),
_mag_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_mag_fifo_overflows")),
_publish_perf(perf_alloc(PC_ELAPSED, "mpu9250_publish")),
_last_accel_range_hit_time(0),
_last_accel_range_hit_count(0),
_mag_enabled(mag_enabled),
_rotation(rotation)
{
// Set sane default calibration values
_accel_calibration.x_scale = 1.0f;
_accel_calibration.y_scale = 1.0f;
_accel_calibration.z_scale = 1.0f;
_accel_calibration.x_offset = 0.0f;
_accel_calibration.y_offset = 0.0f;
_accel_calibration.z_offset = 0.0f;
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
if (_mag_enabled) {
_mag_calibration.x_scale = 1.0f;
_mag_calibration.y_scale = 1.0f;
_mag_calibration.z_scale = 1.0f;
_mag_calibration.x_offset = 0.0f;
_mag_calibration.y_offset = 0.0f;
_mag_calibration.z_offset = 0.0f;
}
// set software low pass filter for controllers
param_t param_handle = param_find("IMU_ACCEL_CUTOFF");
float param_val = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
if (param_handle != PARAM_INVALID && (param_get(param_handle, &param_val) == PX4_OK)) {
_accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val);
_accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val);
_accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val);
} else {
PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
}
param_handle = param_find("IMU_GYRO_CUTOFF");
param_val = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
if (param_handle != PARAM_INVALID && (param_get(param_handle, &param_val) == PX4_OK)) {
_gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val);
_gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val);
_gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val);
} else {
PX4_ERR("IMU_GYRO_CUTOFF param invalid");
}
}
DfMpu9250Wrapper::~DfMpu9250Wrapper()
{
perf_free(_read_counter);
perf_free(_error_counter);
perf_free(_fifo_overflow_counter);
perf_free(_fifo_corruption_counter);
perf_free(_gyro_range_hit_counter);
perf_free(_accel_range_hit_counter);
if (_mag_enabled) {
perf_free(_mag_fifo_overflow_counter);
}
perf_free(_publish_perf);
}
int DfMpu9250Wrapper::start()
{
// TODO: don't publish garbage here
sensor_accel_s accel_report = {};
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
&_accel_orb_class_instance, ORB_PRIO_DEFAULT);
if (_accel_topic == nullptr) {
PX4_ERR("sensor_accel advert fail");
return -1;
}
// TODO: don't publish garbage here
sensor_gyro_s gyro_report = {};
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
&_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
if (_gyro_topic == nullptr) {
PX4_ERR("sensor_gyro advert fail");
return -1;
}
if (_mag_enabled) {
}
/* Subscribe to param update topic. */
if (_param_update_sub < 0) {
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
}
/* Init device and start sensor. */
int ret = init();
if (ret != 0) {
PX4_ERR("MPU9250 init fail: %d", ret);
return ret;
}
ret = MPU9250::start();
if (ret != 0) {
PX4_ERR("MPU9250 start fail: %d", ret);
return ret;
}
PX4_DEBUG("MPU9250 device id is: %d", m_id.dev_id);
/* Force getting the calibration values. */
_update_accel_calibration();
_update_gyro_calibration();
_update_mag_calibration();
return 0;
}
int DfMpu9250Wrapper::stop()
{
/* Stop sensor. */
int ret = MPU9250::stop();
if (ret != 0) {
PX4_ERR("MPU9250 stop fail: %d", ret);
return ret;
}
return 0;
}
void DfMpu9250Wrapper::info()
{
perf_print_counter(_read_counter);
perf_print_counter(_error_counter);
perf_print_counter(_fifo_overflow_counter);
perf_print_counter(_fifo_corruption_counter);
perf_print_counter(_gyro_range_hit_counter);
perf_print_counter(_accel_range_hit_counter);
if (_mag_enabled) {
perf_print_counter(_mag_fifo_overflow_counter);
}
perf_print_counter(_publish_perf);
}
void DfMpu9250Wrapper::_update_gyro_calibration()
{
// TODO: replace magic number
for (unsigned i = 0; i < 3; ++i) {
// TODO: remove printfs and add error counter
char str[30];
(void)sprintf(str, "CAL_GYRO%u_ID", i);
int32_t device_id;
int res = param_get(param_find(str), &device_id);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
continue;
}
if ((uint32_t)device_id != m_id.dev_id) {
continue;
}
(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
res = param_get(param_find(str), &_gyro_calibration.x_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
res = param_get(param_find(str), &_gyro_calibration.y_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
res = param_get(param_find(str), &_gyro_calibration.z_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
// We got calibration values, let's exit.
return;
}
_gyro_calibration.x_offset = 0.0f;
_gyro_calibration.y_offset = 0.0f;
_gyro_calibration.z_offset = 0.0f;
}
void DfMpu9250Wrapper::_update_accel_calibration()
{
// TODO: replace magic number
for (unsigned i = 0; i < 3; ++i) {
// TODO: remove printfs and add error counter
char str[30];
(void)sprintf(str, "CAL_ACC%u_ID", i);
int32_t device_id;
int res = param_get(param_find(str), &device_id);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
continue;
}
if ((uint32_t)device_id != m_id.dev_id) {
continue;
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
res = param_get(param_find(str), &_accel_calibration.x_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
res = param_get(param_find(str), &_accel_calibration.y_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
res = param_get(param_find(str), &_accel_calibration.z_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
res = param_get(param_find(str), &_accel_calibration.x_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", i);
res = param_get(param_find(str), &_accel_calibration.y_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
res = param_get(param_find(str), &_accel_calibration.z_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
// We got calibration values, let's exit.
return;
}
// Set sane default calibration values
_accel_calibration.x_scale = 1.0f;
_accel_calibration.y_scale = 1.0f;
_accel_calibration.z_scale = 1.0f;
_accel_calibration.x_offset = 0.0f;
_accel_calibration.y_offset = 0.0f;
_accel_calibration.z_offset = 0.0f;
}
void DfMpu9250Wrapper::_update_mag_calibration()
{
if (!_mag_enabled) {
return;
}
// TODO: replace magic number
for (unsigned i = 0; i < 3; ++i) {
// TODO: remove printfs and add error counter
char str[30];
(void)sprintf(str, "CAL_MAG%u_ID", i);
int32_t device_id;
int res = param_get(param_find(str), &device_id);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
continue;
}
if ((uint32_t)device_id != m_id.dev_id) {
continue;
}
(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
res = param_get(param_find(str), &_mag_calibration.x_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
res = param_get(param_find(str), &_mag_calibration.y_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
res = param_get(param_find(str), &_mag_calibration.z_scale);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_XOFF", i);
res = param_get(param_find(str), &_mag_calibration.x_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YOFF", i);
res = param_get(param_find(str), &_mag_calibration.y_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
res = param_get(param_find(str), &_mag_calibration.z_offset);
if (res != OK) {
PX4_ERR("Could not access param %s", str);
}
// We got calibration values, let's exit.
return;
}
// Set sane default calibration values
_mag_calibration.x_scale = 1.0f;
_mag_calibration.y_scale = 1.0f;
_mag_calibration.z_scale = 1.0f;
_mag_calibration.x_offset = 0.0f;
_mag_calibration.y_offset = 0.0f;
_mag_calibration.z_offset = 0.0f;
}
int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
{
/* Check if calibration values are still up-to-date. */
bool updated;
orb_check(_param_update_sub, &updated);
if (updated) {
parameter_update_s parameter_update;
orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
_update_accel_calibration();
_update_gyro_calibration();
_update_mag_calibration();
}
sensor_accel_s accel_report = {};
sensor_gyro_s gyro_report = {};
sensor_mag_s mag_report = {};
accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
accel_report.error_count = gyro_report.error_count = mag_report.error_count = data.error_counter;
// ACCEL
float xraw_f = data.accel_m_s2_x;
float yraw_f = data.accel_m_s2_y;
float zraw_f = data.accel_m_s2_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
// MPU9250 driver from DriverFramework does not provide any raw values
// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
accel_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [m / s^2 * 1000];
accel_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [m / s^2 * 1000];
accel_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [m / s^2 * 1000];
// adjust values according to the calibration
float x_in_new = (xraw_f - _accel_calibration.x_offset) * _accel_calibration.x_scale;
float y_in_new = (yraw_f - _accel_calibration.y_offset) * _accel_calibration.y_scale;
float z_in_new = (zraw_f - _accel_calibration.z_offset) * _accel_calibration.z_scale;
accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
matrix::Vector3f aval_integrated;
_accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
accel_report.x_integral = aval_integrated(0);
accel_report.y_integral = aval_integrated(1);
accel_report.z_integral = aval_integrated(2);
// GYRO
xraw_f = data.gyro_rad_s_x;
yraw_f = data.gyro_rad_s_y;
zraw_f = data.gyro_rad_s_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
// MPU9250 driver from DriverFramework does not provide any raw values
// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
gyro_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [rad / s * 1000];
gyro_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [rad / s * 1000];
gyro_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [rad / s * 1000];
// adjust values according to the calibration
float x_gyro_in_new = xraw_f - _gyro_calibration.x_offset;
float y_gyro_in_new = yraw_f - _gyro_calibration.y_offset;
float z_gyro_in_new = zraw_f - _gyro_calibration.z_offset;
gyro_report.x = _gyro_filter_x.apply(x_gyro_in_new);
gyro_report.y = _gyro_filter_y.apply(y_gyro_in_new);
gyro_report.z = _gyro_filter_z.apply(z_gyro_in_new);
matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
matrix::Vector3f gval_integrated;
bool sensor_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
gyro_report.x_integral = gval_integrated(0);
gyro_report.y_integral = gval_integrated(1);
gyro_report.z_integral = gval_integrated(2);
// if gyro integrator did not return a sample we can return here
// Note: the accel integrator receives the same timestamp as the gyro integrator
// so we do not need to handle it seperately
if (!sensor_notify) {
return 0;
}
// Update all the counters.
perf_set_count(_read_counter, data.read_counter);
perf_set_count(_error_counter, data.error_counter);
perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
if (_mag_enabled) {
perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter);
}
perf_begin(_publish_perf);
// TODO: get these right
gyro_report.scaling = -1.0f;
gyro_report.device_id = m_id.dev_id;
accel_report.scaling = -1.0f;
accel_report.device_id = m_id.dev_id;
if (_mag_enabled) {
mag_report.timestamp = accel_report.timestamp;
mag_report.is_external = false;
mag_report.scaling = -1.0f;
mag_report.device_id = m_id.dev_id;
xraw_f = data.mag_ga_x;
yraw_f = data.mag_ga_y;
zraw_f = data.mag_ga_z;
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
// MPU9250 driver from DriverFramework does not provide any raw values
// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
mag_report.x_raw = xraw_f * 1000; // (int16) [Gs * 1000]
mag_report.y_raw = yraw_f * 1000; // (int16) [Gs * 1000]
mag_report.z_raw = zraw_f * 1000; // (int16) [Gs * 1000]
mag_report.x = (xraw_f - _mag_calibration.x_offset) * _mag_calibration.x_scale;
mag_report.y = (yraw_f - _mag_calibration.y_offset) * _mag_calibration.y_scale;
mag_report.z = (zraw_f - _mag_calibration.z_offset) * _mag_calibration.z_scale;
}
// TODO: when is this ever blocked?
if (!(m_pub_blocked) && sensor_notify) {
if (_gyro_topic != nullptr) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
}
if (_accel_topic != nullptr) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
if (_mag_enabled) {
if (_mag_topic == nullptr) {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
&_mag_orb_class_instance, ORB_PRIO_LOW);
} else {
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
}
}
// Report if there are high vibrations, every 10 times it happens.
const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
// Report every 5s.
const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
if (threshold_reached && due_to_report) {
mavlink_log_critical(&_mavlink_log_pub,
"High accelerations, range exceeded %llu times",
data.accel_range_hit_counter);
_last_accel_range_hit_time = hrt_absolute_time();
_last_accel_range_hit_count = data.accel_range_hit_counter;
}
}
perf_end(_publish_perf);
// TODO: check the return codes of this function
return 0;
};
namespace df_mpu9250_wrapper
{
DfMpu9250Wrapper *g_dev = nullptr;
int start(bool mag_enabled, enum Rotation rotation);
int stop();
int info();
void usage();
int start(bool mag_enabled, enum Rotation rotation)
{
g_dev = new DfMpu9250Wrapper(mag_enabled, rotation);
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfMpu9250Wrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfMpu9250Wrapper start failed");
return ret;
}
// Open the IMU sensor
DevHandle h;
DevMgr::getHandle(IMU_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
IMU_DEVICE_PATH, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
return 0;
}
int stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
int ret = g_dev->stop();
if (ret != 0) {
PX4_ERR("driver could not be stopped");
return ret;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_INFO("state @ %p", g_dev);
g_dev->info();
return 0;
}
void
usage()
{
PX4_INFO("Usage: df_mpu9250_wrapper 'start', 'start_without_mag', 'info', 'stop'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
}
} // namespace df_mpu9250_wrapper
int
df_mpu9250_wrapper_main(int argc, char *argv[])
{
int ch;
enum Rotation rotation = ROTATION_NONE;
int ret = 0;
int myoptind = 1;
const char *myoptarg = NULL;
/* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
break;
default:
df_mpu9250_wrapper::usage();
return 0;
}
}
if (argc <= 1) {
df_mpu9250_wrapper::usage();
return 1;
}
const char *verb = argv[myoptind];
if (!strcmp(verb, "start_without_mag")) {
ret = df_mpu9250_wrapper::start(false, rotation);
}
else if (!strcmp(verb, "start")) {
ret = df_mpu9250_wrapper::start(true, rotation);
}
else if (!strcmp(verb, "stop")) {
ret = df_mpu9250_wrapper::stop();
}
else if (!strcmp(verb, "info")) {
ret = df_mpu9250_wrapper::info();
}
else {
df_mpu9250_wrapper::usage();
return 1;
}
return ret;
}

46
src/drivers/driver_framework_wrapper/df_trone_wrapper/CMakeLists.txt
View File

@ -1,46 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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_directories(../../../lib/DriverFramework/drivers)
px4_add_module(
MODULE platforms__posix__drivers__df_trone_wrapper
MAIN df_trone_wrapper
SRCS
df_trone_wrapper.cpp
DEPENDS
git_driverframework
df_driver_framework
df_trone
)

362
src/drivers/driver_framework_wrapper/df_trone_wrapper/df_trone_wrapper.cpp
View File

@ -1,362 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 df_trone_wrapper.cpp
* Driver to access the TROne of the DriverFramework.
*
* @author Nicolas de Palezieux <ndepal@gmail.com>
*/
#include <px4_platform_common/px4_config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_platform_common/getopt.h>
#include <errno.h>
#include <string>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_range_finder.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
#include <trone/TROne.hpp>
#include <DevMgr.hpp>
extern "C" { __EXPORT int df_trone_wrapper_main(int argc, char *argv[]); }
using namespace DriverFramework;
class DfTROneWrapper : public TROne
{
public:
DfTROneWrapper(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
~DfTROneWrapper();
/**
* Start automatic measurement.
*
* @return 0 on success
*/
int start();
/**
* Stop automatic measurement.
*
* @return 0 on success
*/
int stop();
private:
int _publish(struct range_sensor_data &data);
uint8_t _rotation;
orb_advert_t _range_topic;
int _orb_class_instance;
// perf_counter_t _range_sample_perf;
};
DfTROneWrapper::DfTROneWrapper(uint8_t rotation) :
TROne(TRONE_DEVICE_PATH),
_rotation(rotation),
_range_topic(nullptr),
_orb_class_instance(-1)
{
}
DfTROneWrapper::~DfTROneWrapper()
{
}
int DfTROneWrapper::start()
{
int ret;
/* Init device and start sensor. */
ret = init();
if (ret != 0) {
PX4_ERR("TROne init fail: %d", ret);
return ret;
}
ret = TROne::start();
if (ret != 0) {
PX4_ERR("TROne start fail: %d", ret);
return ret;
}
return 0;
}
int DfTROneWrapper::stop()
{
/* Stop sensor. */
int ret = TROne::stop();
if (ret != 0) {
PX4_ERR("TROne stop fail: %d", ret);
return ret;
}
return 0;
}
int DfTROneWrapper::_publish(struct range_sensor_data &data)
{
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
d.timestamp = hrt_absolute_time();
d.min_distance = float(TRONE_MIN_DISTANCE); /* m */
d.max_distance = float(TRONE_MAX_DISTANCE); /* m */
d.current_distance = float(data.dist);
d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
d.id = 0; // TODO set proper ID
d.orientation = _rotation;
d.variance = 0.0f;
d.signal_quality = -1;
if (_range_topic == nullptr) {
_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
&_orb_class_instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
}
return 0;
};
namespace df_trone_wrapper
{
DfTROneWrapper *g_dev = nullptr;
int start(uint8_t rotation);
int stop();
int info();
int probe();
void usage();
int start(uint8_t rotation)
{
PX4_ERR("start");
g_dev = new DfTROneWrapper(rotation);
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfTROneWrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfTROneWrapper start failed");
return ret;
}
// Open the range sensor
DevHandle h;
DevMgr::getHandle(TRONE_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
TRONE_DEVICE_PATH, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
return 0;
}
int stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
int ret = g_dev->stop();
if (ret != 0) {
PX4_ERR("driver could not be stopped");
return ret;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_DEBUG("state @ %p", g_dev);
return 0;
}
/**
* Who am i
*/
int
probe()
{
int ret;
if (g_dev == nullptr) {
ret = start(distance_sensor_s::ROTATION_DOWNWARD_FACING);
if (ret) {
PX4_ERR("Failed to start");
return ret;
}
}
ret = g_dev->probe();
if (ret) {
PX4_ERR("Failed to probe");
return ret;
}
PX4_DEBUG("state @ %p", g_dev);
return 0;
}
void
usage()
{
PX4_WARN("Usage: df_trone_wrapper 'start', 'info', 'stop'");
}
} // namespace df_trone_wrapper
int
df_trone_wrapper_main(int argc, char *argv[])
{
int ch;
int ret = 0;
int myoptind = 1;
const char *myoptarg = NULL;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
/* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
break;
default:
df_trone_wrapper::usage();
return 0;
}
}
if (argc <= 1) {
df_trone_wrapper::usage();
return 1;
}
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ret = df_trone_wrapper::start(rotation);
}
else if (!strcmp(verb, "stop")) {
ret = df_trone_wrapper::stop();
}
else if (!strcmp(verb, "info")) {
ret = df_trone_wrapper::info();
}
else if (!strcmp(verb, "probe")) {
ret = df_trone_wrapper::probe();
}
else {
df_trone_wrapper::usage();
return 1;
}
return ret;
}

16
src/drivers/drv_device.h
View File

@ -46,15 +46,13 @@
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#include "DevIOCTL.h"
#define _DEVICEIOCBASE (0x100)
#define _DEVICEIOC(_n) (_PX4_IOC(_DEVICEIOCBASE, _n))
#ifdef __PX4_POSIX
#ifndef SIOCDEVPRIVATE
#define SIOCDEVPRIVATE 1
#endif
#define DIOC_GETPRIV SIOCDEVPRIVATE
#endif
/**
* Return device ID, to enable matching of configuration parameters
* (such as compass offsets) to specific sensors
*/
#define DEVIOCGDEVICEID _DEVICEIOC(2)
#endif /* _DRV_DEVICE_H */

4
src/drivers/linux_pwm_out/bbblue_pwm_rc.cpp
View File

@ -30,7 +30,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifdef __PX4_POSIX_BBBLUE
#ifdef CONFIG_ARCH_BOARD_BEAGLEBONE_BLUE
#include <fcntl.h>
#include <errno.h>
@ -79,4 +79,4 @@ int BBBlueRcPWMOut::send_output_pwm(const uint16_t *pwm, int num_outputs)
return ret;
}
#endif // __PX4_POSIX_BBBLUE
#endif // CONFIG_ARCH_BOARD_BEAGLEBONE_BLUE

2
src/drivers/linux_pwm_out/linux_pwm_out.cpp
View File

@ -235,7 +235,7 @@ void task_main(int argc, char *argv[])
PX4_INFO("Starting PWM output in ocpoc_mmap mode");
pwm_out = new OcpocMmapPWMOut(_max_num_outputs);
#ifdef __DF_BBBLUE
#ifdef CONFIG_ARCH_BOARD_BEAGLEBONE_BLUE
} else if (strcmp(_protocol, "bbblue_rc") == 0) {
PX4_INFO("Starting PWM output in bbblue_rc mode");

3
src/drivers/magnetometer/ist8310/ist8310.cpp
View File

@ -874,9 +874,6 @@ IST8310::collect()
/* post a report to the ring */
_reports->force(&new_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/*
periodically check the range register and configuration
registers. With a bad I2C cable it is possible for the

2
src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp
View File

@ -48,8 +48,6 @@
#include <sys/types.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>

2
src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp
View File

@ -48,8 +48,6 @@
#include <sys/types.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/spi.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>

2
src/drivers/magnetometer/qmc5883/qmc5883_i2c.cpp
View File

@ -49,8 +49,6 @@
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>

2
src/drivers/magnetometer/qmc5883/qmc5883_spi.cpp
View File

@ -49,8 +49,6 @@
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/spi.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>

2
src/drivers/magnetometer/rm3100/rm3100_i2c.cpp
View File

@ -48,7 +48,7 @@
#include <sys/types.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>

2
src/drivers/magnetometer/rm3100/rm3100_spi.cpp
View File

@ -48,8 +48,6 @@
#include <sys/types.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/spi.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_device.h>

2
src/drivers/px4io/px4io.cpp
View File

@ -58,7 +58,7 @@
#include <math.h>
#include <crc32.h>
#include <arch/board/board.h>
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>

1
src/drivers/qshell/qurt/qshell.cpp
View File

@ -58,7 +58,6 @@
#include <uORB/topics/qshell_retval.h>
#include <drivers/drv_hrt.h>
#include "DriverFramework.hpp"
#define MAX_ARGS 8 // max number of whitespace separated args after app name

56
src/drivers/qurt/fc_addon/mpu_spi/CMakeLists.txt
View File

@ -1,56 +0,0 @@
############################################################################
#
# 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(mpu9x50 SHARED IMPORTED GLOBAL)
set_target_properties(mpu9x50 PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libmpu9x50.so)
px4_add_module(
MODULE platforms__qurt__fc_addon__mpu_spi
MAIN mpu9x50
INCLUDES
${FC_ADDON}/flight_controller/hexagon/inc
SRCS
mpu9x50_main.cpp
DEPENDS
)
target_link_libraries(platforms__qurt__fc_addon__mpu_spi PRIVATE mpu9x50)

641
src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
View File

@ -1,641 +0,0 @@
/****************************************************************************
*
* 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 <stdint.h>
#include <fcntl.h>
#include <unistd.h>
#include <signal.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/getopt.h>
// TODO-JYW:
// Include references to the driver framework. This will produce a duplicate definition
// of qurt_log. Use a #define for the qurt_log function to avoid redefinition. This code
// change must still be made. Document the latter change to be clear.
// #include <df_stubs.h>
// #include <SPIDevObj.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#ifdef __cplusplus
extern "C" {
#endif
#include <semaphore.h>
#include <mpu9x50.h>
#ifdef __cplusplus
}
#endif
/** driver 'main' command */
extern "C" { __EXPORT int mpu9x50_main(int argc, char *argv[]); }
#define MAX_LEN_DEV_PATH 32
// TODO - need to load this from parameter file
#define SPI_INT_GPIO 65 // GPIO pin for Data Ready Interrupt
namespace mpu9x50
{
/** SPI device path that mpu9x50 is connected to */
static char _device[MAX_LEN_DEV_PATH];
/** flag indicating if mpu9x50 app is running */
static bool _is_running = false;
/** flag indicating if measurement thread should exit */
static bool _task_should_exit = false;
/** handle to the task main thread */
static px4_task_t _task_handle = -1;
/** IMU measurement data */
static struct mpu9x50_data _data;
static orb_advert_t _gyro_pub = nullptr; /**< gyro data publication */
static int _gyro_orb_class_instance; /**< instance handle for gyro devices */
static orb_advert_t _accel_pub = nullptr; /**< accelerameter data publication */
static int _accel_orb_class_instance; /**< instance handle for accel devices */
static orb_advert_t _mag_pub = nullptr; /**< compass data publication */
static int _mag_orb_class_instance; /**< instance handle for mag devices */
static int _params_sub; /**< parameter update subscription */
static sensor_gyro_s _gyro; /**< gyro report */
static sensor_accel_s _accel; /**< accel report */
static sensor_mag_s _mag; /**< mag report */
static struct gyro_calibration_s _gyro_sc; /**< gyro scale */
static struct accel_calibration_s _accel_sc; /**< accel scale */
static struct mag_calibration_s _mag_sc; /**< mag scale */
static enum gyro_lpf_e _gyro_lpf = MPU9X50_GYRO_LPF_20HZ; /**< gyro lpf enum value */
static enum acc_lpf_e _accel_lpf = MPU9X50_ACC_LPF_20HZ; /**< accel lpf enum value */
static enum gyro_sample_rate_e _imu_sample_rate = MPU9x50_SAMPLE_RATE_500HZ; /**< IMU sample rate enum */
struct {
param_t accel_x_offset;
param_t accel_x_scale;
param_t accel_y_offset;
param_t accel_y_scale;
param_t accel_z_offset;
param_t accel_z_scale;
param_t gyro_x_offset;
param_t gyro_y_offset;
param_t gyro_z_offset;
param_t mag_x_offset;
param_t mag_x_scale;
param_t mag_y_offset;
param_t mag_y_scale;
param_t mag_z_offset;
param_t mag_z_scale;
param_t gyro_lpf_enum;
param_t accel_lpf_enum;
param_t imu_sample_rate_enum;
} _params_handles; /**< parameter handles */
bool exit_mreasurement = false;
/** Print out the usage information */
static void usage();
/** 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[]);
/**
* create and advertise all publicatoins
* @return true on success, false otherwise
*/
static bool create_pubs();
/** update all sensor reports with the latest sensor reading */
static void update_reports();
/** publish all sensor reports */
static void publish_reports();
/** update all parameters */
static void parameters_update();
/** initialize all parameter handles and load the initial parameter values */
static void parameters_init();
/** poll parameter update */
static void parameter_update_poll();
static int64_t _isr_data_ready_timestamp = 0;
/**
* MPU9x50 data ready interrupt service routine
* @param[out] context address of the context data provided by user whill
* registering the interrupt servcie routine
*/
static void *data_ready_isr(void *context);
void *data_ready_isr(void *context)
{
if (exit_mreasurement) {
return NULL;
}
_isr_data_ready_timestamp = hrt_absolute_time();
// send signal to measurement thread
px4_task_kill(_task_handle, SIGRTMIN);
return NULL;
}
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, &param_update);
parameters_update();
}
}
void parameters_update()
{
PX4_DEBUG("mpu9x50_parameters_update");
float v;
int v_int;
// accel params
if (param_get(_params_handles.accel_x_offset, &v) == 0) {
_accel_sc.x_offset = v;
PX4_DEBUG("mpu9x50_parameters_update accel_x_offset %f", v);
}
if (param_get(_params_handles.accel_x_scale, &v) == 0) {
_accel_sc.x_scale = v;
PX4_DEBUG("mpu9x50_parameters_update accel_x_scale %f", v);
}
if (param_get(_params_handles.accel_y_offset, &v) == 0) {
_accel_sc.y_offset = v;
PX4_DEBUG("mpu9x50_parameters_update accel_y_offset %f", v);
}
if (param_get(_params_handles.accel_y_scale, &v) == 0) {
_accel_sc.y_scale = v;
PX4_DEBUG("mpu9x50_parameters_update accel_y_scale %f", v);
}
if (param_get(_params_handles.accel_z_offset, &v) == 0) {
_accel_sc.z_offset = v;
PX4_DEBUG("mpu9x50_parameters_update accel_z_offset %f", v);
}
if (param_get(_params_handles.accel_z_scale, &v) == 0) {
_accel_sc.z_scale = v;
PX4_DEBUG("mpu9x50_parameters_update accel_z_scale %f", v);
}
// gyro params
if (param_get(_params_handles.gyro_x_offset, &v) == 0) {
_gyro_sc.x_offset = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_x_offset %f", v);
}
if (param_get(_params_handles.gyro_y_offset, &v) == 0) {
_gyro_sc.y_offset = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_y_offset %f", v);
}
if (param_get(_params_handles.gyro_z_offset, &v) == 0) {
_gyro_sc.z_offset = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_z_offset %f", v);
}
// mag params
if (param_get(_params_handles.mag_x_offset, &v) == 0) {
_mag_sc.x_offset = v;
PX4_DEBUG("mpu9x50_parameters_update mag_x_offset %f", v);
}
if (param_get(_params_handles.mag_x_scale, &v) == 0) {
_mag_sc.x_scale = v;
PX4_DEBUG("mpu9x50_parameters_update mag_x_scale %f", v);
}
if (param_get(_params_handles.mag_y_offset, &v) == 0) {
_mag_sc.y_offset = v;
PX4_DEBUG("mpu9x50_parameters_update mag_y_offset %f", v);
}
if (param_get(_params_handles.mag_y_scale, &v) == 0) {
_mag_sc.y_scale = v;
PX4_DEBUG("mpu9x50_parameters_update mag_y_scale %f", v);
}
if (param_get(_params_handles.mag_z_offset, &v) == 0) {
_mag_sc.z_offset = v;
PX4_DEBUG("mpu9x50_parameters_update mag_z_offset %f", v);
}
if (param_get(_params_handles.mag_z_scale, &v) == 0) {
_mag_sc.z_scale = v;
PX4_DEBUG("mpu9x50_parameters_update mag_z_scale %f", v);
}
// LPF params
if (param_get(_params_handles.gyro_lpf_enum, &v_int) == 0) {
if (v_int >= NUM_MPU9X50_GYRO_LPF) {
PX4_WARN("invalid gyro_lpf_enum %d use default %d", v_int, _gyro_lpf);
} else {
_gyro_lpf = (enum gyro_lpf_e)v_int;
PX4_DEBUG("mpu9x50_parameters_update gyro_lpf_enum %d", _gyro_lpf);
}
}
if (param_get(_params_handles.accel_lpf_enum, &v_int) == 0) {
if (v_int >= NUM_MPU9X50_ACC_LPF) {
PX4_WARN("invalid accel_lpf_enum %d use default %d", v_int, _accel_lpf);
} else {
_accel_lpf = (enum acc_lpf_e)v_int;
PX4_DEBUG("mpu9x50_parameters_update accel_lpf_enum %d", _accel_lpf);
}
}
if (param_get(_params_handles.imu_sample_rate_enum, &v_int) == 0) {
if (v_int >= NUM_MPU9X50_SAMPLE_RATE) {
PX4_WARN("invalid imu_sample_rate %d use default %d", v_int, _imu_sample_rate);
} else {
_imu_sample_rate = (enum gyro_sample_rate_e)v_int;
PX4_DEBUG("mpu9x50_parameters_update imu_sample_rate %d", _imu_sample_rate);
}
}
}
void parameters_init()
{
_params_handles.accel_x_offset = param_find("CAL_ACC0_XOFF");
_params_handles.accel_x_scale = param_find("CAL_ACC0_XSCALE");
_params_handles.accel_y_offset = param_find("CAL_ACC0_YOFF");
_params_handles.accel_y_scale = param_find("CAL_ACC0_YSCALE");
_params_handles.accel_z_offset = param_find("CAL_ACC0_ZOFF");
_params_handles.accel_z_scale = param_find("CAL_ACC0_ZSCALE");
_params_handles.gyro_x_offset = param_find("CAL_GYRO0_XOFF");
_params_handles.gyro_y_offset = param_find("CAL_GYRO0_YOFF");
_params_handles.gyro_z_offset = param_find("CAL_GYRO0_ZOFF");
_params_handles.mag_x_offset = param_find("CAL_MAG0_XOFF");
_params_handles.mag_x_scale = param_find("CAL_MAG0_XSCALE");
_params_handles.mag_y_offset = param_find("CAL_MAG0_YOFF");
_params_handles.mag_y_scale = param_find("CAL_MAG0_YSCALE");
_params_handles.mag_z_offset = param_find("CAL_MAG0_ZOFF");
_params_handles.mag_z_scale = param_find("CAL_MAG0_ZSCALE");
_params_handles.gyro_lpf_enum = param_find("MPU_GYRO_LPF_ENM");
_params_handles.accel_lpf_enum = param_find("MPU_ACC_LPF_ENM");
_params_handles.imu_sample_rate_enum = param_find("MPU_SAMPLE_R_ENM");
parameters_update();
}
bool create_pubs()
{
// initialize the reports
memset(&_gyro, 0, sizeof(sensor_gyro_s));
memset(&_accel, 0, sizeof(sensor_accel_s));
memset(&_mag, 0, sizeof(sensor_mag_s));
_gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro,
&_gyro_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_gyro_pub == nullptr) {
PX4_ERR("sensor_gyro advert fail");
return false;
}
_accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel,
&_accel_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_accel_pub == nullptr) {
PX4_ERR("sensor_accel advert fail");
return false;
}
_mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag,
&_mag_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_mag_pub == nullptr) {
PX4_ERR("sensor_mag advert fail");
return false;
}
return true;
}
void update_reports()
{
_gyro.timestamp = _data.timestamp;
_gyro.x = (_data.gyro_raw[0] * _data.gyro_scaling) - _gyro_sc.x_offset;
_gyro.y = (_data.gyro_raw[1] * _data.gyro_scaling) - _gyro_sc.y_offset;
_gyro.z = (_data.gyro_raw[2] * _data.gyro_scaling) - _gyro_sc.z_offset;
_gyro.temperature = _data.temperature;
_gyro.scaling = _data.gyro_scaling;
_gyro.x_raw = _data.gyro_raw[0];
_gyro.y_raw = _data.gyro_raw[1];
_gyro.z_raw = _data.gyro_raw[2];
_accel.timestamp = _data.timestamp;
_accel.x = ((_data.accel_raw[0] * _data.accel_scaling) - _accel_sc.x_offset) * _accel_sc.x_scale;
_accel.y = ((_data.accel_raw[1] * _data.accel_scaling) - _accel_sc.y_offset) * _accel_sc.y_scale;
_accel.z = ((_data.accel_raw[2] * _data.accel_scaling) - _accel_sc.z_offset) * _accel_sc.z_scale;
_accel.temperature = _data.temperature;
_accel.scaling = _data.accel_scaling;
_accel.x_raw = _data.accel_raw[0];
_accel.y_raw = _data.accel_raw[1];
_accel.z_raw = _data.accel_raw[2];
if (_data.mag_data_ready) {
_mag.timestamp = _data.timestamp;
_mag.x = ((_data.mag_raw[0] * _data.mag_scaling) - _mag_sc.x_offset) * _mag_sc.x_scale;
_mag.y = ((_data.mag_raw[1] * _data.mag_scaling) - _mag_sc.y_offset) * _mag_sc.y_scale;
_mag.z = ((_data.mag_raw[2] * _data.mag_scaling) - _mag_sc.z_offset) * _mag_sc.z_scale;
_mag.scaling = _data.mag_scaling;
_mag.temperature = _data.temperature;
_mag.x_raw = _data.mag_raw[0];
_mag.y_raw = _data.mag_raw[1];
_mag.z_raw = _data.mag_raw[2];
}
}
void publish_reports()
{
if (orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &_gyro) != OK) {
PX4_WARN("failed to publish gyro report");
} else {
//PX4_DEBUG("MPU_GYRO_RAW: %d %d %d", _gyro.x_raw, _gyro.y_raw, _gyro.z_raw)
//PX4_DEBUG("MPU_GYRO: %f %f %f", _gyro.x, _gyro.y, _gyro.z)
}
if (orb_publish(ORB_ID(sensor_accel), _accel_pub, &_accel) != OK) {
PX4_WARN("failed to publish accel report");
} else {
//PX4_DEBUG("MPU_ACCEL_RAW: %d %d %d", _accel.x_raw, _accel.y_raw, _accel.z_raw)
//PX4_DEBUG("MPU_ACCEL: %f %f %f", _accel.x, _accel.y, _accel.z)
}
if (_data.mag_data_ready) {
if (orb_publish(ORB_ID(sensor_mag), _mag_pub, &_mag) != OK) {
PX4_WARN("failed to publish mag report");
} else {
//PX4_DEBUG("MPU_MAG_RAW: %d %d %d", _mag.x_raw, _mag.y_raw, _mag.z_raw)
//PX4_DEBUG("MPU_MAG: %f %f %f", _mag.x, _mag.y, _mag.z)
}
}
}
void task_main(int argc, char *argv[])
{
PX4_WARN("enter mpu9x50 task_main");
sigset_t set;
int sig = 0;
int rv;
exit_mreasurement = false;
parameters_init();
// create the mpu9x50 default configuration structure
struct mpu9x50_config config = {
.gyro_lpf = _gyro_lpf,
.acc_lpf = _accel_lpf,
.gyro_fsr = MPU9X50_GYRO_FSR_500DPS,
.acc_fsr = MPU9X50_ACC_FSR_4G,
.gyro_sample_rate = _imu_sample_rate,
.compass_enabled = true,
.compass_sample_rate = MPU9x50_COMPASS_SAMPLE_RATE_100HZ,
.spi_dev_path = _device,
};
// TODO-JYW:
// manually register with the DriverFramework to allow this driver to
// be found by other modules
// DriverFramework::StubSensor<DriverFramework::SPIDevObj> stub_sensor("mpu9x50", "/dev/accel0", "/dev/accel");
if (mpu9x50_initialize(&config) != 0) {
PX4_WARN("error initializing mpu9x50. Quit!");
goto exit;
}
if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL)
//if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL)
!= 0) {
PX4_WARN("error registering data ready interrupt. Quit!");
goto exit;
}
// create all uorb publications
if (!create_pubs()) {
goto exit;
}
// subscribe to parameter_update topic
_params_sub = orb_subscribe(ORB_ID(parameter_update));
// initialize signal
sigemptyset(&set);
sigaddset(&set, SIGRTMIN);
while (!_task_should_exit) {
// wait on signal
rv = sigwait(&set, &sig);
// check if we are waken up by the proper signal
if (rv != 0 || sig != SIGRTMIN) {
PX4_WARN("mpu9x50 sigwait failed rv %d sig %d", rv, sig);
continue;
}
// read new IMU data and store the results in data
if (mpu9x50_get_data(&_data) != 0) {
PX4_WARN("mpu9x50_get_data() failed");
continue;
}
// since the context switch takes long time, override the timestamp provided by mpu9x50_get_data()
// with the ts of isr.
// Note: This is ok for MPU sample rate of < 1000; Higer than 1000 Sample rates will be an issue
// as the data is not consistent.
_data.timestamp = _isr_data_ready_timestamp;
// poll parameter update
parameter_update_poll();
// data is ready
update_reports();
// publish all sensor reports
publish_reports();
}
exit_mreasurement = true;
exit:
PX4_WARN("closing mpu9x50");
mpu9x50_close();
}
/** mpu9x50 main entrance */
void task_main_trampoline(int argc, char *argv[])
{
PX4_WARN("task_main_trampoline");
task_main(argc, argv);
}
void start()
{
ASSERT(_task_handle == -1);
/* start the task */
_task_handle = px4_task_spawn_cmd("mpu9x50_main",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1500,
(px4_main_t)&task_main_trampoline,
nullptr);
if (_task_handle < 0) {
warn("mpu9x50_main 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 usage()
{
PX4_WARN("missing command: try 'start', 'stop', 'status'");
PX4_WARN("options:");
PX4_WARN(" -D device");
}
}; // namespace mpu9x50
int mpu9x50_main(int argc, char *argv[])
{
const char *device = NULL;
int ch;
int myoptind = 1;
const char *myoptarg = NULL;
while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'D':
device = myoptarg;
break;
default:
mpu9x50::usage();
return 1;
}
}
// Check on required arguments
if (device == NULL || strlen(device) == 0) {
mpu9x50::usage();
return 1;
}
memset(mpu9x50::_device, 0, MAX_LEN_DEV_PATH);
strncpy(mpu9x50::_device, device, strlen(device));
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
if (mpu9x50::_is_running) {
PX4_WARN("mpu9x50 already running");
return 1;
}
mpu9x50::start();
} else if (!strcmp(verb, "stop")) {
if (mpu9x50::_is_running) {
PX4_WARN("mpu9x50 is not running");
return 1;
}
mpu9x50::stop();
} else if (!strcmp(verb, "status")) {
PX4_WARN("mpu9x50 is %s", mpu9x50::_is_running ? "running" : "stopped");
return 0;
} else {
mpu9x50::usage();
return 1;
}
return 0;
}

86
src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_params.c
View File

@ -1,86 +0,0 @@
/****************************************************************************
*
* 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_platform_common/px4_config.h>
#include <parameters/param.h>
/**
* Low pass filter frequency for Gyro
*
* @value 0 MPU9X50_GYRO_LPF_250HZ
* @value 1 MPU9X50_GYRO_LPF_184HZ
* @value 2 MPU9X50_GYRO_LPF_92HZ
* @value 3 MPU9X50_GYRO_LPF_41HZ
* @value 4 MPU9X50_GYRO_LPF_20HZ
* @value 5 MPU9X50_GYRO_LPF_10HZ
* @value 6 MPU9X50_GYRO_LPF_5HZ
* @value 7 MPU9X50_GYRO_LPF_3600HZ_NOLPF
*
* @group MPU9x50 Configuration
*/
PARAM_DEFINE_INT32(MPU_GYRO_LPF_ENM, 4);
/**
* Low pass filter frequency for Accelerometer
*
*
* @value 0 MPU9X50_ACC_LPF_460HZ
* @value 1 MPU9X50_ACC_LPF_184HZ
* @value 2 MPU9X50_ACC_LPF_92HZ
* @value 3 MPU9X50_ACC_LPF_41HZ
* @value 4 MPU9X50_ACC_LPF_20HZ
* @value 5 MPU9X50_ACC_LPF_10HZ
* @value 6 MPU9X50_ACC_LPF_5HZ
* @value 7 MPU9X50_ACC_LPF_460HZ_NOLPF
*
* @group MPU9x50 Configuration
*/
PARAM_DEFINE_INT32(MPU_ACC_LPF_ENM, 4);
/**
* Sample rate in Hz
*
* @value 0 MPU9x50_SAMPLE_RATE_100HZ
* @value 1 MPU9x50_SAMPLE_RATE_200HZ
* @value 2 MPU9x50_SAMPLE_RATE_500HZ
* @value 3 MPU9x50_SAMPLE_RATE_1000HZ
*
* @group MPU9x50 Configuration
*/
PARAM_DEFINE_INT32(MPU_SAMPLE_R_ENM, 2);

55
src/drivers/qurt/fc_addon/rc_receiver/CMakeLists.txt
View File

@ -1,55 +0,0 @@
############################################################################
#
# 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()
add_library(rc_receiver SHARED IMPORTED GLOBAL)
set_target_properties(rc_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
INCLUDES
${FC_ADDON}/flight_controller/hexagon/inc
SRCS
rc_receiver_main.cpp
)
target_link_libraries(platforms__qurt__fc_addon__rc_receiver PRIVATE rc_receiver)

347
src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp
View File

@ -1,347 +0,0 @@
/****************************************************************************
*
* 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_platform_common/getopt.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/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, &param_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 = ts;
_rc_in.timestamp_last_signal = _rc_in.timestamp;
_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;
}

53
src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_params.c
View File

@ -1,53 +0,0 @@
/****************************************************************************
*
* 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_platform_common/px4_config.h>
#include <parameters/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);

56
src/drivers/qurt/fc_addon/uart_esc/CMakeLists.txt
View File

@ -1,56 +0,0 @@
############################################################################
#
# 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(uart_esc SHARED IMPORTED GLOBAL)
set_target_properties(uart_esc PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libuart_esc.so)
px4_add_module(
MODULE platforms__qurt__fc_addon__uart_esc
MAIN uart_esc
INCLUDES
${FC_ADDON}/flight_controller/hexagon/inc
SRCS
uart_esc_main.cpp
DEPENDS
)
target_link_libraries(platforms__qurt__fc_addon__uart_esc PRIVATE uart_esc)

521
src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp
View File

@ -1,521 +0,0 @@
/****************************************************************************
*
* 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 <stdint.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/posix.h>
#include <errno.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <lib/mixer/MixerGroup.hpp>
#include <lib/mixer/mixer_multirotor_normalized.generated.h>
#include <parameters/param.h>
#ifdef __cplusplus
extern "C" {
#endif
#include <uart_esc.h>
#ifdef __cplusplus
}
#endif
/** driver 'main' command */
extern "C" { __EXPORT int uart_esc_main(int argc, char *argv[]); }
#define MAX_LEN_DEV_PATH 32
namespace uart_esc
{
#define UART_ESC_MAX_MOTORS 4
volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit
static char _device[MAX_LEN_DEV_PATH];
static bool _is_running = false; // flag indicating if uart_esc app is running
static px4_task_t _task_handle = -1; // handle to the task main thread
UartEsc *esc; // esc instance
void uart_esc_rotate_motors(int16_t *motor_rpm, int num_rotors); // motor re-mapping
// subscriptions
int _controls_sub;
int _armed_sub;
int _param_sub;
// filenames
// /dev/fs/ is mapped to /usr/share/data/adsp/
static const char *MIXER_FILENAME = "/dev/fs/mixer_config.mix";
// publications
orb_advert_t _outputs_pub;
// topic structures
actuator_controls_s _controls;
actuator_armed_s _armed;
parameter_update_s _param_update;
actuator_outputs_s _outputs;
/** Print out the usage information */
static void usage();
/** uart_esc start */
static void start(const char *device) __attribute__((unused));
/** uart_esc stop */
static void stop();
/** task main trampoline function */
static void task_main_trampoline(int argc, char *argv[]);
/** uart_esc thread primary entry point */
static void task_main(int argc, char *argv[]);
/** mixer initialization */
static MultirotorMixer *mixer;
static int initialize_mixer(const char *mixer_filename);
static int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
static void parameters_init();
static void parameters_update();
struct {
int model;
int baudrate;
int px4_motor_mapping[UART_ESC_MAX_MOTORS];
} _parameters;
struct {
param_t model;
param_t baudrate;
param_t px4_motor_mapping[UART_ESC_MAX_MOTORS];
} _parameter_handles;
void parameters_init()
{
_parameter_handles.model = param_find("UART_ESC_MODEL");
_parameter_handles.baudrate = param_find("UART_ESC_BAUD");
/* PX4 motor mapping parameters */
for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) {
char nbuf[20];
/* min values */
sprintf(nbuf, "UART_ESC_MOTOR%d", i + 1);
_parameter_handles.px4_motor_mapping[i] = param_find(nbuf);
}
parameters_update();
}
void parameters_update()
{
PX4_WARN("uart_esc_main parameters_update");
int32_t v_int;
if (param_get(_parameter_handles.model, &v_int) == 0) {
_parameters.model = v_int;
PX4_WARN("UART_ESC_MODEL %d", _parameters.model);
}
if (param_get(_parameter_handles.baudrate, &v_int) == 0) {
_parameters.baudrate = v_int;
PX4_WARN("UART_ESC_BAUD %d", _parameters.baudrate);
}
for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) {
if (param_get(_parameter_handles.px4_motor_mapping[i], &v_int) == 0) {
_parameters.px4_motor_mapping[i] = v_int;
PX4_WARN("UART_ESC_MOTOR%d %d", i + 1, _parameters.px4_motor_mapping[i]);
}
}
}
int mixer_control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls[control_group].control[control_index];
/* motor spinup phase - lock throttle to zero *
if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
* limit the throttle output to zero during motor spinup,
* as the motors cannot follow any demand yet
*
input = 0.0f;
}
}
*/
return 0;
}
int initialize_mixer(const char *mixer_filename)
{
mixer = nullptr;
int mixer_initialized = -1;
char buf[2048];
unsigned int buflen = sizeof(buf);
PX4_INFO("Initializing mixer from config file in %s", mixer_filename);
int fd_load = open(mixer_filename, O_RDONLY);
if (fd_load != -1) {
int nRead = read(fd_load, buf, buflen);
close(fd_load);
if (nRead > 0) {
mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
if (mixer != nullptr) {
PX4_INFO("Successfully initialized mixer from config file");
mixer_initialized = 0;
} else {
PX4_WARN("Unable to parse from mixer config file");
}
} else {
PX4_WARN("Unable to read from mixer config file");
}
} else {
PX4_WARN("Unable to open mixer config file");
}
// mixer file loading failed, fall back to default mixer configuration for
// QUAD_X airframe
if (mixer_initialized < 0) {
float roll_scale = 1;
float pitch_scale = 1;
float yaw_scale = 1;
float deadband = 0;
mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
MultirotorGeometry::QUAD_X,
roll_scale, pitch_scale, yaw_scale, deadband);
if (mixer == nullptr) {
PX4_ERR("mixer initialization failed");
mixer_initialized = -1;
return mixer_initialized;
}
PX4_WARN("mixer config file not found, successfully initialized default quad x mixer");
mixer_initialized = 0;
}
return mixer_initialized;
}
/**
* Rotate the motor rpm values based on the motor mappings configuration stored
* in motor_mapping
*/
void uart_esc_rotate_motors(int16_t *motor_rpm, int num_rotors)
{
ASSERT(num_rotors <= UART_ESC_MAX_MOTORS);
int i;
int16_t motor_rpm_copy[UART_ESC_MAX_MOTORS];
for (i = 0; i < num_rotors; i++) {
motor_rpm_copy[i] = motor_rpm[i];
}
for (i = 0; i < num_rotors; i++) {
motor_rpm[_parameters.px4_motor_mapping[i] - 1] = motor_rpm_copy[i];
}
}
void task_main(int argc, char *argv[])
{
PX4_INFO("enter uart_esc task_main");
_outputs_pub = nullptr;
parameters_init();
esc = UartEsc::get_instance();
if (esc == NULL) {
PX4_ERR("failed to new UartEsc instance");
} else if (esc->initialize((enum esc_model_t)_parameters.model,
_device, _parameters.baudrate) < 0) {
PX4_ERR("failed to initialize UartEsc");
} else {
// Subscribe for orb topics
_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); // single group for now
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_param_sub = orb_subscribe(ORB_ID(parameter_update));
// initialize publication structures
memset(&_outputs, 0, sizeof(_outputs));
// set up poll topic and limit poll interval
px4_pollfd_struct_t fds[1];
fds[0].fd = _controls_sub;
fds[0].events = POLLIN;
//orb_set_interval(_controls_sub, 10); // max actuator update period, ms
// set up mixer
if (initialize_mixer(MIXER_FILENAME) < 0) {
PX4_ERR("Mixer initialization failed.");
_task_should_exit = true;
}
// Main loop
while (!_task_should_exit) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
// Handle new actuator controls data
if (fds[0].revents & POLLIN) {
// Grab new controls data
orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
// Mix to the outputs
_outputs.timestamp = hrt_absolute_time();
int16_t motor_rpms[UART_ESC_MAX_MOTORS];
if (_armed.armed && !_armed.lockdown && !_armed.manual_lockdown) {
_outputs.noutputs = mixer->mix(&_outputs.output[0], actuator_controls_s::NUM_ACTUATOR_CONTROLS);
// Make sure we support only up to UART_ESC_MAX_MOTORS motors
if (_outputs.noutputs > UART_ESC_MAX_MOTORS) {
PX4_ERR("Unsupported motors %d, up to %d motors supported",
_outputs.noutputs, UART_ESC_MAX_MOTORS);
continue;
}
// Send outputs to the ESCs
for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
// map -1.0 - 1.0 outputs to RPMs
motor_rpms[outIdx] = (int16_t)(((_outputs.output[outIdx] + 1.0) / 2.0) *
(esc->max_rpm() - esc->min_rpm()) + esc->min_rpm());
}
uart_esc_rotate_motors(motor_rpms, _outputs.noutputs);
} else {
_outputs.noutputs = UART_ESC_MAX_MOTORS;
for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
motor_rpms[outIdx] = 0;
_outputs.output[outIdx] = -1.0;
}
}
esc->send_rpms(&motor_rpms[0], _outputs.noutputs);
// TODO-JYW: TESTING-TESTING
// MAINTAIN FOR REFERENCE, COMMENT OUT FOR RELEASE BUILDS
// static int count=0;
// count++;
// if (!(count % 100)) {
// PX4_DEBUG(" ");
// PX4_DEBUG("Time t: %13lu, Armed: %d ",(unsigned long)_outputs.timestamp,_armed.armed);
// PX4_DEBUG("Act Controls: 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_controls.control[0],_controls.control[1],_controls.control[2],_controls.control[3]);
// PX4_DEBUG("Act Outputs : 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_outputs.output[0],_outputs.output[1],_outputs.output[2],_outputs.output[3]);
// }
// TODO-JYW: TESTING-TESTING
/* Publish mixed control outputs */
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
}
// Check for updates in other subscripions
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
PX4_DEBUG("arming status updated, _armed.armed: %d", _armed.armed);
}
orb_check(_param_sub, &updated);
if (updated) {
// Even though we are only interested in the update status of the parameters, copy
// the subscription to clear the update status.
orb_copy(ORB_ID(parameter_update), _param_sub, &_param_update);
parameters_update();
}
}
}
PX4_WARN("closing uart_esc");
delete esc;
}
/** uart_esc main entrance */
void task_main_trampoline(int argc, char *argv[])
{
PX4_WARN("task_main_trampoline");
task_main(argc, argv);
}
void start()
{
ASSERT(_task_handle == -1);
/* start the task */
_task_handle = px4_task_spawn_cmd("uart_esc_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 usage()
{
PX4_WARN("missing command: try 'start', 'stop', 'status'");
PX4_WARN("options:");
PX4_WARN(" -D device");
}
}; // namespace uart_esc
int uart_esc_main(int argc, char *argv[])
{
const char *device = NULL;
int ch;
int myoptind = 1;
const char *myoptarg = NULL;
while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'D':
device = myoptarg;
break;
default:
uart_esc::usage();
return 1;
}
}
// Check on required arguments
if (device == NULL || strlen(device) == 0) {
uart_esc::usage();
return 1;
}
memset(uart_esc::_device, 0, MAX_LEN_DEV_PATH);
strncpy(uart_esc::_device, device, strlen(device));
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
if (uart_esc::_is_running) {
PX4_WARN("uart_esc already running");
return 1;
}
uart_esc::start();
}
else if (!strcmp(verb, "stop")) {
if (uart_esc::_is_running) {
PX4_WARN("uart_esc is not running");
return 1;
}
uart_esc::stop();
}
else if (!strcmp(verb, "status")) {
PX4_WARN("uart_esc is %s", uart_esc::_is_running ? "running" : "stopped");
return 0;
} else {
uart_esc::usage();
return 1;
}
return 0;
}

139
src/drivers/qurt/fc_addon/uart_esc/uart_esc_params.c
View File

@ -1,139 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Ramakrishna Kintada. 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 uart_esc_params.c
*
* Parameters defined for the uart esc driver
*/
#include <px4_platform_common/px4_config.h>
#include <parameters/param.h>
/**
* ESC model
*
* See esc_model_t enum definition in uart_esc_dev.h for all supported ESC
* model enum values.
*
* @value 0 ESC_200QX
* @value 1 ESC_350QX
* @value 2 ESC_210QC
*
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_MODEL, 2);
/**
* ESC UART baud rate
*
* Default rate is 250Kbps, whic is used in off-the-shelf QRP ESC products.
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000);
/**
* The PX4 default motor mappings are
* 1 4
* [front]
* 3 2
*
* The following paramters define the motor mappings in reference to the
* PX4 motor mapping convention.
*/
/**
* Default PX4 motor mappings
* 1 4
* [front]
* 3 2
*/
// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 1);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 3);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 4);
/**
* Motor mappings for 350QX
* 4 3
* [front]
* 1 2
*/
// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 4);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3);
/**
* Motor mappings for 200QX
* 2 3
* [front]
* 1 4
*/
// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 2);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 4);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3);
/**
* Motor mappings for 210QC
* 4 3
* [front]
* 1 2
*/
/**
* Motor 1 Mapping
*
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 4);
/**
* Motor 2 Mapping
*
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
/**
* Motor 3 Mapping
*
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1);
/**
* Motor 4 Mapping
*
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3);

41
src/drivers/qurt/tests/hello/CMakeLists.txt
View File

@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 2015 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 platforms__qurt__tests__hello
MAIN hello
SRCS
hello_main.cpp
hello_start_qurt.cpp
hello_example.cpp
DEPENDS
)

61
src/drivers/qurt/tests/hello/hello_example.cpp
View File

@ -1,61 +0,0 @@
/****************************************************************************
*
* 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 hello_example.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include "hello_example.h"
#include <px4_platform_common/log.h>
#include <unistd.h>
#include <stdio.h>
px4::AppState HelloExample::appState;
int HelloExample::main()
{
appState.setRunning(true);
int i = 0;
while (!appState.exitRequested() && i < 5) {
PX4_DEBUG(" Doing work...");
++i;
}
return 0;
}

54
src/drivers/qurt/tests/hello/hello_example.h
View File

@ -1,54 +0,0 @@
/****************************************************************************
*
* 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 hello_example.h
* Example app for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#pragma once
#include <px4_platform_common/app.h>
class HelloExample
{
public:
HelloExample() {}
~HelloExample() {}
int main();
static px4::AppState appState; /* track requests to terminate app */
};

56
src/drivers/qurt/tests/hello/hello_main.cpp
View File

@ -1,56 +0,0 @@
/****************************************************************************
*
* 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 hello_main.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include <px4_platform_common/log.h>
#include <px4_platform_common/app.h>
#include <px4_platform_common/init.h>
#include "hello_example.h"
#include <stdio.h>
int PX4_MAIN(int argc, char **argv)
{
px4::init(argc, argv, "hello");
PX4_DEBUG("hello");
HelloExample hello;
hello.main();
PX4_DEBUG("goodbye");
return 0;
}

101
src/drivers/qurt/tests/hello/hello_start_qurt.cpp
View File

@ -1,101 +0,0 @@
/****************************************************************************
*
* 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 hello_start_linux.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "hello_example.h"
#include <px4_platform_common/log.h>
#include <px4_platform_common/app.h>
#include <px4_platform_common/tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
static int daemon_task; /* Handle of deamon task / thread */
//using namespace px4;
extern "C" __EXPORT int hello_main(int argc, char *argv[]);
static void usage()
{
PX4_DEBUG("usage: hello {start|stop|status}");
}
int hello_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return 1;
}
if (!strcmp(argv[1], "start")) {
if (HelloExample::appState.isRunning()) {
PX4_DEBUG("already running");
/* this is not an error */
return 0;
}
daemon_task = px4_task_spawn_cmd("hello",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
16000,
PX4_MAIN,
(char *const *)argv);
return 0;
}
if (!strcmp(argv[1], "stop")) {
HelloExample::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (HelloExample::appState.isRunning()) {
PX4_DEBUG("is running");
} else {
PX4_DEBUG("not started");
}
return 0;
}
usage();
return 1;
}

Some files were not shown because too many files have changed in this diff Show More