fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rebase changes on upstream master

Browse Source 
This brings in many of the changes from the PX4 fork on ATLFLight.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2016-01-18 23:16:31 -08:00 committed by Julian Oes
parent efe9344fc2
commit 9f3bf8e9f4
71 changed files with 2734 additions and 564 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@ -13,9 +13,6 @@
[submodule "Tools/gencpp"]
path = Tools/gencpp
url = https://github.com/ros/gencpp.git
[submodule "src/lib/dspal"]
path = src/lib/dspal
url = https://github.com/ATLFlight/dspal.git
[submodule "Tools/jMAVSim"]
path = Tools/jMAVSim
url = https://github.com/PX4/jMAVSim.git

9
CMakeLists.txt
View File

@ -243,12 +243,12 @@ px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0")
px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest")
px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan")
px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework")
px4_add_git_submodule(TARGET git_ecl PATH "src/lib/ecl")
px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim")
px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo")
px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix")
px4_add_git_submodule(TARGET git_cmake_hexagon PATH "cmake/cmake_hexagon")
add_custom_target(submodule_clean
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
@ -293,6 +293,7 @@ px4_join(OUT CMAKE_C_FLAGS LIST "${c_flags}" GLUE " ")
px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ")
include_directories(${include_dirs})
message("INCLUDE_DIRS=${include_dirs}")
link_directories(${link_dirs})
add_definitions(${definitions})
@ -329,7 +330,11 @@ execute_process(COMMAND cmake -E make_directory ${ep_base}/Install/include)
#
set(module_libraries)
foreach(module ${config_module_list})
add_subdirectory(src/${module})
if(module MATCHES "Eagle")
add_subdirectory(${module} ${CMAKE_BINARY_DIR}/${module})
else()
add_subdirectory(src/${module})
endif()
px4_mangle_name(${module} mangled_name)
list(APPEND module_libraries ${mangled_name})
#message(STATUS "adding module: ${module}")

5
Makefile
View File

@ -155,6 +155,9 @@ posix_sitl_ekf2:
ros_sitl_default:
@echo "This target is deprecated. Use make 'posix_sitl_default gazebo' instead."
ros_sitl_default:
$(call cmake-build,$@)
qurt_eagle_travis:
$(call cmake-build,$@)
@ -163,7 +166,7 @@ qurt_eagle_release:
posix_eagle_release:
$(call cmake-build,$@)
qurt_eagle_default:
$(call cmake-build,$@)

2
cmake/cmake_hexagon

@ -1 +1 @@
Subproject commit 829f22eff345c934ca8939b2385768ca5e33794c
Subproject commit 720078ffe4964ee87742248f4563633d1f2b852c

17
cmake/common/px4_base.cmake
View File

@ -90,7 +90,7 @@ include(CMakeParseArguments)
# endfunction()
#
# test(NAME "hello" LIST a b c)
#
#
# OUTPUT:
# name: hello
# list: a b c
@ -349,11 +349,11 @@ function(px4_generate_messages)
list(APPEND msg_files_out ${msg_out_path}/${msg}.h)
endforeach()
add_custom_command(OUTPUT ${msg_files_out}
COMMAND ${PYTHON_EXECUTABLE}
COMMAND ${PYTHON_EXECUTABLE}
Tools/px_generate_uorb_topic_headers.py
${QUIET}
-d msg
-o ${msg_out_path}
-o ${msg_out_path}
-e msg/templates/uorb
-t ${CMAKE_BINARY_DIR}/topics_temporary
DEPENDS ${DEPENDS} ${MSG_FILES}
@ -370,11 +370,11 @@ function(px4_generate_messages)
list(APPEND msg_multi_files_out ${msg_multi_out_path}/px4_${msg}.h)
endforeach()
add_custom_command(OUTPUT ${msg_multi_files_out}
COMMAND ${PYTHON_EXECUTABLE}
COMMAND ${PYTHON_EXECUTABLE}
Tools/px_generate_uorb_topic_headers.py
${QUIET}
-d msg
-o ${msg_multi_out_path}
-o ${msg_multi_out_path}
-e msg/templates/px4/uorb
-t ${CMAKE_BINARY_DIR}/multi_topics_temporary/${OS}
-p "px4_"
@ -556,6 +556,7 @@ function(px4_add_common_flags)
-funsafe-math-optimizations
-ffunction-sections
-fdata-sections
-fPIC
)
endif()
@ -685,7 +686,7 @@ endfunction()
# Input:
# dirname : path to module dir
#
# Output:
# Output:
# newname : module name
#
# Example:
@ -716,8 +717,8 @@ endfunction()
function(px4_create_git_hash_header)
px4_parse_function_args(
NAME px4_create_git_hash_header
ONE_VALUE HEADER
REQUIRED HEADER
ONE_VALUE HEADER
REQUIRED HEADER
ARGN ${ARGN})
execute_process(
COMMAND git rev-parse HEAD

1
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

@ -83,6 +83,7 @@ set(config_module_list
modules/attitude_estimator_q
modules/ekf_att_pos_estimator
modules/position_estimator_inav
modules/ekf2
#
# Vehicle Control

3
cmake/configs/posix_eagle_default.cmake
View File

@ -1,6 +1,9 @@
include(posix/px4_impl_posix)
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake)
set(CONFIG_SHMEM "1")
set(config_module_list
drivers/device

9
cmake/configs/posix_eagle_release.cmake
View File

@ -16,8 +16,14 @@ set(CMAKE_PROGRAM_PATH
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
set(CONFIG_SHMEM "1")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DENABLE_SHMEM")
set(config_module_list
drivers/device
drivers/boards/sitl
drivers/led
systemcmds/param
systemcmds/ver
@ -28,6 +34,9 @@ set(config_module_list
modules/systemlib
modules/uORB
modules/dataman
modules/sdlog2
modules/simulator
modules/commander
lib/mathlib
lib/mathlib/math/filter

40
cmake/configs/qurt_eagle_release.cmake
View File

@ -1,18 +1,38 @@
include(qurt/px4_impl_qurt)
if ("${HEXAGON_DRIVERS_ROOT}" STREQUAL "")
message(FATAL_ERROR "HEXAGON_DRIVERS_ROOT is not set")
if ("$ENV{EAGLE_ADDON_ROOT}" STREQUAL "")
message(FATAL_ERROR "Enviroment variable EAGLE_ADDON_ROOT must be set")
else()
set(EAGLE_ADDON_ROOT $ENV{EAGLE_ADDON_ROOT})
endif()
if ("${EAGLE_DRIVERS_SRC}" STREQUAL "")
message(FATAL_ERROR "EAGLE_DRIVERS_SRC is not set")
if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
else()
set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
endif()
include_directories(${HEXAGON_DRIVERS_ROOT}/inc)
if ("$ENV{EAGLE_DRIVERS_SRC}" STREQUAL "")
message(FATAL_ERROR "Environment variable EAGLE_DRIVERS_SRC must be set")
else()
set(EAGLE_DRIVERS_SRC $ENV{EAGLE_DRIVERS_SRC})
endif()
STRING(REGEX REPLACE "//" "/" EAGLE_DRIVERS_SRC ${EAGLE_DRIVERS_SRC})
STRING(REGEX REPLACE "/" "__" EAGLE_DRIVERS_MODULE_PREFIX ${EAGLE_DRIVERS_SRC})
include_directories(${EAGLE_ADDON_ROOT}/flight_controller/hexagon/inc)
include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include)
message("hexagon_sdk_root is ${HEXAGON_SDK_ROOT}")
set(QURT_ENABLE_STUBS "0")
set(CONFIG_SHMEM "1")
# For Actual flight we need to link against the driver dynamic libraries
set(target_libraries
-L${HEXAGON_DRIVERS_ROOT}/libs
-L${EAGLE_ADDON_ROOT}/flight_controller/hexagon/libs
mpu9x50
uart_esc
csr_gps
@ -29,10 +49,10 @@ set(config_module_list
#
drivers/device
modules/sensors
$(EAGLE_DRIVERS_SRC)/mpu9x50
$(EAGLE_DRIVERS_SRC)/uart_esc
$(EAGLE_DRIVERS_SRC)/rc_receiver
$(EAGLE_DRIVERS_SRC)/csr_gps
${EAGLE_DRIVERS_SRC}/mpu9x50
${EAGLE_DRIVERS_SRC}/uart_esc
${EAGLE_DRIVERS_SRC}/rc_receiver
${EAGLE_DRIVERS_SRC}/csr_gps
#
# System commands

10
cmake/configs/qurt_eagle_travis.cmake
View File

@ -1,11 +1,19 @@
include(qurt/px4_impl_qurt)
# Run a full link with build stubs to make sure qurt target isn't broken
set(QURT_ENABLE_STUBS "1")
set(QURT_ENABLE_STUBS "0")
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-qurt.cmake)
include(${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/qurt_app.cmake)
if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
else()
set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
endif()
include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include)
set(config_module_list
drivers/device
drivers/boards/sitl

22
cmake/posix/px4_impl_posix.cmake
View File

@ -182,6 +182,7 @@ if(UNIX AND APPLE)
else()
set(added_definitions
-D__PX4_POSIX
-D__PX4_LINUX
@ -196,12 +197,33 @@ else()
)
endif()
if ("${BOARD}" STREQUAL "eagle")
if ("$ENV{HEXAGON_ARM_SYSROOT}" STREQUAL "")
message(FATAL_ERROR "HEXAGON_ARM_SYSROOT not set")
else()
set(HEXAGON_ARM_SYSROOT $ENV{HEXAGON_ARM_SYSROOT})
endif()
# Add the toolchain specific flags
set(HEXAGON_ARM_SYSROOT ${HEXAGON_SDK_ROOT}/sysroot)
set(added_cflags ${POSIX_CMAKE_C_FLAGS} --sysroot=${HEXAGON_ARM_SYSROOT})
set(added_cxx_flags ${POSIX_CMAKE_CXX_FLAGS} --sysroot=${HEXAGON_ARM_SYSROOT})
list(APPEND added_exe_linker_flags
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/usr/lib/arm-linux-gnueabihf
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/lib/arm-linux-gnueabihf
--sysroot=${HEXAGON_ARM_SYSROOT}
)
else()
# Add the toolchain specific flags
set(added_cflags ${POSIX_CMAKE_C_FLAGS})
set(added_cxx_flags ${POSIX_CMAKE_CXX_FLAGS})
endif()
# output
foreach(var ${inout_vars})
string(TOLOWER ${var} lower_var)

4
cmake/qurt/apps.h_in
View File

@ -36,9 +36,9 @@ ${builtin_apps_string}
void list_builtins(map<string,px4_main_t> &apps)
{
printf("Builtin Commands:\\n");
PX4_INFO("Builtin Commands:\\n");
for (map<string,px4_main_t>::iterator it=apps.begin(); it!=apps.end(); ++it)
printf("\\t%s\\n", (it->first).c_str());
PX4_INFO("%s : 0x%x\n", (it->first).c_str(), it->second);
}
static int shutdown_main(int argc, char *argv[])

4
cmake/qurt/px4_impl_qurt.cmake
View File

@ -158,7 +158,7 @@ function(px4_os_add_flags)
LINK_DIRS ${LINK_DIRS}
DEFINITIONS ${DEFINITIONS})
set(DSPAL_ROOT src/lib/dspal)
set(DSPAL_ROOT src/lib/DriverFramework/dspal)
set(added_include_dirs
${DSPAL_ROOT}/include
${DSPAL_ROOT}/sys
@ -222,7 +222,7 @@ function(px4_os_prebuild_targets)
ONE_VALUE OUT BOARD THREADS
REQUIRED OUT BOARD
ARGN ${ARGN})
add_custom_target(${OUT} DEPENDS git_dspal)
add_custom_target(${OUT} DEPENDS git_driverframework)
endfunction()

1
cmake/qurt/qurt_funcs.cmake
View File

@ -45,4 +45,3 @@
#
# The macros are called from the top level CMakeLists.txt
#
px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")

24
cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake
View File

@ -17,6 +17,12 @@
include(CMakeForceCompiler)
if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
message(FATAL_ERROR "HEXAGON_SDK_ROOT not set")
else()
set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_SDK_ROOT})
endif()
# this one is important
set(CMAKE_SYSTEM_NAME Generic)
@ -24,13 +30,19 @@ set(CMAKE_SYSTEM_NAME Generic)
set(CMAKE_SYSTEM_VERSION 1)
# specify the cross compiler
find_program(C_COMPILER arm-linux-gnueabihf-gcc)
find_program(C_COMPILER arm-linux-gnueabihf-gcc
PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin
NO_DEFAULT_PATH
)
if(NOT C_COMPILER)
message(FATAL_ERROR "could not find arm-linux-gnueabihf-gcc compiler")
endif()
cmake_force_c_compiler(${C_COMPILER} GNU)
find_program(CXX_COMPILER arm-linux-gnueabihf-g++)
find_program(CXX_COMPILER arm-linux-gnueabihf-g++
PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin
NO_DEFAULT_PATH
)
if(NOT CXX_COMPILER)
message(FATAL_ERROR "could not find arm-linux-gnueabihf-g++ compiler")
endif()
@ -39,7 +51,10 @@ cmake_force_cxx_compiler(${CXX_COMPILER} GNU)
# compiler tools
foreach(tool objcopy nm ld)
string(TOUPPER ${tool} TOOL)
find_program(${TOOL} arm-linux-gnueabihf-${tool})
find_program(${TOOL} arm-linux-gnueabihf-${tool}
PATHS ${HEXAGON_SDK_ROOT}/gcc-linaro-arm-linux-gnueabihf-4.8-2013.08_linux/bin
NO_DEFAULT_PATH
)
if(NOT ${TOOL})
message(FATAL_ERROR "could not find arm-linux-gnueabihf-${tool}")
endif()
@ -54,8 +69,11 @@ foreach(tool echo patch grep rm mkdir nm genromfs cp touch make unzip)
endif()
endforeach()
set(C_FLAGS "--sysroot=${HEXAGON_SDK_ROOT}/sysroot")
set(LINKER_FLAGS "-Wl,-gc-sections")
set(CMAKE_EXE_LINKER_FLAGS ${LINKER_FLAGS})
set(CMAKE_C_FLAGS ${C_FLAGS})
set(CMAKE_CXX_LINKER_FLAGS ${C_FLAGS})
# where is the target environment
set(CMAKE_FIND_ROOT_PATH get_file_component(${C_COMPILER} PATH))

8
posix-configs/eagle/200qx/mainapp-calib.config Normal file
View File

@ -0,0 +1,8 @@
uorb start
muorb start
mavlink start -u 14556
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
mavlink boot_complete
commander start

7
posix-configs/eagle/200qx/mainapp-flight.config Normal file
View File

@ -0,0 +1,7 @@
uorb start
muorb start
mavlink start -u 14556
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
mavlink boot_complete

72
posix-configs/eagle/200qx/px4-calib.config Normal file
View File

@ -0,0 +1,72 @@
uorb 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 RC_RECEIVER_TYPE 1
rc_receiver start -D /dev/tty-1
attitude_estimator_q start
position_estimator_inav start
mc_pos_control start
mc_att_control start
sleep 1
param set SYS_AUTOSTART 4001
param set SYS_AUTOCONFIG 1
param set MAV_TYPE 2​
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
sensors start
param set MC_YAW_P 7.0
param set MC_YAWRATE_P 0.08
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.08
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.0001
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.0001
param set ATT_W_MAG 0.00
param set PE_MAG_NOISE 1.0f
param set SENS_BOARD_ROT 0
param set MPU_GYRO_LPF_ENUM 4
param set MPU_ACC_LPF_ENUM 4
param set MPU_SAMPLE_RATE_ENUM 2
sleep 1
mpu9x50 start -D /dev/spi-1
uart_esc start -D /dev/tty-2
csr_gps start -D /dev/tty-3
list_devices
list_files
list_tasks
list_topics

80
posix-configs/eagle/200qx/px4-flight.config Normal file
View File

@ -0,0 +1,80 @@
uorb 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
commander start
param set RC_RECEIVER_TYPE 1
rc_receiver start -D /dev/tty-1
attitude_estimator_q start
position_estimator_inav start
mc_pos_control start
mc_att_control start
sleep 1
param set SYS_AUTOSTART 4001
param set SYS_AUTOCONFIG 1
param set MAV_TYPE 2​
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
sensors start
param set MC_YAW_P 7.0
param set MC_YAWRATE_P 0.08
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.08
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.0001
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.0001
param set ATT_W_MAG 0.00
param set PE_MAG_NOISE 1.0f
param set SENS_BOARD_ROT 0
param set MPU_GYRO_LPF_ENUM 4
param set MPU_ACC_LPF_ENUM 4
param set MPU_SAMPLE_RATE_ENUM 2
sleep 1
mpu9x50 start -D /dev/spi-1
param set UART_ESC_MODEL 0
param set UART_ESC_BAUDRATE 250000
param set UART_ESC_PX4MOTOR1 2
param set UART_ESC_PX4MOTOR2 4
param set UART_ESC_PX4MOTOR3 1
param set UART_ESC_PX4MOTOR4 3
sleep 1
uart_esc start -D /dev/tty-2
csr_gps start -D /dev/tty-3
list_devices
list_files
list_tasks
list_topics

8
posix-configs/eagle/210qc/mainapp-calib.config Normal file
View File

@ -0,0 +1,8 @@
uorb start
muorb start
mavlink start -u 14556
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
mavlink boot_complete
commander start

7
posix-configs/eagle/210qc/mainapp-flight.config Normal file
View File

@ -0,0 +1,7 @@
uorb start
muorb start
mavlink start -u 14556
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
mavlink boot_complete

72
posix-configs/eagle/210qc/px4-calib.config Normal file
View File

@ -0,0 +1,72 @@
uorb 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 RC_RECEIVER_TYPE 1
rc_receiver start -D /dev/tty-1
attitude_estimator_q start
position_estimator_inav start
mc_pos_control start
mc_att_control start
sleep 1
param set SYS_AUTOSTART 4001
param set SYS_AUTOCONFIG 1
param set MAV_TYPE 2​
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
sensors start
param set MC_YAW_P 7.0
param set MC_YAWRATE_P 0.08
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.08
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.0001
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.0001
param set ATT_W_MAG 0.00
param set PE_MAG_NOISE 1.0f
param set SENS_BOARD_ROT 0
param set MPU_GYRO_LPF_ENUM 4
param set MPU_ACC_LPF_ENUM 4
param set MPU_SAMPLE_RATE_ENUM 2
sleep 1
mpu9x50 start -D /dev/spi-1
uart_esc start -D /dev/tty-2
csr_gps start -D /dev/tty-3
list_devices
list_files
list_tasks
list_topics

95
posix-configs/eagle/210qc/px4-flight.config Normal file
View File

@ -0,0 +1,95 @@
uorb 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
commander start
param set RC_RECEIVER_TYPE 1
rc_receiver start -D /dev/tty-1
attitude_estimator_q start
position_estimator_inav start
mc_pos_control start
mc_att_control start
param set MC_YAW_P 12
param set MC_YAWRATE_P 0.08
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.08
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.001
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
sleep 1
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
sensors start
param set ATT_W_MAG 0.00
param set PE_MAG_NOISE 1.0f
param set SENS_BOARD_ROT 0
param set MPU_GYRO_LPF_ENUM 4
param set MPU_ACC_LPF_ENUM 4
param set MPU_SAMPLE_RATE_ENUM 2
param set GYRO0_XOFF -0.0028
param set GYRO0_YOFF -0.0047
param set GYRO0_ZOFF -0.0034
param set GYRO0_XSCALE 1.0000
param set GYRO0_YSCALE 1.0000
param set GYRO0_ZSCALE 1.0000
param set MAG0_XOFF 0.0000
param set MAG0_YOFF 0.0000
param set MAG0_ZOFF 0.0000
param set MAG0_XSCALE 1.0000
param set MAG0_YSCALE 1.0000
param set MAG0_ZSCALE 1.0000
param set ACC0_XOFF -0.0941
param set ACC0_YOFF 0.1168
param set ACC0_ZOFF -0.0398
param set ACC0_XSCALE 1.0004
param set ACC0_YSCALE 0.9974
param set ACC0_ZSCALE 0.9951
sleep 1
mpu9x50 start -D /dev/spi-1
param set UART_ESC_MODEL 2
param set UART_ESC_BAUDRATE 250000
param set UART_ESC_PX4MOTOR1 4
param set UART_ESC_PX4MOTOR2 2
param set UART_ESC_PX4MOTOR3 1
param set UART_ESC_PX4MOTOR4 3
sleep 1
uart_esc start -D /dev/tty-2
csr_gps start -D /dev/tty-3
list_devices
list_files
list_tasks
list_topics

8
posix-configs/eagle/flight/mainapp-calib.config Normal file
View File

@ -0,0 +1,8 @@
uorb start
muorb start
mavlink start -u 14556
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
mavlink boot_complete
commander start

7
posix-configs/eagle/flight/mainapp-flight.config Normal file
View File

@ -0,0 +1,7 @@
uorb start
muorb start
mavlink start -u 14556
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
mavlink boot_complete

56
posix-configs/eagle/flight/px4-calib.config Normal file
View File

@ -0,0 +1,56 @@
uorb 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 RC_RECEIVER_TYPE 1
rc_receiver start -D /dev/tty-1
attitude_estimator_q start
position_estimator_inav start
mc_pos_control start
mc_att_control start
sleep 1
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
sensors start
param set ATT_W_MAG 0.00
param set PE_MAG_NOISE 1.0f
param set MPU_GYRO_LPF_ENUM 4
param set MPU_ACC_LPF_ENUM 4
param set MPU_SAMPLE_RATE_ENUM 2
sleep 1
mpu9x50 start -D /dev/spi-1
uart_esc start -D /dev/tty-2
csr_gps start -D /dev/tty-3
list_devices
list_files
list_tasks
list_topics

58
posix-configs/eagle/flight/px4-flight.config Normal file
View File

@ -0,0 +1,58 @@
uorb 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
commander start
param set RC_RECEIVER_TYPE 1
rc_receiver start -D /dev/tty-1
attitude_estimator_q start
position_estimator_inav start
mc_pos_control start
mc_att_control start
sleep 1
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
sensors start
param set ATT_W_MAG 0.00
param set PE_MAG_NOISE 1.0f
param set MPU_GYRO_LPF_ENUM 4
param set MPU_ACC_LPF_ENUM 4
param set MPU_SAMPLE_RATE_ENUM 2
sleep 1
mpu9x50 start -D /dev/spi-1
uart_esc start -D /dev/tty-2
csr_gps start -D /dev/tty-3
pressure start -D /dev/i2c-2
list_devices
list_files
list_tasks
list_topics

8
posix-configs/eagle/hil/mainapphil.config Normal file
View File

@ -0,0 +1,8 @@
uorb start
muorb start
mavlink start -u 14556
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
mavlink boot_complete
simulator start -p

53
posix-configs/eagle/hil/px4.config Normal file
View File

@ -0,0 +1,53 @@
uorb 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
commander start -hil
sensors start
attitude_estimator_q start
position_estimator_inav start
mc_pos_control start
mc_att_control start
sleep 1
pwm_out_sim mode_pwm
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
param set MAV_TYPE 2
mixer load /dev/pwm_output0 /startup/quad_x.main.mix
list_devices
list_files
list_tasks
list_topics
sleep 10
list_tasks
sleep 10

40
src/firmware/posix/CMakeLists.txt
View File

@ -1,17 +1,20 @@
include_directories(${CMAKE_CURRENT_BINARY_DIR})
px4_posix_generate_builtin_commands(
OUT apps.h
MODULE_LIST ${module_libraries})
add_executable(mainapp
${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
apps.h
)
if ("${BOARD}" STREQUAL "eagle")
FASTRPC_STUB_GEN(../qurt/px4muorb.idl)
if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" AND NOT APPLE)
target_link_libraries(mainapp
QURT_APP(
APP_NAME mainapp
IDL_NAME px4muorb
APPS_DEST "/home/linaro"
SOURCES
${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
apps.h
LINK_LIBS
-Wl,--start-group
${module_libraries}
df_driver_framework
@ -20,10 +23,27 @@ if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" AND NOT APPLE)
-Wl,--end-group
)
else()
target_link_libraries(mainapp
${module_libraries}
pthread m
add_executable(mainapp
${CMAKE_SOURCE_DIR}/src/platforms/posix/main.cpp
apps.h
)
if (${CMAKE_C_COMPILER_ID} STREQUAL "Clang" AND NOT APPLE)
target_link_libraries(mainapp
-Wl,--start-group
${module_libraries}
df_driver_framework
${df_driver_libraries}
pthread m rt
-Wl,--end-group
)
else()
target_link_libraries(mainapp
${module_libraries}
df_driver_framework
${df_driver_libraries}
pthread m
)
endif()
endif()
add_custom_target(run_config

15
src/firmware/qurt/CMakeLists.txt
View File

@ -6,6 +6,8 @@ px4_qurt_generate_builtin_commands(
OUT ${CMAKE_BINARY_DIR}/apps.h
MODULE_LIST ${module_libraries})
FASTRPC_STUB_GEN(px4muorb.idl)
# Enable build without HexagonSDK to check link dependencies
if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
add_definitions(-DQURT_EXE_BUILD)
@ -13,12 +15,17 @@ if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
${CMAKE_SOURCE_DIR}/src/platforms/qurt/dspal/dspal_stub.c
${CMAKE_BINARY_DIR}/apps.h)
else()
QURT_BUNDLE(APP_NAME
mainapp
DSP_SOURCES
message("module_libraries = ${module_libraries}")
message("target_libraries = ${target_libraries}")
# Generate the DSP lib and stubs but not the apps side executable
# The Apps side executable is generated via the posix_eagle_xxxx target
QURT_LIB(APP_NAME mainapp
IDL_NAME px4muorb
SOURCES
${CMAKE_SOURCE_DIR}/src/platforms/qurt/dspal/dspal_stub.c
${CMAKE_BINARY_DIR}/apps.h
DSP_LINK_LIBS
LINK_LIBS
${module_libraries}
${target_libraries}
df_driver_framework

41
src/firmware/qurt/mainapp.idl
View File

@ -1,41 +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 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.
*
****************************************************************************/
#ifndef MAINAPP_IDL
#define MAINAPP_IDL
#include "AEEStdDef.idl"
interface mainapp{
uint32 test();
};
#endif /*MAINAPP_IDL*/

4
src/include/mavlink/mavlink_log.h
View File

@ -110,7 +110,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n"); } while(0);
/**
* Send a mavlink critical message and print to console.
*
@ -121,7 +121,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
fprintf(stderr, "telem> "); \
fprintf(stderr, _text, ##__VA_ARGS__); \
fprintf(stderr, "\n"); } while(0);
/**
* Send a mavlink emergency message and print to console.
*

32
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -400,6 +400,35 @@ void AttitudeEstimatorQ::task_main()
if (!_failsafe) {
uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
#ifdef __PX4_POSIX
perf_end(_perf_accel);
perf_end(_perf_mpu);
perf_end(_perf_mag);
#endif
if (_voter_gyro.failover_count() > 0) {
_failsafe = true;
flags = _voter_gyro.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!",
_voter_gyro.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
if (_voter_accel.failover_count() > 0) {
_failsafe = true;
flags = _voter_accel.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!",
_voter_accel.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
#ifdef __PX4_POSIX
perf_end(_perf_accel);
@ -607,8 +636,11 @@ void AttitudeEstimatorQ::task_main()
/* attitude quaternions for control state */
ctrl_state.q[0] = _q(0);
ctrl_state.q[1] = _q(1);
ctrl_state.q[2] = _q(2);
ctrl_state.q[3] = _q(3);
/* attitude rates for control state */

8
src/modules/commander/PreflightCheck.cpp
View File

@ -539,6 +539,14 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
}
}
#ifdef __PX4_QURT
// WARNING: Preflight checks are important and should be added back when
// all the sensors are supported
PX4_WARN("WARNING: Preflight checks PASS always.");
failed = false;
#endif
/* Report status */
return !failed;
}

19
src/modules/commander/commander.cpp
View File

@ -687,7 +687,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
}
if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
if ((hil_ret != TRANSITION_DENIED) && (arming_ret != TRANSITION_DENIED) && (main_ret != TRANSITION_DENIED)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@ -769,7 +769,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
(double)cmd->param1,
(double)cmd->param2,
(double)cmd->param3,
@ -1431,12 +1431,15 @@ int commander_thread_main(int argc, char *argv[])
pthread_attr_init(&commander_low_prio_attr);
pthread_attr_setstacksize(&commander_low_prio_attr, 2880);
#ifndef __PX4_QURT
struct sched_param param;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
#endif
pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
pthread_attr_destroy(&commander_low_prio_attr);
@ -1607,6 +1610,11 @@ int commander_thread_main(int argc, char *argv[])
_usb_telemetry_active = true;
}
/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
_usb_telemetry_active = true;
}
if (telemetry.heartbeat_time > 0) {
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
}
@ -2188,8 +2196,13 @@ int commander_thread_main(int argc, char *argv[])
}
/* RC input check */
#ifndef __PX4_QURT
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
(hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {
#else
// HACK: remove old data check due to timestamp issue in QURT
if (!status.rc_input_blocked && sp_man.timestamp != 0) {
#endif
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
@ -2969,7 +2982,7 @@ set_control_mode()
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
//control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
control_mode.flag_control_offboard_enabled = false;
switch (status.nav_state) {

11
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -993,6 +993,10 @@ void MulticopterPositionControl::control_auto(float dt)
}
if (current_setpoint_valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* scaled space: 1 == position error resulting max allowed speed */
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here
@ -1187,6 +1191,13 @@ MulticopterPositionControl::task_main()
poll_subscriptions();
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_yaw = euler_angles(2);
parameters_update(false);
hrt_abstime t = hrt_absolute_time();

5
src/modules/muorb/adsp/module.mk
View File

@ -41,6 +41,11 @@ ifeq ($(PX4_TARGET_OS),qurt)
SRCS = \
px4muorb.cpp \
uORBFastRpcChannel.cpp
SRCS = param/param.c
endif
endif
SRCS += perf_counter.c \
INCLUDE_DIRS += \
${PX4_BASE}/src/modules/uORB

6
src/modules/muorb/krait/CMakeLists.txt
View File

@ -30,9 +30,9 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
if("${DSPAL_STUBS_ENABLE}" STREQUAL "1")
include_directories(../krait-stubs)
endif()
#if("${DSPAL_STUBS_ENABLE}" STREQUAL "1")
# include_directories(../krait-stubs)
#endif()
px4_add_module(
MODULE modules__muorb__krait

26
src/modules/navigator/mission_block.cpp
View File

@ -417,6 +417,32 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
}
}
void
MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
{
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
/* use current position and use return altitude as clearance */
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
if (min_clearance > 0.0f) {
item->altitude += min_clearance;
}
item->altitude_is_relative = false;
item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->acceptance_radius = (_navigator->get_acceptance_radius() > min_clearance / 2.0f) ?
(min_clearance / 2) : _navigator->get_acceptance_radius();
item->time_inside = 0.0f;
item->pitch_min = min_pitch;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}
void
MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch)
{

12
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@ -81,6 +81,7 @@
#define MIN_VALID_W 0.00001f
#define PUB_INTERVAL 10000 // limit publish rate to 100 Hz
#define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s
#define MAX_WAIT_FOR_BARO_SAMPLE 3000000 // wait 3 secs for the baro to respond
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
@ -410,10 +411,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* wait for initial baro value */
bool wait_baro = true;
TerrainEstimator *terrain_estimator = new TerrainEstimator();
thread_running = true;
hrt_abstime baro_wait_for_sample_time = hrt_absolute_time();
while (wait_baro && !thread_should_exit) {
int ret = px4_poll(&fds_init[0], 1, 1000);
@ -421,14 +422,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) {
/* poll error */
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
PX4_WARN("INAV poll error");
} else if (ret > 0) {
} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
wait_baro = false;
mavlink_log_info(mavlink_fd, "[inav] timed out waiting for a baro sample");
}
else if (ret > 0) {
if (fds_init[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
baro_timestamp = sensor.baro_timestamp[0];
baro_wait_for_sample_time = hrt_absolute_time();
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {

15
src/modules/simulator/simulator_mavlink.cpp
View File

@ -530,8 +530,13 @@ void Simulator::pollForMAVLinkMessages(bool publish)
pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
pthread_attr_destroy(&sender_thread_attr);
mavlink_status_t udp_status = {};
mavlink_status_t serial_status = {};
// set the threads name
#ifdef __PX4_DARWIN
pthread_setname_np("sim_rcv");
#else
pthread_setname_np(pthread_self(), "sim_rcv");
#endif
bool sim_delay = false;
@ -575,9 +580,10 @@ void Simulator::pollForMAVLinkMessages(bool publish)
if (len > 0) {
mavlink_message_t msg;
mavlink_status_t status;
for (int i = 0; i < len; i++) {
if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &udp_status)) {
if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) {
// have a message, handle it
handle_message(&msg, publish);
}
@ -591,9 +597,10 @@ void Simulator::pollForMAVLinkMessages(bool publish)
if (len > 0) {
mavlink_message_t msg;
mavlink_status_t status;
for (int i = 0; i < len; ++i) {
if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) {
if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) {
// have a message, handle it
handle_message(&msg, true);
}

7
src/modules/systemlib/CMakeLists.txt
View File

@ -36,7 +36,6 @@ include_directories(${CMAKE_BINARY_DIR}/src/modules/param)
set(SRCS
perf_counter.c
param/param.c
conversions.c
cpuload.c
pid/pid.c
@ -57,8 +56,14 @@ if(${OS} STREQUAL "nuttx")
printload.c
up_cxxinitialize.c
)
elseif ("${CONFIG_SHMEM}" STREQUAL "1")
list(APPEND SRCS
param/param_shmem.c
print_load_posix.c
)
else()
list(APPEND SRCS
param/param.c
print_load_posix.c
)
endif()

8
src/modules/systemlib/bson/tinybson.c
View File

@ -54,15 +54,9 @@
#define CODER_CHECK(_c) do { if (_c->dead) { debug("coder dead"); return -1; }} while(0)
#define CODER_KILL(_c, _reason) do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0)
#ifdef __PX4_QURT
#define BSON_READ px4_read
#define BSON_WRITE px4_write
#define BSON_FSYNC px4_fsync
#else
#define BSON_READ read
#define BSON_WRITE write
#define BSON_FSYNC fsync
#endif
#define BSON_FSYNC px4_fsync
static int
read_x(bson_decoder_t decoder, void *p, size_t s)

2
src/modules/systemlib/circuit_breaker_params.c
View File

@ -42,6 +42,8 @@
* parameter needs to set to the key (magic).
*/
#ifdef __PX4_QURT
/**
* Circuit breaker for power supply check
*

1199
src/modules/systemlib/param/param_shmem.c Normal file
View File

File diff suppressed because it is too large Load Diff

4
src/modules/systemlib/system_params.c
View File

@ -37,6 +37,8 @@
* System wide parameters
*/
#ifdef __PX4_QURT
/**
* Auto-start script index.
*
@ -111,3 +113,5 @@ PARAM_DEFINE_INT32(SYS_COMPANION, 157600);
* @group System
*/
PARAM_DEFINE_INT32(SYS_PARAM_VER, 1);
#endif

24
src/modules/uORB/uORBDevices_posix.cpp
View File

@ -287,6 +287,7 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
case ORBIOCSETINTERVAL:
sd->update_interval = arg;
sd->last_update = hrt_absolute_time();
return PX4_OK;
case ORBIOCGADVERTISER:
@ -405,7 +406,6 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
* don't match then we might have a visible update.
*/
while (sd->generation != _generation) {
/*
* Handle non-rate-limited subscribers.
*/
@ -426,31 +426,17 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
break;
}
/*
* If the interval timer is still running, the topic should not
* appear updated, even though at this point we know that it has.
* We have previously been through here, so the subscriber
* must have collected the update we reported, otherwise
* update_reported would still be true.
*/
if (!hrt_called(&sd->update_call)) {
// If we have not yet reached the deadline, then assume that we can ignore any
// newly received data.
if (sd->last_update + sd->update_interval > hrt_absolute_time()) {
break;
}
/*
* Make sure that we don't consider the topic to be updated again
* until the interval has passed once more by restarting the interval
* timer and thereby re-scheduling a poll notification at that time.
*/
hrt_call_after(&sd->update_call,
sd->update_interval,
&uORB::DeviceNode::update_deferred_trampoline,
(void *)this);
/*
* Remember that we have told the subscriber that there is data.
*/
sd->update_reported = true;
sd->last_update = hrt_absolute_time();
ret = true;
break;

1
src/modules/uORB/uORBDevices_posix.hpp
View File

@ -111,6 +111,7 @@ private:
struct SubscriberData {
unsigned generation; /**< last generation the subscriber has seen */
unsigned update_interval; /**< if nonzero minimum interval between updates */
uint64_t last_update; /**< time at which the last update was provided, used when update_interval is nonzero */
struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */
void *poll_priv; /**< saved copy of fds->f_priv while poll is active */
bool update_reported; /**< true if we have reported the update via poll/check */

4
src/platforms/posix/drivers/barosim/baro.cpp
View File

@ -36,6 +36,8 @@
* Driver for the simulated barometric pressure sensor
*/
#define __STDC_FORMAT_MACROS
#include <inttypes.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_time.h>
@ -389,7 +391,7 @@ BAROSIM::devRead(void *buffer, size_t buflen)
int
BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg)
{
PX4_WARN("baro IOCTL %llu", hrt_absolute_time());
PX4_WARN("baro IOCTL %" PRIu64 , hrt_absolute_time());
switch (cmd) {

8
src/platforms/posix/px4_layer/CMakeLists.txt
View File

@ -30,6 +30,13 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
if("${CONFIG_SHMEM}" STREQUAL "1")
list(APPEND SHMEM_SRCS
shmem_posix.c
)
endif()
px4_add_module(
MODULE platforms__posix__px4_layer
COMPILE_FLAGS
@ -41,6 +48,7 @@ px4_add_module(
lib_crc32.c
drv_hrt.c
px4_log.c
${SHMEM_SRCS}
DEPENDS
platforms__common
)

3
src/platforms/posix/px4_layer/module.mk
View File

@ -41,5 +41,8 @@ SRCS = \
lib_crc32.c \
drv_hrt.c \
px4_log.c
ifeq ($(CONFIG_SHMEM), 1)
SRCS += shmem_posix.c
endif
MAXOPTIMIZATION = -Os

16
src/platforms/posix/px4_layer/px4_posix_impl.cpp
View File

@ -58,6 +58,16 @@ __BEGIN_DECLS
long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
#ifdef ENABLE_SHMEM
extern void init_params(void);
#endif
#ifdef ENABLE_SHMEM
extern void init_own_params(void);
extern unsigned int init_other_params(void);
extern unsigned int param_sync_done;
#endif
__END_DECLS
namespace px4
@ -72,6 +82,12 @@ void init_once(void)
work_queues_init();
hrt_work_queue_init();
hrt_init();
#ifdef ENABLE_SHMEM
PX4_INFO("Starting shared memory param sync\n");
init_own_params();
param_sync_done=init_other_params();
#endif
}
void init(int argc, char *argv[], const char *app_name)

270
src/platforms/posix/px4_layer/shmem_posix.c Normal file
View File

@ -0,0 +1,270 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#include <px4_defines.h>
#include <px4_posix.h>
#include <string.h>
#include <stdbool.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <errno.h>
#include <semaphore.h>
#include <sys/stat.h>
#include <drivers/drv_hrt.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include "systemlib/param/param.h"
#include <shmem.h>
#define SHMEM_DEBUG
int mem_fd;
unsigned char *map_base, *virt_addr;
struct shmem_info* shmem_info_p;
static void* map_memory(off_t target);
int get_shmem_lock(void);
void release_shmem_lock(void);
void init_shared_memory(void);
void copy_params_to_shmem(struct param_info_s *);
void update_to_shmem(param_t param, union param_value_u value);
int update_from_shmem(param_t param, union param_value_u *value);
uint64_t update_from_shmem_prev_time=0, update_from_shmem_current_time=0;
static unsigned char adsp_changed_index[MAX_SHMEM_PARAMS/8+1];
struct param_wbuf_s {
param_t param;
union param_value_u val;
bool unsaved;
};
extern struct param_wbuf_s * param_find_changed(param_t param);
#define MEMDEVICE "/dev/mem"
static void* map_memory(off_t target)
{
if((mem_fd = open(MEMDEVICE, O_RDWR | O_SYNC)) == -1)
{
PX4_ERR("Cannot open %s\n", MEMDEVICE);
exit(1);
}
/* Map one page */
map_base = (unsigned char*) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, target & ~MAP_MASK);
if(map_base == (void *) -1)
{
PX4_ERR("Cannot mmap /dev/atl_mem\n");
exit(1);
}
return (map_base + (target & MAP_MASK) + LOCK_SIZE);
}
int get_shmem_lock(void)
{
int i=0;
/*ioctl calls cmpxchg*/
while(ioctl(mem_fd, LOCK_MEM)!=0)
{
PX4_INFO("Could not get lock, spinning\n");
usleep(100000); //sleep for 100 msec
i++;
if(i>100) break;
}
if(i>100) return -1;
return 0; //got the lock
}
void release_shmem_lock(void)
{
*(virt_addr-LOCK_SIZE)=1;
}
void init_shared_memory(void)
{
virt_addr = map_memory(MAP_ADDRESS); //16K space
shmem_info_p = (struct shmem_info*)virt_addr;
//PX4_INFO("linux memory mapped\n");
}
void copy_params_to_shmem(struct param_info_s *param_info_base)
{
param_t param;
unsigned int i;
if(get_shmem_lock()!=0) {
PX4_ERR("Could not get shmem lock\n");
return;
}
//PX4_INFO("%d krait params allocated\n", param_count());
for (param = 0; param<param_count(); param++) {
struct param_wbuf_s *s = param_find_changed(param);
if(s==NULL) shmem_info_p->params_val[param] = param_info_base[param].val;
else shmem_info_p->params_val[param] = s->val;
#ifdef SHMEM_DEBUG
if(param_type(param)==PARAM_TYPE_INT32){
{PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param));}
}
else if(param_type(param)==PARAM_TYPE_FLOAT){
{PX4_INFO("%d: written %f for param %s to shared mem", param, (double)shmem_info_p->params_val[param].f, param_name(param));}
}
#endif
}
//PX4_INFO("written %u params to shmem offset %lu\n", param_count(), (unsigned char*)&shmem_info_p->params_count-(unsigned char*)shmem_info_p);
for(i=0;i<MAX_SHMEM_PARAMS/8+1;i++)
{
shmem_info_p->krait_changed_index[i]=0;
adsp_changed_index[i]=0;
}
release_shmem_lock();
}
/*update value and param's change bit in shared memory*/
void update_to_shmem(param_t param, union param_value_u value)
{
unsigned int byte_changed, bit_changed;
if(get_shmem_lock()!=0) {
fprintf(stderr, "Could not get shmem lock\n");
return;
}
shmem_info_p->params_val[param] = value;
byte_changed = param/8;
bit_changed = 1 << param%8;
shmem_info_p->krait_changed_index[byte_changed]|=bit_changed;
//PX4_INFO("set %d bit on krait changed index[%d] to %d\n", bit_changed, byte_changed, shmem_info_p->krait_changed_index[byte_changed]);
#ifdef SHMEM_DEBUG
if(param_type(param)==PARAM_TYPE_INT32)
{PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d\n", value.i, param_name(param), byte_changed, bit_changed);}
else if(param_type(param)==PARAM_TYPE_FLOAT)
{PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d\n", (double)value.f, param_name(param), byte_changed, bit_changed);}
#endif
release_shmem_lock();
}
static void update_index_from_shmem(void)
{
unsigned int i;
if(get_shmem_lock()!=0) {
fprintf(stderr, "Could not get shmem lock\n");
return;
}
//PX4_INFO("Updating index from shmem\n");
for(i=0;i<MAX_SHMEM_PARAMS/8+1;i++)
adsp_changed_index[i]=shmem_info_p->adsp_changed_index[i];
release_shmem_lock();
}
static void update_value_from_shmem(param_t param, union param_value_u *value)
{
unsigned int byte_changed, bit_changed;
if(get_shmem_lock()!=0) {
fprintf(stderr, "Could not get shmem lock\n");
return;
}
*value = shmem_info_p->params_val[param];
/*also clear the index since we are holding the lock*/
byte_changed = param/8;
bit_changed = 1 << param%8;
shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed;
release_shmem_lock();
#ifdef SHMEM_DEBUG
if(param_type(param)==PARAM_TYPE_INT32)
{PX4_INFO("Got value %d for param %s from shmem, cleared adsp index %d:%d\n", value->i, param_name(param), byte_changed, bit_changed);}
else if(param_type(param)==PARAM_TYPE_FLOAT)
{PX4_INFO("Got value %f for param %s from shmem, cleared adsp index %d:%d\n", (double)value->f, param_name(param), byte_changed, bit_changed);}
#endif
}
int update_from_shmem(param_t param, union param_value_u *value)
{
unsigned int byte_changed, bit_changed;
unsigned int retval = 0;
update_from_shmem_current_time = hrt_absolute_time();
if((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) //update every 1 second
{
update_from_shmem_prev_time = update_from_shmem_current_time;
update_index_from_shmem();
}
byte_changed = param/8;
bit_changed = 1 << param%8;
if(adsp_changed_index[byte_changed] & bit_changed)
{
update_value_from_shmem(param, value);
adsp_changed_index[byte_changed] &= ~bit_changed; //clear the bit
retval = 1;
}
//else {PX4_INFO("no change to param %s\n", param_name(param));}
//PX4_INFO("%s %d bit on adsp index[%d]\n", (retval)?"cleared":"unchanged", bit_changed, byte_changed);
return retval;
}

2
src/platforms/posix/tests/muorb/muorb_test_main.cpp
View File

@ -43,7 +43,7 @@
#include "muorb_test_example.h"
#include <stdio.h>
#include "uORB/uORBManager.hpp"
#include "uORBKraitFastRpcChannel.hpp"
#include "../../../modules/muorb/krait/uORBKraitFastRpcChannel.hpp"
int PX4_MAIN(int argc, char **argv)
{

17
src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
View File

@ -61,10 +61,12 @@ static int writer_main(int argc, char *argv[])
int fd = px4_open(TESTDEV, PX4_F_WRONLY);
int fd = px4_open(TESTDEV, PX4_F_WRONLY);
if (fd < 0) {
PX4_INFO("Writer: Open failed %d %d", fd, px4_errno);
return -px4_errno;
}
src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp
int ret;
int i = 0;
@ -92,6 +94,12 @@ static int writer_main(int argc, char *argv[])
class PrivData
{
PrivData() : _read_offset(0) {}
~PrivData() {}
size_t _read_offset;
};
public:
PrivData() : _read_offset(0) {}
~PrivData() {}
@ -321,12 +329,16 @@ int VCDevExample::main()
(char *const *)NULL);
ret = 0;
PX4_INFO("TEST: BLOCKING POLL ---------------");
if (do_poll(fd, -1, 3, 0)) {
ret = 1;
goto fail2;
PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
if(do_poll(fd, 0, 3, 0)) {
ret = 1;
goto fail2;
goto fail2;
}
PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
@ -348,8 +360,7 @@ int VCDevExample::main()
if (do_poll(fd, 1000, 3, 0)) {
ret = 1;
goto fail2;
}
PX4_INFO("TEST: waiting for writer to stop");
fail2:
g_exit = true;
px4_close(fd);

10
src/platforms/px4_posix.h
View File

@ -77,10 +77,20 @@ typedef sem_t px4_sem_t;
#define px4_sem_init sem_init
#define px4_sem_wait sem_wait
#define px4_sem_post sem_post
#define px4_sem_getvalue sem_getvalue
#define px4_sem_destroy sem_destroy
#if 0
// TODO: Implement this function or remove it from the implementation.
// #define px4_sem_timedwait sem_timedwait
inline int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *abstime)
{
return -1;
}
#endif
#ifdef __PX4_QURT
__EXPORT int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *abstime);
#else

15
src/platforms/qurt/px4_layer/CMakeLists.txt
View File

@ -31,6 +31,8 @@
#
############################################################################
include_directories(${HEXAGON_SDK_ROOT}/lib/common/qurt/ADSPv5MP/include)
set(QURT_LAYER_SRCS
px4_qurt_impl.cpp
px4_qurt_tasks.cpp
@ -39,6 +41,7 @@ set(QURT_LAYER_SRCS
qurt_stubs.c
main.cpp
params.c
shmem_qurt.c
)
if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
list(APPEND QURT_LAYER_SRCS
@ -47,18 +50,6 @@ if ("${QURT_ENABLE_STUBS}" STREQUAL "1")
)
endif()
# For Eagle, the commands are specific to the build config label
# e.g. config_qurt_eagle_hil uses commands_hil.c
if ("${BOARD}" STREQUAL "eagle")
# The CI test target can use the hil commands
if ("${LABEL}" STREQUAL "travis")
set(CONFIG_SRC commands_hil.c)
else()
set(CONFIG_SRC commands_${LABEL}.c)
endif()
endif()
px4_add_module(
MODULE platforms__qurt__px4_layer

45
src/platforms/qurt/px4_layer/commands_hello.c
View File

@ -1,45 +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 commands_hello.c
* Commands to run for the "qurt_hello" config
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
const char *get_commands()
{
static const char *commands = "hello start";
return commands;
}

80
src/platforms/qurt/px4_layer/commands_muorb.c
View File

@ -1,80 +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 commands_muorb_test.c
* Commands to run for the "qurt_muorb_test" config
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
const char *get_commands()
{
static const char *commands =
"uorb start\n"
"muorb_test start\n";
/*
"hil mode_pwm\n"
"mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n";
*/
/*
"param show\n"
"param set CAL_GYRO_ID 2293760\n"
"param set CAL_ACC0_ID 1310720\n"
"param set CAL_ACC1_ID 1376256\n"
"param set CAL_MAG0_ID 196608\n"
"gyrosim start\n"
"accelsim start\n"
"rgbled start\n"
"tone_alarm start\n"
"simulator start -s\n"
"commander start\n"
"sensors start\n"
"ekf_att_pos_estimator start\n"
"mc_pos_control start\n"
"mc_att_control start\n"
"param set MAV_TYPE 2\n"
"param set RC1_MAX 2015\n"
"param set RC1_MIN 996\n"
"param set RC_TRIM 1502\n"
*/
return commands;
/*====================================== Working set
======================================*/
//"muorb_test start\n"
//"gyrosim start\n"
//"adcsim start\n"
}

175
src/platforms/qurt/px4_layer/commands_release.c
View File

@ -1,175 +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 commands_muorb_test.c
* Commands to run for the "qurt_muorb_test" config
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
const char *get_commands()
{
static const char *commands =
"uorb start\n"
"param set CAL_GYRO0_ID 2293760\n"
"param set CAL_ACC0_ID 1310720\n"
"param set CAL_ACC1_ID 1376256\n"
"param set CAL_MAG0_ID 196608\n"
"commander start\n"
"param set RC_RECEIVER_TYPE 1\n"
"rc_receiver start -D /dev/tty-1\n"
"attitude_estimator_q start\n"
"position_estimator_inav start\n"
//"ekf_att_pos_estimator start\n"
"mc_pos_control start\n"
"mc_att_control start\n"
"sleep 1\n"
// Channel mapping for Spektrum DX8
"param set RC_MAP_THROTTLE 1\n"
"param set RC_MAP_ROLL 2\n"
"param set RC_MAP_PITCH 3\n"
"param set RC_MAP_YAW 4\n"
"param set RC_MAP_MODE_SW 5\n"
"param set RC_MAP_POSCTL_SW 6\n"
// RC calibration for Spektrum DX8
"param set RC1_MAX 852\n"
"param set RC1_MIN 171\n"
"param set RC1_TRIM 171\n"
"param set RC1_REV 1\n"
"param set RC2_MAX 852\n"
"param set RC2_MIN 171\n"
"param set RC2_TRIM 512\n"
"param set RC2_REV -1\n"
"param set RC3_MAX 852\n"
"param set RC3_MIN 171\n"
"param set RC3_TRIM 512\n"
"param set RC3_REV 1\n"
"param set RC4_MAX 852\n"
"param set RC4_MIN 171\n"
"param set RC4_TRIM 514\n"
"param set RC4_REV -1\n"
"param set RC5_MAX 852\n"
"param set RC5_MIN 171\n"
"param set RC5_TRIM 512\n"
"param set RC5_REV 1\n"
"param set RC6_MAX 852\n"
"param set RC6_MIN 171\n"
"param set RC6_TRIM 171\n"
"param set RC6_REV 1\n"
// // RC calibration for DX6i
// "param set RC1_MAX 2015\n"
// "param set RC1_MIN 996\n"
// "param set RC1_TRIM 1502\n"
// "param set RC1_REV -1\n"
// "param set RC2_MAX 2016 \n"
// "param set RC2_MIN 995\n"
// "param set RC2_TRIM 1500\n"
// "param set RC3_MAX 2003\n"
// "param set RC3_MIN 992\n"
// "param set RC3_TRIM 992\n"
// "param set RC4_MAX 2011\n"
// "param set RC4_MIN 997\n"
// "param set RC4_TRIM 1504\n"
// "param set RC4_REV -1\n"
// "param set RC6_MAX 2016\n"
// "param set RC6_MIN 992\n"
// "param set RC6_TRIM 1504\n"
// "param set RC_CHAN_CNT 8\n"
// "param set RC_MAP_MODE_SW 5\n"
// "param set RC_MAP_POSCTL_SW 7\n"
// "param set RC_MAP_RETURN_SW 8\n"
"sensors start\n"
"param set MC_YAW_P 7.0\n"
"param set MC_YAWRATE_P 0.1125\n"
"param set MC_YAWRATE_I 0.0\n"
"param set MC_YAWRATE_D 0\n"
"param set MC_PITCH_P 6.0\n"
"param set MC_PITCHRATE_P 0.125\n"
"param set MC_PITCHRATE_I 0.0\n"
"param set MC_PITCHRATE_D 0.0\n"
"param set MC_ROLL_P 6.0\n"
"param set MC_ROLLRATE_P 0.1125\n"
"param set MC_ROLLRATE_I 0.0\n"
"param set MC_ROLLRATE_D 0.0\n"
"param set ATT_W_MAG 0.00\n"
"param set PE_MAG_NOISE 1.0f\n" // ekf_att_pos only
"param set SENS_BOARD_ROT 6\n"
"param set CAL_GYRO0_XOFF 0.0\n"
"param set CAL_GYRO0_YOFF 0.0\n"
"param set CAL_GYRO0_ZOFF 0.0\n"
"param set CAL_GYRO0_XSCALE 1.000000\n"
"param set CAL_GYRO0_YSCALE 1.000000\n"
"param set CAL_GYRO0_ZSCALE 1.000000\n"
"param set CAL_ACC0_XOFF 0.0\n"
"param set CAL_ACC0_YOFF 0.0\n"
"param set CAL_ACC0_ZOFF 0.0\n"
"param set CAL_ACC0_XSCALE 1.0\n"
"param set CAL_ACC0_YSCALE 1.0\n"
"param set CAL_ACC0_ZSCALE 1.0\n"
// "param set CAL_ACC0_XOFF 0.064189\n"
// "param set CAL_ACC0_YOFF 0.153990\n"
// "param set CAL_ACC0_ZOFF -0.000567\n"
"param set MPU_GYRO_LPF_ENUM 4\n"
"param set MPU_ACC_LPF_ENUM 4\n"
"param set MPU_SAMPLE_RATE_ENUM 2\n"
"sleep 1\n"
"mpu9x50 start -D /dev/spi-1\n"
"uart_esc start -D /dev/tty-2\n"
"csr_gps start -D /dev/tty-3\n"
"param set MAV_TYPE 2\n"
"list_devices\n"
"list_files\n"
"list_tasks\n"
"list_topics\n"
;
return commands;
}
/*
simulator numbers
"param set MC_YAW_P 1.5\n"
"param set MC_PITCH_P 3.0\n"
"param set MC_ROLL_P 3.0\n"
"param set MC_YAWRATE_P 0.2\n"
"param set MC_PITCHRATE_P 0.03\n"
"param set MC_ROLLRATE_P 0.03\n"
*/

68
src/platforms/qurt/px4_layer/commands_test.c
View File

@ -1,68 +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 commands_default.c
* Commands to run for the "qurt_default" config
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
const char *get_commands()
{
static const char *commands =
"hello start\n"
"uorb start\n"
"simulator start -s\n"
"barosim start\n"
"adcsim start\n"
"accelsim start\n"
"gyrosim start\n"
"list_devices\n"
"list_topics\n"
"list_tasks\n"
"param show *\n"
"rgbled start\n"
#if 0
"sensors start\n"
"param set CAL_GYRO0_ID 2293760\n"
"param set CAL_ACC0_ID 1310720\n"
"hil mode_pwm"
"param set CAL_ACC1_ID 1376256\n"
"param set CAL_MAG0_ID 196608\n"
"mavlink start -d /tmp/ttyS0\n"
"commander start\n"
#endif
;
return commands;
}

55
src/platforms/qurt/px4_layer/main.cpp
View File

@ -46,7 +46,7 @@
#include <string>
#include <map>
#include <stdio.h>
#include <apps.h>
#include <stdlib.h>
#include "get_commands.h"
using namespace std;
@ -67,24 +67,28 @@ static void run_cmd(map<string, px4_main_t> &apps, const vector<string> &appargs
// command is appargs[0]
string command = appargs[0];
if (apps.find(command) != apps.end()) {
//replaces app.find with iterator code to avoid null pointer exception
for (map<string,px4_main_t>::iterator it=apps.begin(); it!=apps.end(); ++it)
if (it->first == command) {
const char *arg[2 + 1];
unsigned int i = 0;
while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
arg[i] = (char *)appargs[i].c_str();
PX4_WARN(" arg = '%s'\n", arg[i]);
PX4_WARN(" arg%d = '%s'\n", i, arg[i]);
++i;
}
arg[i] = (char *)0;
//PX4_DEBUG_PRINTF(i);
apps[command](i, (char **)arg);
if(apps[command]==NULL)
PX4_ERR("Null function !!\n");
else {
apps[command](i, (char **)arg);
break;
}
} else {
PX4_WARN("NOT FOUND.");
list_builtins(apps);
}
}
@ -104,6 +108,7 @@ static void process_commands(map<string, px4_main_t> &apps, const char *cmds)
const char *b = cmds;
char arg[256];
// Eat leading whitespace
eat_whitespace(b, i);
@ -159,6 +164,42 @@ int dspal_main(int argc, char *argv[]);
__END_DECLS
#define COMMANDS_ADSP_FILE "/dev/fs/px4.config"
const char *get_commands()
{
int fd = open(COMMANDS_ADSP_FILE, O_RDONLY);
if(fd>0)
{
static char *commands;
char buf[4096];
int bytes_read, total_bytes=0;
PX4_INFO("reading commands from %s\n", COMMANDS_ADSP_FILE);
do{
bytes_read = read(fd, (void*)buf, sizeof(buf));
if(bytes_read>0)
{
commands=(char*)realloc(commands, total_bytes+bytes_read);
memcpy(commands+total_bytes, buf, bytes_read);
total_bytes += bytes_read;
}
}while((unsigned int)bytes_read>0);
close(fd);
return (const char*)commands;
}
PX4_ERR("Could not open %s\n", COMMANDS_ADSP_FILE);
static const char *commands =
"uorb start\n"
;
return commands;
}
int dspal_entry(int argc, char *argv[])
{
PX4_INFO("In main\n");

21
src/platforms/qurt/px4_layer/module.mk
View File

@ -44,23 +44,8 @@ SRCS = \
px4_qurt_tasks.cpp \
lib_crc32.c \
drv_hrt.c \
qurt_stubs.c \
main.cpp
ifeq ($(CONFIG),qurt_hello)
SRCS += commands_hello.c
endif
ifeq ($(CONFIG),qurt_default)
SRCS += commands_default.c
endif
ifeq ($(CONFIG),qurt_muorb_test)
SRCS += commands_muorb_test.c
endif
ifeq ($(CONFIG),qurt_hil)
SRCS += commands_hil.c
endif
ifeq ($(CONFIG),qurt_adsp)
SRCS += commands_adsp.c
endif
qurt_stubs.c \
main.cpp \
shmem_qurt.c
MAXOPTIMIZATION = -Os

7
src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
View File

@ -71,6 +71,9 @@ unsigned int sleep(unsigned int sec)
}
extern void hrt_init(void);
extern void init_own_params();
extern unsigned int init_other_params();
extern unsigned int param_sync_done;
#if 0
void qurt_log(const char *fmt, ...)
@ -108,6 +111,10 @@ void init_once(void)
hrt_work_queue_init();
hrt_init();
PX4_WARN("after calling hrt_init");
/*Shared memory param sync*/
init_own_params();
param_sync_done=init_other_params();
}
void init(int argc, char *argv[], const char *app_name)

2
src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
View File

@ -106,7 +106,7 @@ static void *entry_adapter(void *ptr)
void
px4_systemreset(bool to_bootloader)
{
PX4_WARN("Called px4_system_reset");
PX4_WARN("Called px4_system_reset but NOT yet implemented.");
}
px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,

260
src/platforms/qurt/px4_layer/shmem_qurt.c Normal file
View File

@ -0,0 +1,260 @@
/****************************************************************************
*
* Copyright (c) 2015 Vijay Venkatraman. 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_defines.h>
#include <string.h>
#include <stdbool.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/stat.h>
#include <atomic_ops.h>
#include "systemlib/param/param.h"
#include <shmem.h>
#include <drivers/drv_hrt.h>
#define SHMEM_DEBUG
int mem_fd;
unsigned char *map_base, *virt_addr;
struct shmem_info* shmem_info_p;
static void* map_memory(off_t target);
int get_shmem_lock(void);
void release_shmem_lock(void);
void init_shared_memory(void);
void copy_params_to_shmem(struct param_info_s *);
void update_to_shmem(param_t param, union param_value_u value);
int update_from_shmem(param_t param, union param_value_u *value);
uint64_t update_from_shmem_prev_time=0, update_from_shmem_current_time=0;
static unsigned char krait_changed_index[MAX_SHMEM_PARAMS/8+1];
struct param_wbuf_s {
param_t param;
union param_value_u val;
bool unsaved;
};
extern struct param_wbuf_s * param_find_changed(param_t param);
static void* map_memory(off_t target)
{
return (void*)(target+LOCK_SIZE);
}
int get_shmem_lock(void)
{
unsigned char *lock = (unsigned char*)(MAP_ADDRESS+LOCK_OFFSET);
unsigned int i=0;
while(!atomic_compare_and_set(lock, 1, 0))
{
PX4_INFO("Could not get lock. spinning\n");
i++;
usleep(1000);
if(i>100) break;
}
if(i>100) return -1;
return 0; //got the lock
}
void release_shmem_lock(void)
{
unsigned char *lock = (unsigned char*)(MAP_ADDRESS+LOCK_OFFSET);
*lock=1;
return;
}
void init_shared_memory(void)
{
//PX4_INFO("Value at lock address is %d\n", *(unsigned int*)0xfbfc000);
virt_addr = map_memory(MAP_ADDRESS);
shmem_info_p = (struct shmem_info*)virt_addr;
//PX4_INFO("adsp memory mapped\n");
}
void copy_params_to_shmem(struct param_info_s *param_info_base)
{
param_t param;
unsigned int i;
if(get_shmem_lock()!=0) {
PX4_INFO("Could not get shmem lock\n");
return;
}
//else PX4_INFO("Got lock\n");
for (param = 0; param<param_count(); param++) {
//{PX4_INFO("writing to offset %d\n", (unsigned char*)(shmem_info_p->adsp_params[param].name)-(unsigned char*)shmem_info_p);}
struct param_wbuf_s *s = param_find_changed(param);
if(s==NULL) shmem_info_p->params_val[param] = param_info_base[param].val;
else shmem_info_p->params_val[param] = s->val;
#ifdef SHMEM_DEBUG
if(param_type(param)==PARAM_TYPE_INT32){
PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param));}
else if(param_type(param)==PARAM_TYPE_FLOAT){
PX4_INFO("%d: written %f for param %s to shared mem", param, shmem_info_p->params_val[param].f, param_name(param));}
#endif
}
for(i=0;i<MAX_SHMEM_PARAMS/8+1;i++)
{
shmem_info_p->adsp_changed_index[i]=0;
krait_changed_index[i]=0;
}
release_shmem_lock();
//PX4_INFO("Released lock\n");
}
/*update value and param's change bit in shared memory*/
void update_to_shmem(param_t param, union param_value_u value)
{
unsigned int byte_changed, bit_changed;
if(!handle_in_range(param))
return;
if(get_shmem_lock()!=0) {
PX4_ERR("Could not get shmem lock\n");
return;
}
shmem_info_p->params_val[param] = value;
byte_changed = param/8;
bit_changed = 1 << param%8;
shmem_info_p->adsp_changed_index[byte_changed]|=bit_changed;
//PX4_INFO("set %d bit on adsp index[%d] to %d\n", bit_changed, byte_changed, shmem_info_p->adsp_changed_index[byte_changed]);
#ifdef SHMEM_DEBUG
if(param_type(param)==PARAM_TYPE_INT32)
{PX4_INFO("Set value %d for param %s to shmem, set adsp index %d:%d\n", value.i, param_name(param), byte_changed, bit_changed);}
else if(param_type(param)==PARAM_TYPE_FLOAT)
{PX4_INFO("Set value %f for param %s to shmem, set adsp index %d:%d\n", value.f, param_name(param), byte_changed, bit_changed);}
#endif
release_shmem_lock();
}
static void update_index_from_shmem(void)
{
unsigned int i;
if(get_shmem_lock()!=0) {
PX4_ERR("Could not get shmem lock\n");
return;
}
//PX4_INFO("Updating index from shmem\n");
for(i=0;i<MAX_SHMEM_PARAMS/8+1;i++)
krait_changed_index[i]=shmem_info_p->krait_changed_index[i];
release_shmem_lock();
}
static void update_value_from_shmem(param_t param, union param_value_u *value)
{
unsigned int byte_changed, bit_changed;
if(get_shmem_lock()!=0) {
PX4_ERR("Could not get shmem lock\n");
return;
}
*value = shmem_info_p->params_val[param];
/*also clear the index since we are holding the lock*/
byte_changed = param/8;
bit_changed = 1 << param%8;
shmem_info_p->krait_changed_index[byte_changed] &= ~bit_changed;
release_shmem_lock();
#ifdef SHMEM_DEBUG
if(param_type(param)==PARAM_TYPE_INT32)
{PX4_INFO("Got value %d for param %s from shmem, cleared krait index %d:%d\n", value->i, param_name(param), byte_changed, bit_changed);}
else if(param_type(param)==PARAM_TYPE_FLOAT)
{PX4_INFO("Got value %f for param %s from shmem, cleared krait index %d:%d\n", value->f, param_name(param), byte_changed, bit_changed);}
#endif
}
int update_from_shmem(param_t param, union param_value_u *value)
{
unsigned int byte_changed, bit_changed;
unsigned int retval = 0;
if(!handle_in_range(param) || value==NULL)
return retval;
update_from_shmem_current_time = hrt_absolute_time();
if((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) //update every 1 second
{
update_from_shmem_prev_time = update_from_shmem_current_time;
update_index_from_shmem();
}
byte_changed = param/8;
bit_changed = 1 << param%8;
if(krait_changed_index[byte_changed] & bit_changed)
{
update_value_from_shmem(param, value);
krait_changed_index[byte_changed] &= ~bit_changed;
retval = 1;
}
//else {PX4_INFO("no change to param %s\n", param_name(param));}
//PX4_INFO("%s %d bit on krait changed index[%d]\n", (retval)?"cleared":"unchanged", bit_changed, byte_changed);
return retval;
}

3
src/platforms/qurt/stubs/stubs_qurt.c
View File

@ -2,6 +2,8 @@
#include <qurt_log.h>
#include <dspal_platform.h>
#ifdef CONFIG_ARCH_BOARD_EAGLE
void HAP_debug(const char *msg, int level, const char *filename, int line)
{
}
@ -14,3 +16,4 @@ int dlinit(int a, char **b)
{
return 1;
}
#endif

2
src/platforms/qurt/tests/muorb/muorb_test_example.cpp
View File

@ -88,7 +88,7 @@ int MuorbTestExample::DefaultTest()
int i = 0;
pwm.error_count++;
sc.gyro_errcount++;
sc.gyro_errcount[i]++;
while (!appState.exitRequested() && i < 10) {

41
src/platforms/qurt/px4_layer/commands_default.c → src/platforms/shmem.h
View File

@ -1,6 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
* 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
@ -30,28 +31,30 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file commands_default.c
* Commands to run for the "qurt_eagle_default" config
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include "get_commands.h"
#define MAX_SHMEM_PARAMS 3850 //MAP_SIZE - (LOCK_SIZE - sizeof(struct shmem_info))
const char *get_commands()
struct shmem_info
{
union param_value_u params_val[MAX_SHMEM_PARAMS];
unsigned char krait_changed_index[MAX_SHMEM_PARAMS/8+1]; /*bit map of all params changed by krait*/
unsigned char adsp_changed_index[MAX_SHMEM_PARAMS/8+1]; /*bit map of all params changed by adsp*/
static const char *commands =
"uorb start\n"
"param set CAL_GYRO0_ID 2293760\n"
"param set CAL_ACC0_ID 1310720\n"
"param set CAL_ACC1_ID 1376256\n"
"param set CAL_MAG0_ID 196608\n"
"commander start\n"
#ifdef __PX4_NUTTX
};
#else
}__attribute__((packed));
#endif
;
#define MAP_ADDRESS 0xfbfc000
#define MAP_SIZE 16384
#define MAP_MASK (MAP_SIZE - 1)
return commands;
#define LOCK_OFFSET 0
#define LOCK_SIZE 4
#define LOCK_MEM 1
#define UNLOCK_MEM 2
}
#define TYPE_MASK 0x1
extern bool handle_in_range(param_t);

22
src/systemcmds/mixer/mixer.cpp
View File

@ -108,7 +108,6 @@ load(const char *devname, const char *fname)
usleep(20000);
int dev;
char buf[2048];
/* open the device */
if ((dev = px4_open(devname, 0)) < 0) {
@ -122,6 +121,9 @@ load(const char *devname, const char *fname)
return 1;
}
#ifndef __PX4_QURT
char buf[2048];
if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) {
warnx("can't load mixer: %s", fname);
return 1;
@ -129,6 +131,24 @@ load(const char *devname, const char *fname)
/* XXX pass the buffer to the device */
int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);
#else
char newbuf[] =
"R: 4x 10000 10000 10000 0\n"
"M: 1\n"
"O: 10000 10000 0 -10000 10000\n"
"S: 0 4 10000 10000 0 -10000 10000\n"
"M: 1\n"
"O: 10000 10000 0 -10000 10000\n"
"S: 0 5 10000 10000 0 -10000 10000\n"
"M: 1\n"
"O: 10000 10000 0 -10000 10000\n"
"S: 0 6 10000 10000 0 -10000 10000\n"
"M: 1\n"
"O: 10000 10000 0 -10000 10000\n"
"S: 0 7 10000 10000 0 -10000 10000\n";
/* XXX pass the buffer to the device */
int ret = px4_ioctl(dev, MIXERIOCLOADBUF, (unsigned long)newbuf);
#endif
if (ret < 0) {
warnx("error loading mixers from %s", fname);