包含全部源代码和文档（修复子模块问题） Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>luogang_branch
parent
546e8424d4
commit
56bd9ec6e7
@ -1 +0,0 @@
|
||||
Subproject commit aafa277b97249517a2324fb87edf36e9893da198
|
||||
@ -0,0 +1,21 @@
|
||||
# Python
|
||||
__pycache__/
|
||||
*.pyc
|
||||
*.pyo
|
||||
*.egg-info/
|
||||
venv/
|
||||
|
||||
# Database
|
||||
zhitu.db
|
||||
*.db
|
||||
|
||||
# Audio data (large files)
|
||||
*.wav
|
||||
|
||||
# Generated results
|
||||
results/
|
||||
|
||||
# IDE
|
||||
.idea/
|
||||
.vscode/
|
||||
*.iml
|
||||
@ -0,0 +1,2 @@
|
||||
# software
|
||||
|
||||
@ -0,0 +1,11 @@
|
||||
{
|
||||
"created_time": "2026-04-20T02:11:45Z",
|
||||
"files": [
|
||||
],
|
||||
"folders": [
|
||||
],
|
||||
"id": "1542",
|
||||
"modified_time": "2026-04-20T02:11:45Z",
|
||||
"signature": "4031563627892149089",
|
||||
"version": 3
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
@ -0,0 +1,4 @@
|
||||
# Mark binary files
|
||||
*.onnx binary
|
||||
*.bin binary
|
||||
*.wav binary
|
||||
@ -0,0 +1,42 @@
|
||||
# Build artifacts
|
||||
build/
|
||||
build/*.o
|
||||
build/*.obj
|
||||
build/*.exe
|
||||
CMakeCache.txt
|
||||
CMakeFiles/
|
||||
cmake-build-*/
|
||||
Makefile
|
||||
|
||||
# IDE
|
||||
.vs/
|
||||
.vscode/
|
||||
*.user
|
||||
*.suo
|
||||
*.sln
|
||||
*.vcxproj*
|
||||
|
||||
# Python cache
|
||||
scripts/__pycache__/
|
||||
*.pyc
|
||||
*.pyo
|
||||
__pycache__/
|
||||
|
||||
# Temporary files
|
||||
*.tmp
|
||||
*.log
|
||||
*.swp
|
||||
*.bak
|
||||
|
||||
# OS
|
||||
.DS_Store
|
||||
Thumbs.db
|
||||
|
||||
# Model training artifacts
|
||||
checkpoints/
|
||||
*.pth
|
||||
runs/
|
||||
|
||||
# Output files
|
||||
output/*.wav
|
||||
output/*.txt
|
||||
@ -0,0 +1,221 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
project(acoustic_analyzer)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
# ── Windows / MinGW fixes ────────────────────────────────────────
|
||||
if(WIN32)
|
||||
add_compile_definitions(_USE_MATH_DEFINES)
|
||||
endif()
|
||||
if(CMAKE_CXX_COMPILER_ID MATCHES "GNU")
|
||||
add_compile_options("-Wa,-mbig-obj")
|
||||
endif()
|
||||
|
||||
option(BUILD_TESTS "Build unit tests" ON)
|
||||
option(BUILD_ROS_WRAPPER "Build ROS wrapper node" OFF)
|
||||
option(BUILD_ONNX_TESTS "Build ONNX classifier tests" ON)
|
||||
|
||||
# ── Eigen3 (bundled) ─────────────────────────────────────────────
|
||||
set(EIGEN3_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third_party/eigen-3.4.0")
|
||||
if(NOT EXISTS "${EIGEN3_INCLUDE_DIR}/Eigen/Core")
|
||||
message(FATAL_ERROR "Eigen3 not found at ${EIGEN3_INCLUDE_DIR}")
|
||||
endif()
|
||||
|
||||
# ── yaml-cpp (optional) ──────────────────────────────────────────
|
||||
set(YAML_CPP_ROOT "")
|
||||
if(WIN32)
|
||||
set(_conda_yaml "C:/Users/$ENV{USERNAME}/miniconda3/Library")
|
||||
if(EXISTS "${_conda_yaml}/include/yaml-cpp/yaml.h")
|
||||
set(YAML_CPP_ROOT "${_conda_yaml}")
|
||||
message(STATUS "Found yaml-cpp in conda: ${YAML_CPP_ROOT}")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
set(YAML_CPP_FOUND FALSE)
|
||||
find_package(yaml-cpp QUIET)
|
||||
if(yaml-cpp_FOUND AND TARGET yaml-cpp)
|
||||
set(YAML_CPP_TARGET yaml-cpp)
|
||||
set(YAML_CPP_FOUND TRUE)
|
||||
if(NOT yaml-cpp_INCLUDE_DIRS AND DEFINED YAML_CPP_INCLUDE_DIR)
|
||||
set(yaml-cpp_INCLUDE_DIRS "${YAML_CPP_INCLUDE_DIR}")
|
||||
endif()
|
||||
elseif(yaml-cpp_FOUND AND DEFINED YAML_CPP_INCLUDE_DIR)
|
||||
set(YAML_CPP_TARGET yaml-cpp)
|
||||
set(YAML_CPP_FOUND TRUE)
|
||||
add_library(yaml-cpp UNKNOWN IMPORTED)
|
||||
set_target_properties(yaml-cpp PROPERTIES
|
||||
IMPORTED_LOCATION "${YAML_CPP_LIBRARY_DIR}/yaml-cpp.lib"
|
||||
INTERFACE_INCLUDE_DIRECTORIES "${YAML_CPP_INCLUDE_DIR}"
|
||||
)
|
||||
if(NOT yaml-cpp_INCLUDE_DIRS)
|
||||
set(yaml-cpp_INCLUDE_DIRS "${YAML_CPP_INCLUDE_DIR}")
|
||||
endif()
|
||||
elseif(YAML_CPP_ROOT)
|
||||
set(yaml-cpp_INCLUDE_DIRS "${YAML_CPP_ROOT}/include")
|
||||
set(yaml-cpp_LIBRARIES "${YAML_CPP_ROOT}/lib/yaml-cpp.lib")
|
||||
if(EXISTS "${yaml-cpp_LIBRARIES}")
|
||||
add_library(yaml-cpp UNKNOWN IMPORTED)
|
||||
set_target_properties(yaml-cpp PROPERTIES
|
||||
IMPORTED_LOCATION "${yaml-cpp_LIBRARIES}"
|
||||
INTERFACE_INCLUDE_DIRECTORIES "${yaml-cpp_INCLUDE_DIRS}"
|
||||
)
|
||||
set(YAML_CPP_TARGET yaml-cpp)
|
||||
set(YAML_CPP_FOUND TRUE)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
if(YAML_CPP_FOUND)
|
||||
message(STATUS "yaml-cpp found. Pipeline will be built.")
|
||||
else()
|
||||
message(WARNING "yaml-cpp not found. Pipeline and ROS wrapper will be disabled.")
|
||||
endif()
|
||||
|
||||
# ── ONNX Runtime ─────────────────────────────────────────────────
|
||||
set(ONNXRuntime_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third_party/onnxruntime" CACHE PATH "ONNX Runtime root")
|
||||
set(ONNXRuntime_INCLUDE_DIRS "${ONNXRuntime_DIR}/include")
|
||||
|
||||
if(WIN32)
|
||||
if(CMAKE_CXX_COMPILER_ID MATCHES "GNU")
|
||||
set(ONNXRuntime_LIB_CANDIDATE "${ONNXRuntime_DIR}/lib/libonnxruntime.a")
|
||||
else()
|
||||
set(ONNXRuntime_LIB_CANDIDATE "${ONNXRuntime_DIR}/lib/onnxruntime.lib")
|
||||
endif()
|
||||
else()
|
||||
set(ONNXRuntime_LIB_CANDIDATE "${ONNXRuntime_DIR}/lib/libonnxruntime.so")
|
||||
endif()
|
||||
|
||||
if(EXISTS "${ONNXRuntime_LIB_CANDIDATE}")
|
||||
set(ONNXRuntime_LIBS "${ONNXRuntime_LIB_CANDIDATE}")
|
||||
set(ONNXRuntime_FOUND TRUE)
|
||||
message(STATUS "ONNX Runtime found: ${ONNXRuntime_LIBS}")
|
||||
else()
|
||||
set(ONNXRuntime_FOUND FALSE)
|
||||
message(WARNING "ONNX Runtime NOT found. ONNX tests disabled.")
|
||||
endif()
|
||||
|
||||
# ── Core library sources ─────────────────────────────────────────
|
||||
set(CORE_BASE_SOURCES
|
||||
src/core/fft_utils.cpp
|
||||
src/core/audio_buffer.cpp
|
||||
src/core/feature_extractor.cpp
|
||||
src/core/gcc_phat_localizer.cpp
|
||||
src/core/distance_estimator.cpp
|
||||
src/core/threat_tracker.cpp
|
||||
)
|
||||
|
||||
set(CORE_ONNX_SOURCES
|
||||
src/core/gunshot_classifier.cpp
|
||||
)
|
||||
if(YAML_CPP_FOUND)
|
||||
list(APPEND CORE_ONNX_SOURCES src/core/pipeline.cpp)
|
||||
endif()
|
||||
|
||||
set(IO_SOURCES
|
||||
src/io/wav_file_source.cpp
|
||||
)
|
||||
if(NOT WIN32)
|
||||
list(APPEND IO_SOURCES src/io/mobile_phone_source.cpp)
|
||||
endif()
|
||||
|
||||
# Build base core library
|
||||
add_library(${PROJECT_NAME}_core_base STATIC ${CORE_BASE_SOURCES})
|
||||
target_include_directories(${PROJECT_NAME}_core_base PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
# Full core library
|
||||
if(ONNXRuntime_FOUND)
|
||||
add_library(${PROJECT_NAME}_core STATIC ${CORE_BASE_SOURCES} ${CORE_ONNX_SOURCES} ${IO_SOURCES})
|
||||
target_include_directories(${PROJECT_NAME}_core PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
${ONNXRuntime_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIR}
|
||||
)
|
||||
if(YAML_CPP_FOUND)
|
||||
target_include_directories(${PROJECT_NAME}_core PUBLIC ${yaml-cpp_INCLUDE_DIRS})
|
||||
target_link_libraries(${PROJECT_NAME}_core PUBLIC ${YAML_CPP_TARGET})
|
||||
endif()
|
||||
target_link_libraries(${PROJECT_NAME}_core PUBLIC ${ONNXRuntime_LIBS})
|
||||
if(NOT WIN32)
|
||||
target_link_libraries(${PROJECT_NAME}_core PUBLIC m)
|
||||
endif()
|
||||
else()
|
||||
add_library(${PROJECT_NAME}_core ALIAS ${PROJECT_NAME}_core_base)
|
||||
endif()
|
||||
|
||||
# ── ROS Wrapper ──────────────────────────────────────────────────
|
||||
if(BUILD_ROS_WRAPPER)
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
geometry_msgs
|
||||
message_generation
|
||||
)
|
||||
|
||||
add_message_files(FILES AcousticThreat.msg AcousticThreatArray.msg)
|
||||
generate_messages(DEPENDENCIES std_msgs)
|
||||
|
||||
catkin_package(CATKIN_DEPENDS roscpp std_msgs sensor_msgs message_runtime)
|
||||
|
||||
add_executable(${PROJECT_NAME}_node
|
||||
src/ros/acoustic_node.cpp
|
||||
src/ros/threat_publisher.cpp
|
||||
src/main.cpp
|
||||
)
|
||||
target_include_directories(${PROJECT_NAME}_node PRIVATE ${catkin_INCLUDE_DIRS})
|
||||
target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_core ${catkin_LIBRARIES})
|
||||
add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_generate_messages_cpp)
|
||||
endif()
|
||||
|
||||
# ── Unit tests ───────────────────────────────────────────────────
|
||||
if(BUILD_TESTS)
|
||||
enable_testing()
|
||||
|
||||
add_executable(test_core_lib tests/test_core_lib.cpp ${CORE_BASE_SOURCES})
|
||||
target_include_directories(test_core_lib PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include ${EIGEN3_INCLUDE_DIR})
|
||||
if(NOT WIN32)
|
||||
target_link_libraries(test_core_lib PRIVATE m)
|
||||
endif()
|
||||
add_test(NAME core_lib COMMAND test_core_lib)
|
||||
|
||||
add_executable(extract_mel_cpp tests/extract_mel_cpp.cpp
|
||||
src/core/fft_utils.cpp src/core/feature_extractor.cpp)
|
||||
target_include_directories(extract_mel_cpp PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include ${EIGEN3_INCLUDE_DIR})
|
||||
if(NOT WIN32)
|
||||
target_link_libraries(extract_mel_cpp PRIVATE m)
|
||||
endif()
|
||||
|
||||
if(ONNXRuntime_FOUND AND BUILD_ONNX_TESTS)
|
||||
add_executable(test_classifier_cpp tests/test_classifier_cpp.cpp
|
||||
src/core/fft_utils.cpp src/core/feature_extractor.cpp src/core/gunshot_classifier.cpp)
|
||||
target_include_directories(test_classifier_cpp PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include ${EIGEN3_INCLUDE_DIR} ${ONNXRuntime_INCLUDE_DIRS})
|
||||
target_compile_definitions(test_classifier_cpp PRIVATE _stdcall=)
|
||||
target_link_libraries(test_classifier_cpp PRIVATE ${ONNXRuntime_LIBS})
|
||||
if(NOT WIN32)
|
||||
target_link_libraries(test_classifier_cpp PRIVATE m)
|
||||
endif()
|
||||
add_test(NAME classifier_cpp COMMAND test_classifier_cpp
|
||||
--model ${CMAKE_CURRENT_SOURCE_DIR}/models/gunshot_classifier.onnx
|
||||
--wav ${CMAKE_CURRENT_SOURCE_DIR}/dataset/binary/val/ambient/drone_noise_val_000.wav)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# ── Install ──────────────────────────────────────────────────────
|
||||
if(BUILD_ROS_WRAPPER AND TARGET ${PROJECT_NAME}_core)
|
||||
install(TARGETS ${PROJECT_NAME}_core
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
|
||||
install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)
|
||||
install(DIRECTORY models/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/models)
|
||||
endif()
|
||||
@ -0,0 +1,32 @@
|
||||
@echo off
|
||||
chcp 65001 >nul
|
||||
setlocal EnableDelayedExpansion
|
||||
|
||||
echo ==========================================
|
||||
echo Acoustic Analyzer Batch Validation Test
|
||||
echo ==========================================
|
||||
set DEMO=build\demo_offline.exe
|
||||
if not exist %DEMO% (
|
||||
echo [ERROR] demo_offline.exe not found. Run build_demo.bat first.
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
echo.
|
||||
echo [TEST 1] Validation set (all subdirectories)...
|
||||
%DEMO% dataset\binary\val
|
||||
if %ERRORLEVEL% NEQ 0 (
|
||||
echo [FAIL] Validation test failed.
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
echo.
|
||||
echo [TEST 2] Single ambient file...
|
||||
%DEMO% dataset\binary\val\ambient\drone_noise_val_000.wav
|
||||
|
||||
echo.
|
||||
echo [TEST 3] Single threat file...
|
||||
%DEMO% dataset\binary\val\threat\synth_threat_val_000.wav
|
||||
|
||||
echo.
|
||||
echo [OK] All batch tests completed.
|
||||
endlocal
|
||||
@ -0,0 +1,52 @@
|
||||
@echo off
|
||||
chcp 65001 >nul
|
||||
setlocal EnableDelayedExpansion
|
||||
|
||||
echo ==========================================
|
||||
echo Acoustic Module - CMake MinGW Build
|
||||
echo ==========================================
|
||||
|
||||
set SRC_ROOT=%~dp0
|
||||
set TEMP_SRC=C:\temp\acoustic_src
|
||||
set TEMP_BUILD=C:\temp\acoustic_build
|
||||
set OUTPUT_DIR=%SRC_ROOT%build
|
||||
|
||||
REM --- Copy source to temp English path (workaround for Chinese path issues) ---
|
||||
echo [INFO] Copying source to %TEMP_SRC% ...
|
||||
if exist %TEMP_SRC% rmdir /s /q %TEMP_SRC%
|
||||
if exist %TEMP_BUILD% rmdir /s /q %TEMP_BUILD%
|
||||
mkdir %TEMP_SRC%
|
||||
xcopy /s /e /i /q "%SRC_ROOT%*" "%TEMP_SRC%\" >nul
|
||||
|
||||
echo [INFO] Running CMake ...
|
||||
mkdir %TEMP_BUILD%
|
||||
cd /d %TEMP_BUILD%
|
||||
cmake "%TEMP_SRC%" -G "MinGW Makefiles" ^
|
||||
-DBUILD_TESTS=ON ^
|
||||
-DBUILD_ONNX_TESTS=ON ^
|
||||
-DBUILD_ROS_WRAPPER=OFF ^
|
||||
-DCMAKE_BUILD_TYPE=Release
|
||||
if %ERRORLEVEL% NEQ 0 (
|
||||
echo [FAIL] CMake configuration failed.
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
echo [INFO] Building with mingw32-make ...
|
||||
mingw32-make -j4
|
||||
if %ERRORLEVEL% NEQ 0 (
|
||||
echo [FAIL] Build failed.
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
REM --- Copy executables back ---
|
||||
if not exist "%OUTPUT_DIR%" mkdir "%OUTPUT_DIR%"
|
||||
for %%E in (test_core_lib.exe extract_mel_cpp.exe test_classifier_cpp.exe demo_offline.exe) do (
|
||||
if exist "%TEMP_BUILD%\%%E" (
|
||||
copy /y "%TEMP_BUILD%\%%E" "%OUTPUT_DIR%\" >nul
|
||||
echo [OK] Copied %%E
|
||||
)
|
||||
)
|
||||
|
||||
echo.
|
||||
echo [OK] CMake MinGW build complete. Executables in %OUTPUT_DIR%
|
||||
endlocal
|
||||
@ -0,0 +1,70 @@
|
||||
@echo off
|
||||
chcp 65001 >nul
|
||||
setlocal EnableDelayedExpansion
|
||||
|
||||
echo ==========================================
|
||||
echo Acoustic Core Library Test Build
|
||||
echo ==========================================
|
||||
|
||||
set SRC_ROOT=%~dp0
|
||||
set EIGEN=%SRC_ROOT%third_party\eigen-3.4.0
|
||||
set ONNX_INC=%SRC_ROOT%third_party\onnxruntime\include
|
||||
|
||||
set INCLUDES=-I%SRC_ROOT%include -I%EIGEN%
|
||||
set FLAGS=-std=c++17 -O2 -D_USE_MATH_DEFINES -Wa,-mbig-obj
|
||||
|
||||
if not exist build mkdir build
|
||||
|
||||
echo [1/3] Building test_core_lib.exe ...
|
||||
g++ %FLAGS% %INCLUDES% ^
|
||||
tests\test_core_lib.cpp ^
|
||||
src\core\fft_utils.cpp ^
|
||||
src\core\audio_buffer.cpp ^
|
||||
src\core\feature_extractor.cpp ^
|
||||
src\core\distance_estimator.cpp ^
|
||||
src\core\gcc_phat_localizer.cpp ^
|
||||
src\core\threat_tracker.cpp ^
|
||||
src\io\wav_file_source.cpp ^
|
||||
-o build\test_core_lib.exe ^
|
||||
-D_stdcall=
|
||||
if %ERRORLEVEL% NEQ 0 (
|
||||
echo [FAIL] test_core_lib build failed.
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
echo [2/3] Building extract_mel_cpp.exe ...
|
||||
g++ %FLAGS% %INCLUDES% ^
|
||||
tests\extract_mel_cpp.cpp ^
|
||||
src\core\fft_utils.cpp ^
|
||||
src\core\feature_extractor.cpp ^
|
||||
src\io\wav_file_source.cpp ^
|
||||
-o build\extract_mel_cpp.exe ^
|
||||
-D_stdcall=
|
||||
if %ERRORLEVEL% NEQ 0 (
|
||||
echo [FAIL] extract_mel_cpp build failed.
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
echo [3/3] Building test_classifier_cpp.exe ...
|
||||
set ONNX_LIB=%SRC_ROOT%third_party\onnxruntime\lib\libonnxruntime.a
|
||||
g++ %FLAGS% %INCLUDES% -I%ONNX_INC% ^
|
||||
tests\test_classifier_cpp.cpp ^
|
||||
src\core\fft_utils.cpp ^
|
||||
src\core\feature_extractor.cpp ^
|
||||
src\core\gunshot_classifier.cpp ^
|
||||
src\io\wav_file_source.cpp ^
|
||||
-o build\test_classifier_cpp.exe ^
|
||||
%ONNX_LIB% -D_stdcall=
|
||||
if %ERRORLEVEL% NEQ 0 (
|
||||
echo [FAIL] test_classifier_cpp build failed.
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
echo.
|
||||
echo [OK] All test executables built successfully.
|
||||
echo.
|
||||
echo Run tests:
|
||||
echo build\test_core_lib.exe
|
||||
echo build\extract_mel_cpp.exe dataset\binary\val\ambient\xxx.wav
|
||||
echo build\test_classifier_cpp.exe --model models\gunshot_classifier.onnx --wav dataset\binary\val\ambient\xxx.wav --label_map models\label_map.json
|
||||
endlocal
|
||||
@ -0,0 +1,40 @@
|
||||
@echo off
|
||||
chcp 65001 >nul
|
||||
setlocal EnableDelayedExpansion
|
||||
|
||||
echo ==========================================
|
||||
echo Acoustic Offline Demo Build (Windows)
|
||||
echo ==========================================
|
||||
|
||||
set SRC_ROOT=%~dp0
|
||||
set EIGEN=%SRC_ROOT%third_party\eigen-3.4.0
|
||||
set ONNX_INC=%SRC_ROOT%third_party\onnxruntime\include
|
||||
set ONNX_LIB=%SRC_ROOT%third_party\onnxruntime\lib\libonnxruntime.a
|
||||
|
||||
set INCLUDES=-I%SRC_ROOT%include -I%EIGEN% -I%ONNX_INC%
|
||||
set FLAGS=-std=c++17 -O2 -D_USE_MATH_DEFINES -Wa,-mbig-obj
|
||||
set LIBS=%ONNX_LIB%
|
||||
|
||||
if not exist build mkdir build
|
||||
|
||||
echo [1/1] Building demo_offline.exe (simplified, no yaml-cpp dependency) ...
|
||||
g++ %FLAGS% %INCLUDES% ^
|
||||
tests\demo_offline.cpp ^
|
||||
src\core\fft_utils.cpp ^
|
||||
src\core\feature_extractor.cpp ^
|
||||
src\core\distance_estimator.cpp ^
|
||||
src\core\gunshot_classifier.cpp ^
|
||||
src\io\wav_file_source.cpp ^
|
||||
-o build\demo_offline.exe ^
|
||||
%LIBS% -D_stdcall=
|
||||
if %ERRORLEVEL% NEQ 0 (
|
||||
echo [FAIL] demo_offline build failed.
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
echo.
|
||||
echo [OK] demo_offline.exe built successfully in build\
|
||||
echo.
|
||||
echo Usage: build\demo_offline.exe dataset\binary\val\ambient\xxx.wav
|
||||
echo build\demo_offline.exe dataset\binary\val\threat\xxx.wav
|
||||
endlocal
|
||||
@ -0,0 +1,47 @@
|
||||
@echo off
|
||||
chcp 65001 >nul
|
||||
setlocal EnableDelayedExpansion
|
||||
|
||||
echo ==========================================
|
||||
echo Acoustic Offline Multichannel Demo Build
|
||||
echo ==========================================
|
||||
|
||||
set SRC_ROOT=%~dp0
|
||||
set EIGEN=%SRC_ROOT%third_party\eigen-3.4.0
|
||||
set ONNX_INC=%SRC_ROOT%third_party\onnxruntime\include
|
||||
set ONNX_LIB=%SRC_ROOT%third_party\onnxruntime\lib\libonnxruntime.a
|
||||
|
||||
set INCLUDES=-I%SRC_ROOT%include -I%EIGEN% -I%ONNX_INC%
|
||||
set FLAGS=-std=c++17 -O2 -D_USE_MATH_DEFINES -Wa,-mbig-obj
|
||||
set LIBS=%ONNX_LIB%
|
||||
|
||||
if not exist build mkdir build
|
||||
|
||||
echo [1/1] Building demo_offline_multichannel.exe ...
|
||||
g++ %FLAGS% %INCLUDES% ^
|
||||
tests\demo_offline_multichannel.cpp ^
|
||||
src\core\pipeline.cpp ^
|
||||
src\core\audio_buffer.cpp ^
|
||||
src\core\fft_utils.cpp ^
|
||||
src\core\feature_extractor.cpp ^
|
||||
src\core\gunshot_classifier.cpp ^
|
||||
src\core\gcc_phat_localizer.cpp ^
|
||||
src\core\distance_estimator.cpp ^
|
||||
src\core\threat_tracker.cpp ^
|
||||
src\io\wav_file_source.cpp ^
|
||||
-o build\demo_offline_multichannel.exe ^
|
||||
%LIBS% -D_stdcall=
|
||||
if %ERRORLEVEL% NEQ 0 (
|
||||
echo [FAIL] demo_offline_multichannel build failed.
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
echo.
|
||||
echo [OK] demo_offline_multichannel.exe built successfully in build\^
|
||||
echo.
|
||||
echo Usage: build\demo_offline_multichannel.exe dataset\multichannel_test.wav --num_mics 4 --layout cross
|
||||
echo build\demo_offline_multichannel.exe dataset\real\threat\ --threshold 0.6 --num_mics 4
|
||||
echo.
|
||||
echo With ground-truth for error analysis:
|
||||
echo build\demo_offline_multichannel.exe synth_90deg_100m.wav --ground_azimuth 90 --ground_distance 100
|
||||
endlocal
|
||||
@ -0,0 +1,48 @@
|
||||
#ifndef ACOUSTIC_ANALYZER_IO_WAV_FILE_SOURCE_H_
|
||||
#define ACOUSTIC_ANALYZER_IO_WAV_FILE_SOURCE_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
/**
|
||||
* @brief WAV 文件音频源
|
||||
*
|
||||
* 支持 16-bit PCM 单声道或多声道 WAV 读取。
|
||||
*/
|
||||
class WavFileSource {
|
||||
public:
|
||||
explicit WavFileSource(const std::string& path, int target_sample_rate = 16000);
|
||||
~WavFileSource();
|
||||
|
||||
bool open();
|
||||
std::size_t read(std::vector<std::vector<float>>& out, std::size_t max_samples);
|
||||
void close();
|
||||
|
||||
std::size_t num_channels() const { return num_channels_; }
|
||||
int sample_rate() const { return target_sample_rate_ > 0 ? target_sample_rate_ : file_sample_rate_; }
|
||||
int file_sample_rate() const { return file_sample_rate_; }
|
||||
std::size_t total_samples() const { return total_samples_; }
|
||||
bool is_open() const { return fp_ != nullptr; }
|
||||
|
||||
private:
|
||||
bool parse_wav_header();
|
||||
void resample_if_needed(std::vector<float>& mono, int src_rate, int dst_rate);
|
||||
std::string path_;
|
||||
int target_sample_rate_;
|
||||
std::FILE* fp_;
|
||||
uint16_t num_channels_;
|
||||
uint32_t file_sample_rate_;
|
||||
uint16_t bits_per_sample_;
|
||||
long data_start_;
|
||||
std::size_t total_samples_;
|
||||
std::size_t read_pos_;
|
||||
};
|
||||
|
||||
} // namespace acoustic
|
||||
|
||||
#endif // ACOUSTIC_ANALYZER_IO_WAV_FILE_SOURCE_H_
|
||||
@ -0,0 +1,15 @@
|
||||
#pragma once
|
||||
#include <memory>
|
||||
#include <ros/ros.h>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
class ThreatPublisher;
|
||||
class AcousticNode;
|
||||
|
||||
std::unique_ptr<AcousticNode> create_acoustic_node(
|
||||
ros::NodeHandle& nh,
|
||||
ros::NodeHandle& pnh,
|
||||
ThreatPublisher* publisher = nullptr);
|
||||
|
||||
} // namespace acoustic
|
||||
Binary file not shown.
@ -0,0 +1,2 @@
|
||||
0: ambient
|
||||
1: threat
|
||||
Binary file not shown.
@ -0,0 +1,8 @@
|
||||
# 单条声源威胁信息
|
||||
string threat_id
|
||||
string sound_type
|
||||
float32 confidence
|
||||
float32 azimuth # 0~360, 0=North, clockwise
|
||||
float32 elevation # -90~90
|
||||
float32 distance # -1 means unknown
|
||||
float32 distance_confidence
|
||||
@ -0,0 +1,3 @@
|
||||
# 一帧声源威胁数组
|
||||
Header header
|
||||
AcousticThreat[] threats
|
||||
@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>acoustic_analyzer</name>
|
||||
<version>1.0.0</version>
|
||||
<description>Acoustic threat analyzer for ZhiTuTouSong UAV system. Gunshot/artillery detection, GCC-PHAT localization, and distance estimation.</description>
|
||||
<maintainer email="team@zhitutousong.com">ZhiTu Team</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>roscpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>message_generation</depend>
|
||||
<depend>message_runtime</depend>
|
||||
|
||||
<export>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,64 @@
|
||||
@echo off
|
||||
chcp 65001 >nul
|
||||
setlocal EnableDelayedExpansion
|
||||
|
||||
echo ==========================================
|
||||
echo Acoustic Module - One-Click Demo
|
||||
echo ==========================================
|
||||
|
||||
set BUILD_DIR=%~dp0build
|
||||
set MODEL=models\gunshot_classifier.onnx
|
||||
set VAL_DIR=dataset\binary\val
|
||||
|
||||
REM --- Ensure executables exist ---
|
||||
if not exist "%BUILD_DIR%\test_core_lib.exe" (
|
||||
echo [INFO] test_core_lib.exe not found, building core tests first...
|
||||
call "%~dp0build_core_test.bat"
|
||||
if %ERRORLEVEL% NEQ 0 exit /b 1
|
||||
)
|
||||
|
||||
if not exist "%BUILD_DIR%\demo_offline.exe" (
|
||||
echo [INFO] demo_offline.exe not found, building demo first...
|
||||
call "%~dp0build_demo.bat"
|
||||
if %ERRORLEVEL% NEQ 0 exit /b 1
|
||||
)
|
||||
|
||||
REM --- Step 1: Core unit tests ---
|
||||
echo.
|
||||
echo [Step 1/3] Running core unit tests...
|
||||
"%BUILD_DIR%\test_core_lib.exe"
|
||||
if %ERRORLEVEL% NEQ 0 (
|
||||
echo [FAIL] Core unit tests failed.
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
REM --- Step 2: ONNX quick inference ---
|
||||
echo.
|
||||
echo [Step 2/3] Running ONNX classifier test...
|
||||
if exist "%MODEL%" (
|
||||
if exist "%VAL_DIR%\ambient" (
|
||||
for %%F in ("%VAL_DIR%\ambient\*.wav") do (
|
||||
echo Testing: %%F
|
||||
"%BUILD_DIR%\test_classifier_cpp.exe" --model "%MODEL%" --wav "%%F"
|
||||
goto :break_onnx
|
||||
)
|
||||
:break_onnx
|
||||
) else (
|
||||
echo [SKIP] No val data found for ONNX test
|
||||
)
|
||||
) else (
|
||||
echo [SKIP] ONNX model not found: %MODEL%
|
||||
)
|
||||
|
||||
REM --- Step 3: Offline pipeline demo ---
|
||||
echo.
|
||||
echo [Step 3/3] Running offline pipeline demo...
|
||||
if exist "%VAL_DIR%" (
|
||||
"%BUILD_DIR%\demo_offline.exe" "%VAL_DIR%"
|
||||
) else (
|
||||
echo [SKIP] Val dataset not found: %VAL_DIR%
|
||||
)
|
||||
|
||||
echo.
|
||||
echo [OK] Demo complete.
|
||||
endlocal
|
||||
@ -0,0 +1,72 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
4 通道模拟信号生成
|
||||
根据指定方位角和距离生成十字阵列 WAV 文件
|
||||
"""
|
||||
import argparse
|
||||
import numpy as np
|
||||
import soundfile as sf
|
||||
|
||||
|
||||
def generate_gunshot_signal(sr=16000, duration=2.0, freq=1000, decay=0.05):
|
||||
t = np.linspace(0, duration, int(sr * duration))
|
||||
# Impulse-like exponential decay sine
|
||||
envelope = np.exp(-t / decay)
|
||||
signal = np.sin(2 * np.pi * freq * t) * envelope
|
||||
# Normalize
|
||||
signal = signal / np.max(np.abs(signal) + 1e-10)
|
||||
return signal.astype(np.float32)
|
||||
|
||||
|
||||
def simulate_array(signal, azimuth_deg, distance_m, sr=16000, spacing=0.15, c=343.0):
|
||||
"""
|
||||
Cross array: mic positions [front, right, back, left]
|
||||
Returns 4 channels with appropriate delays and attenuation
|
||||
"""
|
||||
az = np.deg2rad(azimuth_deg)
|
||||
# Source position in plane
|
||||
src_x = distance_m * np.sin(az)
|
||||
src_y = distance_m * np.cos(az)
|
||||
mics = np.array([
|
||||
[0.0, spacing],
|
||||
[spacing, 0.0],
|
||||
[0.0, -spacing],
|
||||
[-spacing, 0.0]
|
||||
])
|
||||
channels = []
|
||||
for mic in mics:
|
||||
dist = np.sqrt((src_x - mic[0])**2 + (src_y - mic[1])**2)
|
||||
delay_sec = dist / c
|
||||
delay_samples = int(delay_sec * sr)
|
||||
# Amplitude attenuation (1/r)
|
||||
att = 1.0 / (dist + 1e-6)
|
||||
ch = np.zeros_like(signal)
|
||||
if delay_samples < len(signal):
|
||||
ch[delay_samples:] = signal[:len(signal) - delay_samples] * att
|
||||
channels.append(ch)
|
||||
return np.stack(channels, axis=0) # [4, samples]
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--output', default='dataset/multichannel_test.wav')
|
||||
parser.add_argument('--azimuth', type=float, default=90.0, help='Azimuth in degrees')
|
||||
parser.add_argument('--distance', type=float, default=100.0, help='Distance in meters')
|
||||
parser.add_argument('--sr', type=int, default=16000)
|
||||
parser.add_argument('--duration', type=float, default=2.0)
|
||||
args = parser.parse_args()
|
||||
|
||||
signal = generate_gunshot_signal(args.sr, args.duration)
|
||||
multich = simulate_array(signal, args.azimuth, args.distance, args.sr)
|
||||
# Normalize to prevent clipping
|
||||
max_val = np.max(np.abs(multich))
|
||||
if max_val > 0:
|
||||
multich = multich / max_val * 0.9
|
||||
|
||||
# soundfile expects [samples, channels] for multi-channel
|
||||
sf.write(args.output, multich.T, args.sr, subtype="PCM_16")
|
||||
print(f"Generated {args.output}: azimuth={args.azimuth}°, distance={args.distance}m, shape={multich.shape}")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -0,0 +1,243 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
二分类 CNN-GRU 训练脚本
|
||||
支持 --resume 从 checkpoint 恢复
|
||||
使用 WeightedRandomSampler + Mixup
|
||||
"""
|
||||
import argparse
|
||||
import os
|
||||
import json
|
||||
import random
|
||||
import numpy as np
|
||||
import torch
|
||||
import torch.nn as nn
|
||||
import torch.optim as optim
|
||||
from torch.utils.data import Dataset, DataLoader, WeightedRandomSampler
|
||||
from torchaudio.transforms import MelSpectrogram
|
||||
import torchaudio.functional as F
|
||||
import soundfile as sf
|
||||
|
||||
|
||||
def set_seed(seed=42):
|
||||
random.seed(seed)
|
||||
np.random.seed(seed)
|
||||
torch.manual_seed(seed)
|
||||
|
||||
|
||||
class AudioDataset(Dataset):
|
||||
def __init__(self, root_dir, n_mels=64, n_fft=2048, hop_length=512, sr=16000, augment=False):
|
||||
self.samples = []
|
||||
self.labels = []
|
||||
self.classes = sorted(os.listdir(root_dir))
|
||||
self.class_to_idx = {c: i for i, c in enumerate(self.classes)}
|
||||
self.sr = sr
|
||||
self.augment = augment
|
||||
self.mel = MelSpectrogram(sample_rate=sr, n_fft=n_fft, hop_length=hop_length,
|
||||
n_mels=n_mels, f_min=0, f_max=8000, center=False)
|
||||
for cls in self.classes:
|
||||
cls_dir = os.path.join(root_dir, cls)
|
||||
if not os.path.isdir(cls_dir):
|
||||
continue
|
||||
for fname in os.listdir(cls_dir):
|
||||
if fname.endswith('.wav'):
|
||||
self.samples.append(os.path.join(cls_dir, fname))
|
||||
self.labels.append(self.class_to_idx[cls])
|
||||
|
||||
def __len__(self):
|
||||
return len(self.samples)
|
||||
|
||||
def __getitem__(self, idx):
|
||||
path = self.samples[idx]
|
||||
waveform, sr = sf.read(path, dtype='float32')
|
||||
if waveform.ndim == 1:
|
||||
waveform = waveform[np.newaxis, :]
|
||||
else:
|
||||
waveform = waveform.T # [channels, samples]
|
||||
waveform = torch.from_numpy(waveform)
|
||||
if sr != self.sr:
|
||||
waveform = F.resample(waveform, sr, self.sr)
|
||||
if waveform.shape[0] > 1:
|
||||
waveform = waveform.mean(dim=0, keepdim=True)
|
||||
# Pad/truncate to 2 seconds
|
||||
target_len = self.sr * 2
|
||||
if waveform.shape[1] < target_len:
|
||||
waveform = torch.nn.functional.pad(waveform, (0, target_len - waveform.shape[1]))
|
||||
else:
|
||||
waveform = waveform[:, :target_len]
|
||||
|
||||
if self.augment:
|
||||
waveform = self.apply_augment(waveform)
|
||||
|
||||
mel = self.mel(waveform) # [1, n_mels, T]
|
||||
mel = (mel + 1e-10).log10() * 10.0
|
||||
# Normalize per mel
|
||||
mel = (mel - mel.mean(dim=-1, keepdim=True)) / (mel.std(dim=-1, keepdim=True) + 1e-8)
|
||||
# Pad/truncate time to 63
|
||||
T = mel.shape[-1]
|
||||
if T < 63:
|
||||
mel = torch.nn.functional.pad(mel, (0, 63 - T))
|
||||
else:
|
||||
mel = mel[:, :, :63]
|
||||
return mel.squeeze(0).transpose(0, 1), self.labels[idx] # [T, n_mels]
|
||||
|
||||
def apply_augment(self, waveform):
|
||||
if random.random() < 0.5:
|
||||
gain = random.uniform(0.8, 1.2)
|
||||
waveform = waveform * gain
|
||||
if random.random() < 0.3:
|
||||
noise = torch.randn_like(waveform) * 0.005
|
||||
waveform = waveform + noise
|
||||
return waveform
|
||||
|
||||
|
||||
class CNNGRU(nn.Module):
|
||||
def __init__(self, n_mels=64, num_classes=2):
|
||||
super().__init__()
|
||||
self.cnn = nn.Sequential(
|
||||
nn.Conv1d(n_mels, 64, kernel_size=3, padding=1),
|
||||
nn.ReLU(),
|
||||
nn.MaxPool1d(2),
|
||||
nn.Conv1d(64, 128, kernel_size=3, padding=1),
|
||||
nn.ReLU(),
|
||||
nn.MaxPool1d(2),
|
||||
nn.Conv1d(128, 256, kernel_size=3, padding=1),
|
||||
nn.ReLU(),
|
||||
nn.AdaptiveAvgPool1d(16)
|
||||
)
|
||||
self.gru = nn.GRU(256, 128, batch_first=True, bidirectional=True)
|
||||
self.fc = nn.Sequential(
|
||||
nn.Linear(256, 64),
|
||||
nn.ReLU(),
|
||||
nn.Dropout(0.3),
|
||||
nn.Linear(64, num_classes)
|
||||
)
|
||||
|
||||
def forward(self, x):
|
||||
# x: [B, T, n_mels]
|
||||
x = x.transpose(1, 2) # [B, n_mels, T]
|
||||
x = self.cnn(x) # [B, 256, 16]
|
||||
x = x.transpose(1, 2) # [B, 16, 256]
|
||||
x, _ = self.gru(x)
|
||||
x = x.mean(dim=1)
|
||||
return self.fc(x)
|
||||
|
||||
|
||||
def mixup_data(x, y, alpha=0.4):
|
||||
if alpha > 0:
|
||||
lam = np.random.beta(alpha, alpha)
|
||||
else:
|
||||
lam = 1
|
||||
batch_size = x.size(0)
|
||||
index = torch.randperm(batch_size).to(x.device)
|
||||
mixed_x = lam * x + (1 - lam) * x[index]
|
||||
y_a, y_b = y, y[index]
|
||||
return mixed_x, y_a, y_b, lam
|
||||
|
||||
|
||||
def train_epoch(model, loader, criterion, optimizer, device, use_mixup=True):
|
||||
model.train()
|
||||
total_loss = 0
|
||||
correct = 0
|
||||
total = 0
|
||||
for x, y in loader:
|
||||
x, y = x.to(device), y.to(device)
|
||||
optimizer.zero_grad()
|
||||
if use_mixup:
|
||||
x, y_a, y_b, lam = mixup_data(x, y)
|
||||
outputs = model(x)
|
||||
loss = lam * criterion(outputs, y_a) + (1 - lam) * criterion(outputs, y_b)
|
||||
else:
|
||||
outputs = model(x)
|
||||
loss = criterion(outputs, y)
|
||||
loss.backward()
|
||||
optimizer.step()
|
||||
total_loss += loss.item()
|
||||
_, predicted = outputs.max(1)
|
||||
total += y.size(0)
|
||||
if not use_mixup:
|
||||
correct += predicted.eq(y).sum().item()
|
||||
else:
|
||||
correct += (lam * predicted.eq(y_a).sum().item()
|
||||
+ (1 - lam) * predicted.eq(y_b).sum().item())
|
||||
return total_loss / len(loader), correct / total
|
||||
|
||||
|
||||
def eval_epoch(model, loader, criterion, device):
|
||||
model.eval()
|
||||
correct = 0
|
||||
total = 0
|
||||
with torch.no_grad():
|
||||
for x, y in loader:
|
||||
x, y = x.to(device), y.to(device)
|
||||
outputs = model(x)
|
||||
_, predicted = outputs.max(1)
|
||||
total += y.size(0)
|
||||
correct += predicted.eq(y).sum().item()
|
||||
return correct / total
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--data', default='dataset/binary/train')
|
||||
parser.add_argument('--val_data', default='dataset/binary/val')
|
||||
parser.add_argument('--epochs', type=int, default=30)
|
||||
parser.add_argument('--batch_size', type=int, default=16)
|
||||
parser.add_argument('--lr', type=float, default=1e-3)
|
||||
parser.add_argument('--resume', default='')
|
||||
parser.add_argument('--output', default='train_output')
|
||||
args = parser.parse_args()
|
||||
|
||||
set_seed(42)
|
||||
device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
|
||||
|
||||
train_ds = AudioDataset(args.data, augment=True)
|
||||
val_ds = AudioDataset(args.val_data, augment=False) if os.path.isdir(args.val_data) else None
|
||||
|
||||
# Weighted sampler
|
||||
class_counts = np.bincount(train_ds.labels)
|
||||
class_weights = 1.0 / class_counts
|
||||
sample_weights = [class_weights[l] for l in train_ds.labels]
|
||||
sampler = WeightedRandomSampler(sample_weights, len(sample_weights), replacement=True)
|
||||
|
||||
train_loader = DataLoader(train_ds, batch_size=args.batch_size, sampler=sampler)
|
||||
val_loader = DataLoader(val_ds, batch_size=args.batch_size, shuffle=False) if val_ds else None
|
||||
|
||||
model = CNNGRU(num_classes=len(train_ds.classes)).to(device)
|
||||
criterion = nn.CrossEntropyLoss()
|
||||
optimizer = optim.Adam(model.parameters(), lr=args.lr)
|
||||
scheduler = optim.lr_scheduler.StepLR(optimizer, step_size=10, gamma=0.5)
|
||||
|
||||
start_epoch = 0
|
||||
if args.resume and os.path.isfile(args.resume):
|
||||
ckpt = torch.load(args.resume, map_location=device)
|
||||
model.load_state_dict(ckpt['model'])
|
||||
optimizer.load_state_dict(ckpt['optimizer'])
|
||||
start_epoch = ckpt['epoch'] + 1
|
||||
print(f"Resumed from epoch {start_epoch}")
|
||||
|
||||
os.makedirs(args.output, exist_ok=True)
|
||||
best_acc = 0.0
|
||||
for epoch in range(start_epoch, args.epochs):
|
||||
train_loss, train_acc = train_epoch(model, train_loader, criterion, optimizer, device)
|
||||
val_acc = eval_epoch(model, val_loader, criterion, device) if val_loader else 0.0
|
||||
scheduler.step()
|
||||
print(f"Epoch {epoch+1}/{args.epochs} | Loss: {train_loss:.4f} | Train Acc: {train_acc:.4f} | Val Acc: {val_acc:.4f}")
|
||||
if val_acc > best_acc:
|
||||
best_acc = val_acc
|
||||
torch.save({
|
||||
'epoch': epoch,
|
||||
'model': model.state_dict(),
|
||||
'optimizer': optimizer.state_dict(),
|
||||
'acc': val_acc
|
||||
}, os.path.join(args.output, 'best_model.pth'))
|
||||
|
||||
# Save label map
|
||||
label_map = {str(i): c for i, c in enumerate(train_ds.classes)}
|
||||
with open(os.path.join(args.output, 'label_map.json'), 'w') as f:
|
||||
json.dump(label_map, f, indent=2)
|
||||
print(f"Training complete. Best val acc: {best_acc:.4f}")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
import torchaudio
|
||||
main()
|
||||
@ -0,0 +1,60 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
C++/Python 特征一致性验证
|
||||
对比 C++ FeatureExtractor 输出与 Python librosa 输出
|
||||
"""
|
||||
import argparse
|
||||
import numpy as np
|
||||
import torch
|
||||
import torchaudio
|
||||
import torchaudio.functional as F
|
||||
import librosa
|
||||
|
||||
|
||||
def extract_python_mel(wav_path, sr=16000, n_fft=2048, hop_length=512, n_mels=64, fmax=8000):
|
||||
y, sr_loaded = librosa.load(wav_path, sr=sr, mono=True)
|
||||
mel = librosa.feature.melspectrogram(
|
||||
y=y, sr=sr, n_fft=n_fft, hop_length=hop_length,
|
||||
n_mels=n_mels, fmax=fmax, htk=True, center=False
|
||||
)
|
||||
mel_db = librosa.power_to_db(mel, ref=1.0)
|
||||
# Normalize per mel
|
||||
mel_db = (mel_db - mel_db.mean(axis=1, keepdims=True)) / (mel_db.std(axis=1, keepdims=True) + 1e-8)
|
||||
# Pad/truncate to 63 frames
|
||||
if mel_db.shape[1] < 63:
|
||||
mel_db = np.pad(mel_db, ((0, 0), (0, 63 - mel_db.shape[1])), mode='edge')
|
||||
else:
|
||||
mel_db = mel_db[:, :63]
|
||||
return mel_db
|
||||
|
||||
|
||||
def load_cpp_mel(bin_path, n_mels=64, n_frames=63):
|
||||
with open(bin_path, 'rb') as f:
|
||||
data = np.fromfile(f, dtype=np.float32)
|
||||
return data.reshape(n_mels, n_frames)
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--wav', required=True)
|
||||
parser.add_argument('--cpp_bin', required=True, help='C++ extracted mel binary')
|
||||
parser.add_argument('--sr', type=int, default=16000)
|
||||
args = parser.parse_args()
|
||||
|
||||
py_mel = extract_python_mel(args.wav, sr=args.sr)
|
||||
cpp_mel = load_cpp_mel(args.cpp_bin)
|
||||
|
||||
diff = np.abs(py_mel - cpp_mel)
|
||||
print(f"Python shape: {py_mel.shape}, C++ shape: {cpp_mel.shape}")
|
||||
print(f"Max absolute diff: {diff.max():.6f}")
|
||||
print(f"Mean absolute diff: {diff.mean():.6f}")
|
||||
print(f"RMSE: {np.sqrt(np.mean((py_mel - cpp_mel)**2)):.6f}")
|
||||
|
||||
if diff.max() < 0.01:
|
||||
print("[PASS] Feature consistency within tolerance.")
|
||||
else:
|
||||
print("[FAIL] Feature mismatch detected.")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -0,0 +1,80 @@
|
||||
#include "acoustic_analyzer/core/audio_buffer.h"
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
AudioBuffer::AudioBuffer(std::size_t capacity_samples, std::size_t num_channels)
|
||||
: capacity_frames_(capacity_samples > 0 ? capacity_samples : 1),
|
||||
num_channels_(num_channels),
|
||||
buffer_(capacity_frames_ * num_channels),
|
||||
head_(0),
|
||||
tail_(0),
|
||||
size_(0) {
|
||||
}
|
||||
|
||||
std::size_t AudioBuffer::Push(const std::vector<float>& samples) {
|
||||
if (num_channels_ == 0) return 0;
|
||||
std::size_t total_frames = samples.size() / num_channels_;
|
||||
for (std::size_t f = 0; f < total_frames; ++f) {
|
||||
for (std::size_t ch = 0; ch < num_channels_; ++ch) {
|
||||
std::size_t idx = head_ * num_channels_ + ch;
|
||||
buffer_[idx] = samples[f * num_channels_ + ch];
|
||||
}
|
||||
head_ = (head_ + 1) % capacity_frames_;
|
||||
if (size_ < capacity_frames_) {
|
||||
++size_;
|
||||
} else {
|
||||
tail_ = (tail_ + 1) % capacity_frames_;
|
||||
}
|
||||
}
|
||||
return total_frames;
|
||||
}
|
||||
|
||||
std::vector<float> AudioBuffer::Pop(std::size_t num_frames) {
|
||||
std::size_t frames_to_read = std::min(num_frames, size_);
|
||||
std::vector<float> result(frames_to_read * num_channels_);
|
||||
for (std::size_t f = 0; f < frames_to_read; ++f) {
|
||||
for (std::size_t ch = 0; ch < num_channels_; ++ch) {
|
||||
result[f * num_channels_ + ch] = buffer_[tail_ * num_channels_ + ch];
|
||||
}
|
||||
tail_ = (tail_ + 1) % capacity_frames_;
|
||||
}
|
||||
size_ -= frames_to_read;
|
||||
return result;
|
||||
}
|
||||
|
||||
std::vector<float> AudioBuffer::Get(std::size_t offset, std::size_t num_frames) const {
|
||||
if (offset >= size_) return {};
|
||||
std::size_t frames_to_read = std::min(num_frames, size_ - offset);
|
||||
std::vector<float> result(frames_to_read * num_channels_);
|
||||
std::size_t start = (tail_ + offset) % capacity_frames_;
|
||||
for (std::size_t f = 0; f < frames_to_read; ++f) {
|
||||
std::size_t pos = (start + f) % capacity_frames_;
|
||||
for (std::size_t ch = 0; ch < num_channels_; ++ch) {
|
||||
result[f * num_channels_ + ch] = buffer_[pos * num_channels_ + ch];
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
std::size_t AudioBuffer::Size() const noexcept {
|
||||
return size_;
|
||||
}
|
||||
|
||||
std::size_t AudioBuffer::Capacity() const noexcept {
|
||||
return capacity_frames_;
|
||||
}
|
||||
|
||||
void AudioBuffer::Clear() {
|
||||
head_ = 0;
|
||||
tail_ = 0;
|
||||
size_ = 0;
|
||||
std::fill(buffer_.begin(), buffer_.end(), 0.0f);
|
||||
}
|
||||
|
||||
std::size_t AudioBuffer::NumChannels() const noexcept {
|
||||
return num_channels_;
|
||||
}
|
||||
|
||||
} // namespace acoustic
|
||||
@ -0,0 +1,94 @@
|
||||
#include "acoustic_analyzer/core/distance_estimator.h"
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
struct DistanceEstimator::Impl {
|
||||
DistanceConfig config;
|
||||
float kalman_x_ = 0.0f;
|
||||
float kalman_p_ = 1.0f;
|
||||
bool kalman_init_ = false;
|
||||
|
||||
explicit Impl(const DistanceConfig& cfg) : config(cfg) {}
|
||||
|
||||
float ComputeSplInternal(const std::vector<float>& audio_signal) {
|
||||
if (audio_signal.empty()) return 0.0f;
|
||||
double sum = 0.0;
|
||||
for (float s : audio_signal) {
|
||||
sum += static_cast<double>(s) * s;
|
||||
}
|
||||
float rms = std::sqrt(static_cast<float>(sum) / audio_signal.size());
|
||||
float spl = 20.0f * std::log10(std::max(rms, 1e-10f)) + 94.0f + config.spl_calibration_offset;
|
||||
return spl;
|
||||
}
|
||||
|
||||
float GetReferenceSpl(const std::string& sound_type) {
|
||||
if (sound_type == "gunshot") return config.ref_spl_gunshot;
|
||||
if (sound_type == "artillery") return config.ref_spl_artillery;
|
||||
if (sound_type == "explosion") return config.ref_spl_explosion;
|
||||
if (sound_type == "threat") return config.ref_spl_gunshot; // binary model uses gunshot ref
|
||||
// Fallback: use gunshot reference
|
||||
return config.ref_spl_gunshot > 0.0f ? config.ref_spl_gunshot : 150.0f;
|
||||
}
|
||||
|
||||
float EstimateInternal(float spl, const std::string& sound_type) {
|
||||
float L_ref = GetReferenceSpl(sound_type);
|
||||
float alpha = config.attenuation_alpha;
|
||||
if (alpha < 1e-6f) alpha = 0.6f;
|
||||
float d = std::pow(10.0f, (L_ref - spl) / (20.0f * alpha));
|
||||
d = std::max(d, 1.0f);
|
||||
d = std::min(d, 5000.0f);
|
||||
return d;
|
||||
}
|
||||
|
||||
float UpdateKalmanInternal(float raw_distance) {
|
||||
float Q = config.kalman_process_noise;
|
||||
float R = config.kalman_measurement_noise;
|
||||
|
||||
if (!kalman_init_) {
|
||||
kalman_x_ = raw_distance;
|
||||
kalman_p_ = 1.0f;
|
||||
kalman_init_ = true;
|
||||
}
|
||||
|
||||
// Predict
|
||||
kalman_p_ += Q;
|
||||
// Update
|
||||
float K = kalman_p_ / (kalman_p_ + R);
|
||||
kalman_x_ += K * (raw_distance - kalman_x_);
|
||||
kalman_p_ = (1.0f - K) * kalman_p_;
|
||||
return kalman_x_;
|
||||
}
|
||||
};
|
||||
|
||||
DistanceEstimator::DistanceEstimator(const DistanceConfig& config)
|
||||
: impl_(std::make_unique<Impl>(config)) {}
|
||||
|
||||
DistanceEstimator::~DistanceEstimator() = default;
|
||||
DistanceEstimator::DistanceEstimator(DistanceEstimator&&) noexcept = default;
|
||||
DistanceEstimator& DistanceEstimator::operator=(DistanceEstimator&&) noexcept = default;
|
||||
|
||||
float DistanceEstimator::ComputeSpl(const std::vector<float>& audio_signal) {
|
||||
return impl_->ComputeSplInternal(audio_signal);
|
||||
}
|
||||
|
||||
float DistanceEstimator::Estimate(float spl, const std::string& sound_type) {
|
||||
return impl_->EstimateInternal(spl, sound_type);
|
||||
}
|
||||
|
||||
float DistanceEstimator::UpdateKalman(float raw_distance) {
|
||||
return impl_->UpdateKalmanInternal(raw_distance);
|
||||
}
|
||||
|
||||
void DistanceEstimator::Reset() {
|
||||
impl_->kalman_init_ = false;
|
||||
impl_->kalman_p_ = 1.0f;
|
||||
}
|
||||
|
||||
const DistanceConfig& DistanceEstimator::Config() const noexcept {
|
||||
return impl_->config;
|
||||
}
|
||||
|
||||
} // namespace acoustic
|
||||
@ -0,0 +1,186 @@
|
||||
#include "acoustic_analyzer/core/feature_extractor.h"
|
||||
#include "acoustic_analyzer/core/fft_utils.h"
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include <numeric>
|
||||
#include <fstream>
|
||||
#include <memory>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
struct FeatureExtractor::Impl {
|
||||
int sample_rate;
|
||||
int n_fft;
|
||||
int hop_length;
|
||||
int n_mels;
|
||||
float f_min;
|
||||
float f_max;
|
||||
float preemphasis;
|
||||
std::vector<float> window;
|
||||
std::vector<float> mel_filter_bank;
|
||||
bool mel_loaded = false;
|
||||
|
||||
Impl(int sr, int fft, int hop, int mels, float fmin, float fmax, float pre)
|
||||
: sample_rate(sr), n_fft(fft), hop_length(hop), n_mels(mels),
|
||||
f_min(fmin), f_max(fmax), preemphasis(pre) {
|
||||
window.resize(n_fft);
|
||||
for (int i = 0; i < n_fft; ++i) {
|
||||
window[i] = 0.5f - 0.5f * std::cos(2.0f * static_cast<float>(M_PI) * i / (n_fft - 1));
|
||||
}
|
||||
int n_fft_bins = n_fft / 2 + 1;
|
||||
mel_filter_bank.resize(n_mels * n_fft_bins);
|
||||
// Try to load from models/mel_filter_bank.bin
|
||||
std::ifstream f("models/mel_filter_bank.bin", std::ios::binary);
|
||||
if (f) {
|
||||
f.read(reinterpret_cast<char*>(mel_filter_bank.data()),
|
||||
mel_filter_bank.size() * sizeof(float));
|
||||
if (f.gcount() == static_cast<std::streamsize>(mel_filter_bank.size() * sizeof(float))) {
|
||||
mel_loaded = true;
|
||||
}
|
||||
}
|
||||
if (!mel_loaded) {
|
||||
GenerateMelFilterBank();
|
||||
mel_loaded = true;
|
||||
}
|
||||
}
|
||||
|
||||
void GenerateMelFilterBank() {
|
||||
int n_fft_bins = n_fft / 2 + 1;
|
||||
float mel_min = 2595.0f * std::log10(1.0f + f_min / 700.0f);
|
||||
float mel_max = 2595.0f * std::log10(1.0f + f_max / 700.0f);
|
||||
std::vector<float> mel_points(n_mels + 2);
|
||||
for (int i = 0; i < n_mels + 2; ++i) {
|
||||
float mel = mel_min + i * (mel_max - mel_min) / (n_mels + 1);
|
||||
mel_points[i] = 700.0f * (std::pow(10.0f, mel / 2595.0f) - 1.0f);
|
||||
}
|
||||
std::vector<float> bin_points(n_mels + 2);
|
||||
for (int i = 0; i < n_mels + 2; ++i) {
|
||||
bin_points[i] = std::floor((n_fft + 1) * mel_points[i] / sample_rate);
|
||||
}
|
||||
for (int i = 0; i < n_mels; ++i) {
|
||||
for (int j = 0; j < n_fft_bins; ++j) {
|
||||
float val = 0.0f;
|
||||
float left = bin_points[i];
|
||||
float center = bin_points[i + 1];
|
||||
float right = bin_points[i + 2];
|
||||
if (j >= left && j <= center && center > left) {
|
||||
val = (j - left) / (center - left);
|
||||
} else if (j >= center && j <= right && right > center) {
|
||||
val = (right - j) / (right - center);
|
||||
}
|
||||
mel_filter_bank[i * n_fft_bins + j] = val;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::MatrixXf ComputeMelSpec(const std::vector<float>& audio) {
|
||||
std::size_t n_samples = audio.size();
|
||||
int n_fft_bins = n_fft / 2 + 1;
|
||||
int n_frames = static_cast<int>((static_cast<int>(n_samples) - n_fft) / hop_length) + 1;
|
||||
if (n_frames < 1) n_frames = 1;
|
||||
|
||||
std::vector<float> preemph(n_samples);
|
||||
apply_preemphasis(audio.data(), preemph.data(), n_samples, preemphasis);
|
||||
|
||||
std::vector<float> fft_buf(n_fft);
|
||||
std::vector<float> power(n_fft_bins);
|
||||
Eigen::MatrixXf mel_spec(n_mels, n_frames);
|
||||
|
||||
for (int t = 0; t < n_frames; ++t) {
|
||||
int start = t * hop_length;
|
||||
for (int i = 0; i < n_fft; ++i) {
|
||||
std::size_t idx = start + i;
|
||||
fft_buf[i] = (idx < n_samples ? preemph[idx] : 0.0f) * window[i];
|
||||
}
|
||||
compute_power_spectrum(fft_buf.data(), n_fft, power.data());
|
||||
for (int m = 0; m < n_mels; ++m) {
|
||||
float sum = 0.0f;
|
||||
for (int b = 0; b < n_fft_bins; ++b) {
|
||||
sum += power[b] * mel_filter_bank[m * n_fft_bins + b];
|
||||
}
|
||||
mel_spec(m, t) = std::log(std::max(sum, 1e-10f));
|
||||
}
|
||||
}
|
||||
|
||||
// Normalize per mel bin (standardization)
|
||||
for (int m = 0; m < n_mels; ++m) {
|
||||
float mean = 0.0f;
|
||||
for (int t = 0; t < n_frames; ++t) {
|
||||
mean += mel_spec(m, t);
|
||||
}
|
||||
mean /= n_frames;
|
||||
float variance = 0.0f;
|
||||
for (int t = 0; t < n_frames; ++t) {
|
||||
float d = mel_spec(m, t) - mean;
|
||||
variance += d * d;
|
||||
}
|
||||
float stddev = std::sqrt(variance / n_frames + 1e-8f);
|
||||
for (int t = 0; t < n_frames; ++t) {
|
||||
mel_spec(m, t) = (mel_spec(m, t) - mean) / stddev;
|
||||
}
|
||||
}
|
||||
|
||||
// Pad or truncate to target 63 frames with edge padding
|
||||
const int target_frames = 63;
|
||||
if (n_frames < target_frames) {
|
||||
Eigen::MatrixXf padded(n_mels, target_frames);
|
||||
int copy_frames = n_frames;
|
||||
int pad_left = (target_frames - copy_frames) / 2;
|
||||
padded.block(0, pad_left, n_mels, copy_frames) = mel_spec.block(0, 0, n_mels, copy_frames);
|
||||
// Edge padding on left
|
||||
for (int t = 0; t < pad_left; ++t) {
|
||||
padded.col(t) = mel_spec.col(0);
|
||||
}
|
||||
// Edge padding on right
|
||||
for (int t = pad_left + copy_frames; t < target_frames; ++t) {
|
||||
padded.col(t) = mel_spec.col(copy_frames - 1);
|
||||
}
|
||||
return padded;
|
||||
} else if (n_frames > target_frames) {
|
||||
return mel_spec.block(0, 0, n_mels, target_frames);
|
||||
}
|
||||
return mel_spec;
|
||||
}
|
||||
};
|
||||
|
||||
FeatureExtractor::FeatureExtractor(int sample_rate,
|
||||
int n_fft,
|
||||
int hop_length,
|
||||
int n_mels,
|
||||
float f_min,
|
||||
float f_max,
|
||||
float preemphasis)
|
||||
: impl_(std::make_unique<Impl>(sample_rate, n_fft, hop_length, n_mels, f_min, f_max, preemphasis)) {}
|
||||
|
||||
FeatureExtractor::~FeatureExtractor() = default;
|
||||
FeatureExtractor::FeatureExtractor(FeatureExtractor&&) noexcept = default;
|
||||
FeatureExtractor& FeatureExtractor::operator=(FeatureExtractor&&) noexcept = default;
|
||||
|
||||
Eigen::MatrixXf FeatureExtractor::MelSpectrogram(const std::vector<float>& audio_samples) {
|
||||
return impl_->ComputeMelSpec(audio_samples);
|
||||
}
|
||||
|
||||
std::vector<Eigen::MatrixXf> FeatureExtractor::MelSpectrogramMultiChannel(
|
||||
const std::vector<float>& audio_samples,
|
||||
std::size_t num_channels) {
|
||||
if (num_channels == 0) return {};
|
||||
std::size_t total = audio_samples.size();
|
||||
std::size_t samples_per_channel = total / num_channels;
|
||||
std::vector<Eigen::MatrixXf> results;
|
||||
results.reserve(num_channels);
|
||||
for (std::size_t ch = 0; ch < num_channels; ++ch) {
|
||||
std::vector<float> ch_audio(samples_per_channel);
|
||||
for (std::size_t i = 0; i < samples_per_channel; ++i) {
|
||||
ch_audio[i] = audio_samples[i * num_channels + ch];
|
||||
}
|
||||
results.push_back(impl_->ComputeMelSpec(ch_audio));
|
||||
}
|
||||
return results;
|
||||
}
|
||||
|
||||
int FeatureExtractor::SampleRate() const noexcept { return impl_->sample_rate; }
|
||||
int FeatureExtractor::NFFT() const noexcept { return impl_->n_fft; }
|
||||
int FeatureExtractor::HopLength() const noexcept { return impl_->hop_length; }
|
||||
int FeatureExtractor::NMels() const noexcept { return impl_->n_mels; }
|
||||
|
||||
} // namespace acoustic
|
||||
@ -0,0 +1,92 @@
|
||||
#include "acoustic_analyzer/core/fft_utils.h"
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
void apply_preemphasis(const float* in, float* out, std::size_t n, float coef) {
|
||||
if (n == 0) return;
|
||||
out[0] = in[0];
|
||||
for (std::size_t i = 1; i < n; ++i) {
|
||||
out[i] = in[i] - coef * in[i - 1];
|
||||
}
|
||||
}
|
||||
|
||||
void apply_hann_window(float* data, std::size_t n) {
|
||||
for (std::size_t i = 0; i < n; ++i) {
|
||||
float w = 0.5f - 0.5f * std::cos(2.0f * static_cast<float>(M_PI) * i / (n - 1));
|
||||
data[i] *= w;
|
||||
}
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
// Iterative Cooley-Tukey complex FFT (power-of-2 sizes only).
|
||||
struct Cpx { float r = 0.0f, i = 0.0f; };
|
||||
|
||||
void fft_inplace(Cpx* data, int n, bool inverse) {
|
||||
int j = 0;
|
||||
for (int i = 1; i < n; ++i) {
|
||||
int bit = n >> 1;
|
||||
for (; j & bit; bit >>= 1) j ^= bit;
|
||||
j ^= bit;
|
||||
if (i < j) std::swap(data[i], data[j]);
|
||||
}
|
||||
for (int len = 2; len <= n; len <<= 1) {
|
||||
float ang = 2.0f * static_cast<float>(M_PI) / len * (inverse ? 1.0f : -1.0f);
|
||||
float wlen_r = std::cos(ang);
|
||||
float wlen_i = std::sin(ang);
|
||||
for (int i = 0; i < n; i += len) {
|
||||
float wr = 1.0f, wi = 0.0f;
|
||||
for (int k = 0; k < len / 2; ++k) {
|
||||
int u = i + k;
|
||||
int v = i + k + len / 2;
|
||||
float ur = data[u].r, ui = data[u].i;
|
||||
float vr = data[v].r * wr - data[v].i * wi;
|
||||
float vi = data[v].r * wi + data[v].i * wr;
|
||||
data[u].r = ur + vr;
|
||||
data[u].i = ui + vi;
|
||||
data[v].r = ur - vr;
|
||||
data[v].i = ui - vi;
|
||||
float wnr = wr * wlen_r - wi * wlen_i;
|
||||
float wni = wr * wlen_i + wi * wlen_r;
|
||||
wr = wnr;
|
||||
wi = wni;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (inverse) {
|
||||
float inv_n = 1.0f / n;
|
||||
for (int i = 0; i < n; ++i) {
|
||||
data[i].r *= inv_n;
|
||||
data[i].i *= inv_n;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // anonymous namespace
|
||||
|
||||
void compute_power_spectrum(const float* frame, int n_fft, float* power_out) {
|
||||
int bins = n_fft / 2 + 1;
|
||||
std::vector<Cpx> data(n_fft);
|
||||
for (int i = 0; i < n_fft; ++i) {
|
||||
data[i].r = frame[i];
|
||||
data[i].i = 0.0f;
|
||||
}
|
||||
fft_inplace(data.data(), n_fft, false);
|
||||
for (int i = 0; i < bins; ++i) {
|
||||
power_out[i] = data[i].r * data[i].r + data[i].i * data[i].i;
|
||||
}
|
||||
}
|
||||
|
||||
bool load_mel_filter_bank(const std::string& path, int n_mels, int n_fft_bins,
|
||||
std::vector<float>& filter_bank) {
|
||||
std::ifstream f(path, std::ios::binary);
|
||||
if (!f) return false;
|
||||
std::size_t expected = static_cast<std::size_t>(n_mels) * n_fft_bins;
|
||||
filter_bank.resize(expected);
|
||||
f.read(reinterpret_cast<char*>(filter_bank.data()), expected * sizeof(float));
|
||||
return f.gcount() == static_cast<std::streamsize>(expected * sizeof(float));
|
||||
}
|
||||
|
||||
} // namespace acoustic
|
||||
@ -0,0 +1,238 @@
|
||||
#include "acoustic_analyzer/core/gunshot_classifier.h"
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <numeric>
|
||||
#include <memory>
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <windows.h>
|
||||
#endif
|
||||
|
||||
#include <onnxruntime_cxx_api.h>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
struct GunshotClassifier::Impl {
|
||||
ClassifierConfig config;
|
||||
std::vector<std::string> labels;
|
||||
bool loaded = false;
|
||||
|
||||
Ort::Env env;
|
||||
std::unique_ptr<Ort::Session> session;
|
||||
std::unique_ptr<Ort::SessionOptions> session_options;
|
||||
|
||||
explicit Impl(const ClassifierConfig& cfg)
|
||||
: config(cfg), env(ORT_LOGGING_LEVEL_WARNING, "acoustic") {
|
||||
session_options = std::make_unique<Ort::SessionOptions>();
|
||||
session_options->SetIntraOpNumThreads(1);
|
||||
session_options->SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_EXTENDED);
|
||||
}
|
||||
|
||||
bool LoadLabelMap(const std::string& path) {
|
||||
std::ifstream f(path);
|
||||
if (!f) return false;
|
||||
std::string content((std::istreambuf_iterator<char>(f)),
|
||||
std::istreambuf_iterator<char>());
|
||||
labels.clear();
|
||||
|
||||
// Try simple JSON object format: {"0": "ambient", "1": "threat"}
|
||||
if (content.find('{') != std::string::npos) {
|
||||
std::size_t pos = 0;
|
||||
while ((pos = content.find('"', pos)) != std::string::npos) {
|
||||
std::size_t key_start = pos + 1;
|
||||
std::size_t key_end = content.find('"', key_start);
|
||||
if (key_end == std::string::npos) break;
|
||||
std::string key = content.substr(key_start, key_end - key_start);
|
||||
int idx = -1;
|
||||
try { idx = std::stoi(key); } catch (...) {}
|
||||
|
||||
std::size_t colon = content.find(':', key_end);
|
||||
if (colon == std::string::npos) break;
|
||||
std::size_t val_quote = content.find('"', colon);
|
||||
if (val_quote == std::string::npos) break;
|
||||
std::size_t val_end = content.find('"', val_quote + 1);
|
||||
if (val_end == std::string::npos) break;
|
||||
std::string label = content.substr(val_quote + 1, val_end - val_quote - 1);
|
||||
|
||||
if (idx >= 0) {
|
||||
if (idx >= static_cast<int>(labels.size())) labels.resize(idx + 1);
|
||||
labels[idx] = label;
|
||||
}
|
||||
pos = val_end + 1;
|
||||
}
|
||||
} else {
|
||||
// Fallback: "index: label" line format
|
||||
std::size_t line_start = 0;
|
||||
while (line_start < content.size()) {
|
||||
std::size_t line_end = content.find('\n', line_start);
|
||||
if (line_end == std::string::npos) line_end = content.size();
|
||||
std::string line = content.substr(line_start, line_end - line_start);
|
||||
line_start = line_end + 1;
|
||||
|
||||
std::size_t pos = line.find(':');
|
||||
if (pos == std::string::npos) continue;
|
||||
int idx = 0;
|
||||
try {
|
||||
idx = std::stoi(line.substr(0, pos));
|
||||
} catch (...) {
|
||||
continue;
|
||||
}
|
||||
std::string label = line.substr(pos + 1);
|
||||
std::size_t start = label.find_first_not_of(" \t\r\n");
|
||||
std::size_t end = label.find_last_not_of(" \t\r\n");
|
||||
if (start != std::string::npos && end != std::string::npos) {
|
||||
label = label.substr(start, end - start + 1);
|
||||
} else {
|
||||
label.clear();
|
||||
}
|
||||
if (idx >= static_cast<int>(labels.size())) {
|
||||
labels.resize(idx + 1);
|
||||
}
|
||||
labels[idx] = label;
|
||||
}
|
||||
}
|
||||
|
||||
// Remove empty entries and compact
|
||||
std::vector<std::string> cleaned;
|
||||
for (const auto& l : labels) {
|
||||
if (!l.empty()) cleaned.push_back(l);
|
||||
}
|
||||
labels = std::move(cleaned);
|
||||
return !labels.empty();
|
||||
}
|
||||
|
||||
bool LoadModel(const std::string& path) {
|
||||
// Verify file exists before attempting ONNX load
|
||||
std::ifstream check(path, std::ios::binary);
|
||||
if (!check) {
|
||||
std::cerr << "[ERROR] Model file not found: " << path << std::endl;
|
||||
return false;
|
||||
}
|
||||
check.close();
|
||||
|
||||
#ifdef _WIN32
|
||||
int wlen = MultiByteToWideChar(CP_UTF8, 0, path.c_str(), -1, nullptr, 0);
|
||||
std::wstring wpath;
|
||||
if (wlen > 0) {
|
||||
wpath.resize(static_cast<std::size_t>(wlen));
|
||||
MultiByteToWideChar(CP_UTF8, 0, path.c_str(), -1, &wpath[0], wlen);
|
||||
}
|
||||
try {
|
||||
session = std::make_unique<Ort::Session>(env, wpath.c_str(), *session_options);
|
||||
} catch (const Ort::Exception& e) {
|
||||
std::cerr << "[ERROR] ONNX Runtime failed to load model: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
#else
|
||||
try {
|
||||
session = std::make_unique<Ort::Session>(env, path.c_str(), *session_options);
|
||||
} catch (const Ort::Exception& e) {
|
||||
std::cerr << "[ERROR] ONNX Runtime failed to load model: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Initialize() {
|
||||
if (!config.label_map_path.empty()) {
|
||||
if (!LoadLabelMap(config.label_map_path)) {
|
||||
// Continue even if label map fails; may have embedded labels
|
||||
}
|
||||
}
|
||||
if (!config.model_path.empty()) {
|
||||
if (!LoadModel(config.model_path)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
loaded = session != nullptr;
|
||||
return loaded;
|
||||
}
|
||||
|
||||
std::pair<std::string, float> Predict(const Eigen::MatrixXf& mel_spectrogram) {
|
||||
if (!loaded || session == nullptr) return {"unknown", 0.0f};
|
||||
|
||||
int n_mels = static_cast<int>(mel_spectrogram.rows());
|
||||
int n_frames = static_cast<int>(mel_spectrogram.cols());
|
||||
|
||||
// Flatten to std::vector<float> with layout [batch, time_frames, n_mels]
|
||||
std::vector<float> input_data(static_cast<std::size_t>(n_mels) * n_frames);
|
||||
for (int t = 0; t < n_frames; ++t) {
|
||||
for (int m = 0; m < n_mels; ++m) {
|
||||
input_data[t * n_mels + m] = mel_spectrogram(m, t);
|
||||
}
|
||||
}
|
||||
|
||||
// Cached memory info and I/O names (created once on first use)
|
||||
static Ort::MemoryInfo memory_info = Ort::MemoryInfo::CreateCpu(OrtArenaAllocator, OrtMemTypeDefault);
|
||||
static const char* input_names[] = {"input"};
|
||||
static const char* output_names[] = {"output"};
|
||||
|
||||
std::vector<int64_t> input_shape = {1, n_frames, n_mels};
|
||||
Ort::Value input_tensor = Ort::Value::CreateTensor<float>(
|
||||
memory_info, input_data.data(), input_data.size(),
|
||||
input_shape.data(), input_shape.size());
|
||||
|
||||
// Run inference
|
||||
auto output_tensors = session->Run(
|
||||
Ort::RunOptions{nullptr},
|
||||
input_names, &input_tensor, 1,
|
||||
output_names, 1);
|
||||
|
||||
// Get output
|
||||
float* output_data = output_tensors.front().GetTensorMutableData<float>();
|
||||
auto output_shape = output_tensors.front().GetTensorTypeAndShapeInfo().GetShape();
|
||||
std::size_t num_classes = labels.empty() ? static_cast<std::size_t>(output_shape.back()) : labels.size();
|
||||
if (num_classes == 0) num_classes = 2;
|
||||
|
||||
// Softmax
|
||||
std::vector<float> probs(num_classes);
|
||||
float max_val = output_data[0];
|
||||
for (std::size_t i = 1; i < num_classes; ++i) {
|
||||
if (output_data[i] > max_val) max_val = output_data[i];
|
||||
}
|
||||
float sum = 0.0f;
|
||||
for (std::size_t i = 0; i < num_classes; ++i) {
|
||||
probs[i] = std::exp(output_data[i] - max_val);
|
||||
sum += probs[i];
|
||||
}
|
||||
for (std::size_t i = 0; i < num_classes; ++i) {
|
||||
probs[i] /= sum;
|
||||
}
|
||||
|
||||
// Find best
|
||||
std::size_t best_idx = 0;
|
||||
for (std::size_t i = 1; i < num_classes; ++i) {
|
||||
if (probs[i] > probs[best_idx]) best_idx = i;
|
||||
}
|
||||
|
||||
if (best_idx >= labels.size()) return {"unknown", probs[best_idx]};
|
||||
return {labels[best_idx], probs[best_idx]};
|
||||
}
|
||||
};
|
||||
|
||||
GunshotClassifier::GunshotClassifier(const ClassifierConfig& config)
|
||||
: impl_(std::make_unique<Impl>(config)) {
|
||||
impl_->Initialize();
|
||||
}
|
||||
|
||||
GunshotClassifier::~GunshotClassifier() = default;
|
||||
GunshotClassifier::GunshotClassifier(GunshotClassifier&&) noexcept = default;
|
||||
GunshotClassifier& GunshotClassifier::operator=(GunshotClassifier&&) noexcept = default;
|
||||
|
||||
std::pair<std::string, float> GunshotClassifier::Predict(const Eigen::MatrixXf& mel_spectrogram) {
|
||||
return impl_->Predict(mel_spectrogram);
|
||||
}
|
||||
|
||||
const std::vector<std::string>& GunshotClassifier::Labels() const noexcept {
|
||||
return impl_->labels;
|
||||
}
|
||||
|
||||
bool GunshotClassifier::IsLoaded() const noexcept {
|
||||
return impl_->loaded;
|
||||
}
|
||||
|
||||
} // namespace acoustic
|
||||
@ -0,0 +1,395 @@
|
||||
#include "acoustic_analyzer/core/pipeline.h"
|
||||
#include "acoustic_analyzer/core/audio_buffer.h"
|
||||
#include "acoustic_analyzer/core/feature_extractor.h"
|
||||
#include "acoustic_analyzer/core/gunshot_classifier.h"
|
||||
#include "acoustic_analyzer/core/gcc_phat_localizer.h"
|
||||
#include "acoustic_analyzer/core/distance_estimator.h"
|
||||
#include "acoustic_analyzer/core/threat_tracker.h"
|
||||
|
||||
// yaml-cpp is optional: if absent, Pipeline still works but FromYaml returns defaults
|
||||
#if defined(__has_include)
|
||||
# if __has_include(<yaml-cpp/yaml.h>)
|
||||
# define ACOUSTIC_HAS_YAML_CPP 1
|
||||
# endif
|
||||
#endif
|
||||
|
||||
#ifdef ACOUSTIC_HAS_YAML_CPP
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#endif
|
||||
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include <numeric>
|
||||
#include <memory>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
struct Pipeline::Impl {
|
||||
PipelineConfig config;
|
||||
|
||||
std::unique_ptr<AudioBuffer> audio_buffer;
|
||||
std::unique_ptr<FeatureExtractor> feature_extractor;
|
||||
std::unique_ptr<GunshotClassifier> classifier;
|
||||
std::unique_ptr<GccPhatLocalizer> localizer;
|
||||
std::unique_ptr<DistanceEstimator> distance_estimator;
|
||||
std::unique_ptr<ThreatTracker> threat_tracker;
|
||||
|
||||
// Temporal smoothing state: per-class probability history
|
||||
std::vector<std::vector<float>> class_history_;
|
||||
int smoothing_window_ = 3;
|
||||
|
||||
explicit Impl(const PipelineConfig& cfg) : config(cfg) {
|
||||
smoothing_window_ = static_cast<int>(cfg.classifier.smoothing_window);
|
||||
if (smoothing_window_ < 1) smoothing_window_ = 1;
|
||||
ResetModules();
|
||||
}
|
||||
|
||||
void ResetModules() {
|
||||
std::size_t chunk_samples = static_cast<std::size_t>(config.sample_rate * config.chunk_duration);
|
||||
audio_buffer = std::make_unique<AudioBuffer>(chunk_samples,
|
||||
static_cast<int>(config.mic_array.num_mics));
|
||||
|
||||
feature_extractor = std::make_unique<FeatureExtractor>(
|
||||
static_cast<int>(config.sample_rate),
|
||||
static_cast<int>(config.n_fft),
|
||||
static_cast<int>(config.hop_length),
|
||||
static_cast<int>(config.n_mels),
|
||||
config.f_min,
|
||||
config.f_max,
|
||||
config.preemphasis);
|
||||
|
||||
classifier = std::make_unique<GunshotClassifier>(config.classifier);
|
||||
|
||||
if (config.mic_array.num_mics >= 2) {
|
||||
localizer = std::make_unique<GccPhatLocalizer>(
|
||||
config.mic_array,
|
||||
static_cast<int>(config.sample_rate),
|
||||
config.max_tdoa,
|
||||
config.interpolation_factor);
|
||||
}
|
||||
|
||||
distance_estimator = std::make_unique<DistanceEstimator>(config.distance);
|
||||
threat_tracker = std::make_unique<ThreatTracker>(
|
||||
config.min_event_interval, 15.0f, 5);
|
||||
|
||||
// Dynamic class history dimension from classifier labels
|
||||
int num_classes = static_cast<int>(classifier->Labels().size());
|
||||
if (num_classes == 0) num_classes = 2; // fallback
|
||||
class_history_.assign(num_classes, std::vector<float>());
|
||||
}
|
||||
|
||||
bool VadGate(const std::vector<float>& audio) {
|
||||
if (audio.empty()) return false;
|
||||
// Energy in dB
|
||||
float energy = 0.0f;
|
||||
for (float s : audio) energy += s * s;
|
||||
energy /= static_cast<float>(audio.size());
|
||||
float energy_db = 10.0f * std::log10(energy + 1e-10f);
|
||||
|
||||
// Zero-crossing rate
|
||||
std::size_t zcr = 0;
|
||||
for (std::size_t i = 1; i < audio.size(); ++i) {
|
||||
if ((audio[i] >= 0.0f) != (audio[i - 1] >= 0.0f)) ++zcr;
|
||||
}
|
||||
float zcr_rate = static_cast<float>(zcr) / (audio.size() - 1);
|
||||
|
||||
bool energy_ok = energy_db > -60.0f;
|
||||
bool zcr_ok = zcr_rate > 0.01f && zcr_rate < 0.5f;
|
||||
return energy_ok && zcr_ok;
|
||||
}
|
||||
|
||||
std::vector<float> TemporalSmoothing(const std::vector<float>& probs) {
|
||||
int num_classes = static_cast<int>(probs.size());
|
||||
if (num_classes == 0) return probs;
|
||||
if (static_cast<int>(class_history_.size()) != num_classes) {
|
||||
class_history_.assign(num_classes, std::vector<float>());
|
||||
}
|
||||
|
||||
std::vector<float> smoothed(num_classes);
|
||||
for (int i = 0; i < num_classes; ++i) {
|
||||
auto& hist = class_history_[i];
|
||||
hist.push_back(probs[i]);
|
||||
if (static_cast<int>(hist.size()) > smoothing_window_) {
|
||||
hist.erase(hist.begin());
|
||||
}
|
||||
float sum = 0.0f;
|
||||
for (float v : hist) sum += v;
|
||||
smoothed[i] = sum / static_cast<float>(hist.size());
|
||||
}
|
||||
return smoothed;
|
||||
}
|
||||
|
||||
AcousticFrame Process(const std::vector<float>& audio_samples) {
|
||||
AcousticFrame frame;
|
||||
frame.timestamp = std::chrono::steady_clock::now();
|
||||
frame.is_clear = true;
|
||||
|
||||
if (audio_samples.empty()) return frame;
|
||||
|
||||
// Push interleaved multi-channel audio into ring buffer
|
||||
audio_buffer->Push(audio_samples);
|
||||
|
||||
std::size_t chunk_samples = static_cast<std::size_t>(config.sample_rate * config.chunk_duration);
|
||||
|
||||
// VAD on first channel
|
||||
std::size_t avail = audio_buffer->Size();
|
||||
std::size_t offset = (avail > chunk_samples) ? (avail - chunk_samples) : 0;
|
||||
auto latest_flat = audio_buffer->Get(offset, chunk_samples);
|
||||
std::vector<float> latest;
|
||||
latest.reserve(latest_flat.size() / config.mic_array.num_mics);
|
||||
for (std::size_t i = 0; i < latest_flat.size(); i += config.mic_array.num_mics) {
|
||||
latest.push_back(latest_flat[i]);
|
||||
}
|
||||
if (!VadGate(latest)) {
|
||||
return frame;
|
||||
}
|
||||
|
||||
// Feature extraction: multi-channel Mel spectrogram
|
||||
auto mel_specs = feature_extractor->MelSpectrogramMultiChannel(
|
||||
audio_samples, config.mic_array.num_mics);
|
||||
|
||||
// Average across channels in frequency domain
|
||||
Eigen::MatrixXf avg_mel;
|
||||
if (!mel_specs.empty()) {
|
||||
avg_mel = mel_specs[0];
|
||||
for (std::size_t i = 1; i < mel_specs.size(); ++i) {
|
||||
avg_mel += mel_specs[i];
|
||||
}
|
||||
if (mel_specs.size() > 1) {
|
||||
avg_mel /= static_cast<float>(mel_specs.size());
|
||||
}
|
||||
}
|
||||
|
||||
// Classification
|
||||
auto [label, confidence] = classifier->Predict(avg_mel);
|
||||
|
||||
// Build probability vector for temporal smoothing
|
||||
const auto& labels = classifier->Labels();
|
||||
int num_classes = static_cast<int>(labels.size());
|
||||
if (num_classes == 0) num_classes = 2;
|
||||
std::vector<float> probs(num_classes, 0.0f);
|
||||
int label_idx = -1;
|
||||
for (int i = 0; i < num_classes; ++i) {
|
||||
if (i < static_cast<int>(labels.size()) && labels[i] == label) {
|
||||
label_idx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (label_idx >= 0) probs[label_idx] = confidence;
|
||||
auto smoothed = TemporalSmoothing(probs);
|
||||
|
||||
// Find best after smoothing
|
||||
float best_conf = 0.0f;
|
||||
int best_idx = 0;
|
||||
for (int i = 0; i < num_classes; ++i) {
|
||||
if (smoothed[i] > best_conf) {
|
||||
best_conf = smoothed[i];
|
||||
best_idx = i;
|
||||
}
|
||||
}
|
||||
|
||||
if (best_idx < static_cast<int>(labels.size()) &&
|
||||
best_conf >= config.confidence_threshold) {
|
||||
std::string best_label = labels[best_idx];
|
||||
|
||||
// Localization
|
||||
float azimuth = 0.0f;
|
||||
float elevation = 0.0f;
|
||||
if (localizer && config.mic_array.num_mics >= 2) {
|
||||
Eigen::MatrixXf audio_mat(static_cast<int>(chunk_samples),
|
||||
static_cast<int>(config.mic_array.num_mics));
|
||||
for (std::size_t ch = 0; ch < config.mic_array.num_mics; ++ch) {
|
||||
std::vector<float> ch_data;
|
||||
ch_data.reserve(latest_flat.size() / config.mic_array.num_mics);
|
||||
for (std::size_t i = ch; i < latest_flat.size(); i += config.mic_array.num_mics) {
|
||||
ch_data.push_back(latest_flat[i]);
|
||||
}
|
||||
for (std::size_t i = 0; i < ch_data.size(); ++i) {
|
||||
audio_mat(static_cast<int>(i), static_cast<int>(ch)) = ch_data[i];
|
||||
}
|
||||
}
|
||||
auto [az, el] = localizer->Localize(audio_mat);
|
||||
azimuth = az;
|
||||
elevation = el;
|
||||
}
|
||||
|
||||
// Distance estimation
|
||||
float distance = -1.0f;
|
||||
float distance_conf = 0.0f;
|
||||
if (distance_estimator) {
|
||||
float spl = distance_estimator->ComputeSpl(latest);
|
||||
float raw_distance = distance_estimator->Estimate(spl, best_label);
|
||||
distance = distance_estimator->UpdateKalman(raw_distance);
|
||||
distance_conf = 0.8f;
|
||||
}
|
||||
|
||||
AcousticThreat threat;
|
||||
threat.timestamp = frame.timestamp;
|
||||
threat.threat_id = "";
|
||||
threat.sound_type = best_label;
|
||||
threat.confidence = best_conf;
|
||||
threat.azimuth = azimuth;
|
||||
threat.elevation = elevation;
|
||||
threat.distance = distance;
|
||||
threat.distance_confidence = distance_conf;
|
||||
|
||||
auto filtered = threat_tracker->Update({threat});
|
||||
frame.threats = std::move(filtered);
|
||||
frame.is_clear = frame.threats.empty();
|
||||
}
|
||||
|
||||
return frame;
|
||||
}
|
||||
};
|
||||
|
||||
Pipeline::Pipeline(const PipelineConfig& config)
|
||||
: impl_(std::make_unique<Impl>(config)) {}
|
||||
|
||||
Pipeline::~Pipeline() = default;
|
||||
Pipeline::Pipeline(Pipeline&&) noexcept = default;
|
||||
Pipeline& Pipeline::operator=(Pipeline&&) noexcept = default;
|
||||
|
||||
AcousticFrame Pipeline::Process(const std::vector<float>& audio_samples) {
|
||||
return impl_->Process(audio_samples);
|
||||
}
|
||||
|
||||
PipelineConfig Pipeline::FromYaml(const std::string& yaml_path) {
|
||||
PipelineConfig config;
|
||||
#ifdef ACOUSTIC_HAS_YAML_CPP
|
||||
try {
|
||||
YAML::Node root = YAML::LoadFile(yaml_path);
|
||||
|
||||
if (root["audio"]) {
|
||||
auto audio = root["audio"];
|
||||
if (audio["sample_rate"])
|
||||
config.sample_rate = audio["sample_rate"].as<uint32_t>();
|
||||
if (audio["chunk_duration"])
|
||||
config.chunk_duration = audio["chunk_duration"].as<float>();
|
||||
if (audio["hop_duration"])
|
||||
config.hop_duration = audio["hop_duration"].as<float>();
|
||||
if (audio["n_channels"])
|
||||
config.mic_array.num_mics = audio["n_channels"].as<uint32_t>();
|
||||
}
|
||||
|
||||
if (root["source"]) {
|
||||
auto src = root["source"];
|
||||
if (src["type"]) src["type"].as<std::string>();
|
||||
}
|
||||
|
||||
if (root["mic_array"]) {
|
||||
auto mic = root["mic_array"];
|
||||
if (mic["num_mics"])
|
||||
config.mic_array.num_mics = mic["num_mics"].as<uint32_t>();
|
||||
if (mic["layout"])
|
||||
config.mic_array.layout = mic["layout"].as<std::string>();
|
||||
if (mic["spacing"])
|
||||
config.mic_array.spacing = mic["spacing"].as<float>();
|
||||
if (mic["positions"]) {
|
||||
config.mic_array.positions.clear();
|
||||
for (const auto& pos : mic["positions"]) {
|
||||
std::array<float, 3> p = {0.0f, 0.0f, 0.0f};
|
||||
if (pos.size() > 0) p[0] = pos[0].as<float>();
|
||||
if (pos.size() > 1) p[1] = pos[1].as<float>();
|
||||
if (pos.size() > 2) p[2] = pos[2].as<float>();
|
||||
config.mic_array.positions.push_back(p);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (root["features"]) {
|
||||
auto feat = root["features"];
|
||||
if (feat["n_mels"]) config.n_mels = feat["n_mels"].as<uint32_t>();
|
||||
if (feat["n_fft"]) {
|
||||
config.n_fft = feat["n_fft"].as<uint32_t>();
|
||||
}
|
||||
if (feat["hop_length"]) {
|
||||
config.hop_length = feat["hop_length"].as<uint32_t>();
|
||||
}
|
||||
if (feat["f_min"]) {
|
||||
config.f_min = feat["f_min"].as<float>();
|
||||
}
|
||||
if (feat["f_max"]) {
|
||||
config.f_max = feat["f_max"].as<float>();
|
||||
}
|
||||
if (feat["preemphasis"]) {
|
||||
config.preemphasis = feat["preemphasis"].as<float>();
|
||||
}
|
||||
}
|
||||
|
||||
if (root["classifier"]) {
|
||||
auto cls = root["classifier"];
|
||||
if (cls["model_path"])
|
||||
config.classifier.model_path = cls["model_path"].as<std::string>();
|
||||
if (cls["label_map_path"])
|
||||
config.classifier.label_map_path = cls["label_map_path"].as<std::string>();
|
||||
if (cls["threshold"])
|
||||
config.classifier.threshold = cls["threshold"].as<float>();
|
||||
if (cls["smoothing_window"])
|
||||
config.classifier.smoothing_window = cls["smoothing_window"].as<uint32_t>();
|
||||
}
|
||||
|
||||
if (root["localization"]) {
|
||||
auto loc = root["localization"];
|
||||
if (loc["max_tdoa"]) {
|
||||
config.max_tdoa = loc["max_tdoa"].as<float>();
|
||||
}
|
||||
if (loc["interpolation_factor"]) {
|
||||
config.interpolation_factor = loc["interpolation_factor"].as<int>();
|
||||
}
|
||||
}
|
||||
|
||||
if (root["distance"]) {
|
||||
auto dist = root["distance"];
|
||||
if (dist["attenuation_alpha"])
|
||||
config.distance.attenuation_alpha = dist["attenuation_alpha"].as<float>();
|
||||
if (dist["kalman_process_noise"])
|
||||
config.distance.kalman_process_noise =
|
||||
dist["kalman_process_noise"].as<float>();
|
||||
if (dist["kalman_measurement_noise"])
|
||||
config.distance.kalman_measurement_noise =
|
||||
dist["kalman_measurement_noise"].as<float>();
|
||||
if (dist["reference_spl"]) {
|
||||
auto ref = dist["reference_spl"];
|
||||
if (ref["threat"])
|
||||
config.distance.ref_spl_gunshot = ref["threat"].as<float>();
|
||||
if (ref["gunshot"])
|
||||
config.distance.ref_spl_gunshot = ref["gunshot"].as<float>();
|
||||
if (ref["artillery"])
|
||||
config.distance.ref_spl_artillery = ref["artillery"].as<float>();
|
||||
if (ref["explosion"])
|
||||
config.distance.ref_spl_explosion = ref["explosion"].as<float>();
|
||||
}
|
||||
}
|
||||
|
||||
if (root["output"]) {
|
||||
auto out = root["output"];
|
||||
if (out["publish_rate"]) {
|
||||
// Not stored directly
|
||||
}
|
||||
if (out["min_event_interval"])
|
||||
config.min_event_interval = out["min_event_interval"].as<float>();
|
||||
}
|
||||
} catch (const YAML::Exception& e) {
|
||||
(void)e;
|
||||
// On parse error, return defaults
|
||||
}
|
||||
#else
|
||||
(void)yaml_path;
|
||||
#endif
|
||||
return config;
|
||||
}
|
||||
|
||||
void Pipeline::Reset() {
|
||||
impl_->audio_buffer->Clear();
|
||||
impl_->threat_tracker->Reset();
|
||||
impl_->distance_estimator->Reset();
|
||||
for (auto& hist : impl_->class_history_) {
|
||||
hist.clear();
|
||||
}
|
||||
}
|
||||
|
||||
const PipelineConfig& Pipeline::Config() const noexcept {
|
||||
return impl_->config;
|
||||
}
|
||||
|
||||
} // namespace acoustic
|
||||
@ -0,0 +1,128 @@
|
||||
#include "acoustic_analyzer/core/threat_tracker.h"
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
ThreatTracker::ThreatTracker(float min_event_interval,
|
||||
float azimuth_gate_deg,
|
||||
int max_missing_frames)
|
||||
: min_event_interval_(min_event_interval),
|
||||
azimuth_gate_deg_(azimuth_gate_deg),
|
||||
max_missing_frames_(max_missing_frames),
|
||||
id_counter_(0) {}
|
||||
|
||||
std::string ThreatTracker::GenerateId() {
|
||||
std::ostringstream oss;
|
||||
oss << "THREAT-" << std::setw(4) << std::setfill('0') << ++id_counter_;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
void ThreatTracker::Reset() {
|
||||
tracks_.clear();
|
||||
id_counter_ = 0;
|
||||
}
|
||||
|
||||
std::size_t ThreatTracker::NumTracked() const noexcept {
|
||||
return tracks_.size();
|
||||
}
|
||||
|
||||
std::vector<AcousticThreat> ThreatTracker::Update(
|
||||
const std::vector<AcousticThreat>& detected_threats) {
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
|
||||
// Increment missing frames for all existing tracks
|
||||
for (auto& track : tracks_) {
|
||||
track.missing_frames++;
|
||||
}
|
||||
|
||||
// Nearest-neighbor data association (azimuth difference < azimuth_gate_deg_)
|
||||
std::vector<bool> det_matched(detected_threats.size(), false);
|
||||
for (auto& track : tracks_) {
|
||||
if (track.missing_frames > max_missing_frames_) continue;
|
||||
|
||||
float best_diff = azimuth_gate_deg_;
|
||||
std::size_t best_idx = detected_threats.size();
|
||||
|
||||
for (std::size_t d = 0; d < detected_threats.size(); ++d) {
|
||||
if (det_matched[d]) continue;
|
||||
float diff = std::abs(detected_threats[d].azimuth - track.last_azimuth);
|
||||
if (diff > 180.0f) diff = 360.0f - diff;
|
||||
if (diff < best_diff) {
|
||||
best_diff = diff;
|
||||
best_idx = d;
|
||||
}
|
||||
}
|
||||
|
||||
if (best_idx < detected_threats.size()) {
|
||||
const auto& det = detected_threats[best_idx];
|
||||
track.last_azimuth = det.azimuth;
|
||||
track.last_elevation = det.elevation;
|
||||
track.sound_type = det.sound_type;
|
||||
track.confidence = det.confidence;
|
||||
track.distance = det.distance;
|
||||
track.distance_confidence = det.distance_confidence;
|
||||
track.missing_frames = 0;
|
||||
det_matched[best_idx] = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize new tracks for unmatched detections
|
||||
for (std::size_t d = 0; d < detected_threats.size(); ++d) {
|
||||
if (det_matched[d]) continue;
|
||||
TrackedThreat track;
|
||||
track.threat_id = GenerateId();
|
||||
track.last_azimuth = detected_threats[d].azimuth;
|
||||
track.last_elevation = detected_threats[d].elevation;
|
||||
track.sound_type = detected_threats[d].sound_type;
|
||||
track.confidence = detected_threats[d].confidence;
|
||||
track.distance = detected_threats[d].distance;
|
||||
track.distance_confidence = detected_threats[d].distance_confidence;
|
||||
track.missing_frames = 0;
|
||||
track.ever_published = false;
|
||||
tracks_.push_back(std::move(track));
|
||||
}
|
||||
|
||||
// Remove stale tracks (连续 5 帧未检测淘汰)
|
||||
tracks_.erase(
|
||||
std::remove_if(tracks_.begin(), tracks_.end(),
|
||||
[this](const TrackedThreat& t) {
|
||||
return t.missing_frames > max_missing_frames_;
|
||||
}),
|
||||
tracks_.end());
|
||||
|
||||
// Filter by min_event_interval and return active threats
|
||||
std::vector<AcousticThreat> output;
|
||||
for (auto& track : tracks_) {
|
||||
if (track.missing_frames != 0) continue;
|
||||
|
||||
bool should_publish = !track.ever_published;
|
||||
if (!should_publish) {
|
||||
float elapsed = std::chrono::duration_cast<std::chrono::duration<float>>(
|
||||
now - track.last_publish_time).count();
|
||||
should_publish = (elapsed >= min_event_interval_);
|
||||
}
|
||||
|
||||
if (should_publish) {
|
||||
track.ever_published = true;
|
||||
track.last_publish_time = now;
|
||||
AcousticThreat threat;
|
||||
threat.timestamp = now;
|
||||
threat.threat_id = track.threat_id;
|
||||
threat.sound_type = track.sound_type;
|
||||
threat.confidence = track.confidence;
|
||||
threat.azimuth = track.last_azimuth;
|
||||
threat.elevation = track.last_elevation;
|
||||
threat.distance = track.distance;
|
||||
threat.distance_confidence = track.distance_confidence;
|
||||
output.push_back(threat);
|
||||
}
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
} // namespace acoustic
|
||||
@ -0,0 +1,167 @@
|
||||
#include "acoustic_analyzer/io/wav_file_source.h"
|
||||
#include <cstring>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
struct WavHeader {
|
||||
char riff[4];
|
||||
uint32_t file_size;
|
||||
char wave[4];
|
||||
char fmt[4];
|
||||
uint32_t fmt_size;
|
||||
uint16_t audio_format;
|
||||
uint16_t num_channels;
|
||||
uint32_t sample_rate;
|
||||
uint32_t byte_rate;
|
||||
uint16_t block_align;
|
||||
uint16_t bits_per_sample;
|
||||
};
|
||||
|
||||
WavFileSource::WavFileSource(const std::string& path, int target_sample_rate)
|
||||
: path_(path), target_sample_rate_(target_sample_rate), fp_(nullptr) {}
|
||||
|
||||
WavFileSource::~WavFileSource() {
|
||||
close();
|
||||
}
|
||||
|
||||
void WavFileSource::close() {
|
||||
if (fp_) {
|
||||
fclose(fp_);
|
||||
fp_ = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
bool WavFileSource::open() {
|
||||
close();
|
||||
fp_ = fopen(path_.c_str(), "rb");
|
||||
if (!fp_) return false;
|
||||
if (!parse_wav_header()) {
|
||||
close();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool WavFileSource::parse_wav_header() {
|
||||
WavHeader hdr;
|
||||
if (fread(&hdr, sizeof(hdr), 1, fp_) != 1) return false;
|
||||
if (std::memcmp(hdr.riff, "RIFF", 4) != 0 || std::memcmp(hdr.wave, "WAVE", 4) != 0) return false;
|
||||
if (hdr.audio_format != 1 && hdr.audio_format != 3) return false; // PCM or IEEE float
|
||||
|
||||
num_channels_ = hdr.num_channels;
|
||||
file_sample_rate_ = hdr.sample_rate;
|
||||
bits_per_sample_ = hdr.bits_per_sample;
|
||||
|
||||
// Find data chunk
|
||||
char chunk_id[4];
|
||||
uint32_t chunk_size;
|
||||
while (fread(chunk_id, 4, 1, fp_) == 1) {
|
||||
if (fread(&chunk_size, 4, 1, fp_) != 1) return false;
|
||||
if (std::memcmp(chunk_id, "data", 4) == 0) {
|
||||
data_start_ = ftell(fp_);
|
||||
int bytes_per_sample = bits_per_sample_ / 8;
|
||||
if (bytes_per_sample == 0) bytes_per_sample = 1;
|
||||
total_samples_ = chunk_size / (num_channels_ * bytes_per_sample);
|
||||
read_pos_ = 0;
|
||||
return true;
|
||||
}
|
||||
fseek(fp_, chunk_size, SEEK_CUR);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
float convert_sample(const uint8_t* raw, int bytes, bool is_float) {
|
||||
if (is_float) {
|
||||
if (bytes == 4) {
|
||||
float val;
|
||||
std::memcpy(&val, raw, 4);
|
||||
return val;
|
||||
}
|
||||
return 0.0f;
|
||||
}
|
||||
// Integer PCM
|
||||
if (bytes == 1) {
|
||||
int32_t val = static_cast<int32_t>(raw[0]) - 128;
|
||||
return val / 128.0f;
|
||||
} else if (bytes == 2) {
|
||||
int16_t val;
|
||||
std::memcpy(&val, raw, 2);
|
||||
return val / 32768.0f;
|
||||
} else if (bytes == 3) {
|
||||
int32_t val = (static_cast<int32_t>(raw[2]) << 16)
|
||||
| (static_cast<int32_t>(raw[1]) << 8)
|
||||
| static_cast<int32_t>(raw[0]);
|
||||
// Sign-extend 24-bit to 32-bit
|
||||
if (val & 0x800000) val |= 0xFF000000;
|
||||
return val / 8388608.0f;
|
||||
} else if (bytes >= 4) {
|
||||
int32_t val;
|
||||
std::memcpy(&val, raw, 4);
|
||||
return std::max(-1.0f, std::min(1.0f, val / 2147483648.0f));
|
||||
}
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
} // anonymous namespace
|
||||
|
||||
void WavFileSource::resample_if_needed(std::vector<float>& mono, int src_rate, int dst_rate) {
|
||||
if (src_rate == dst_rate || dst_rate <= 0) return;
|
||||
double ratio = static_cast<double>(dst_rate) / src_rate;
|
||||
std::size_t new_len = static_cast<std::size_t>(mono.size() * ratio);
|
||||
std::vector<float> resampled(new_len);
|
||||
for (std::size_t i = 0; i < new_len; ++i) {
|
||||
double src_idx = i / ratio;
|
||||
std::size_t idx0 = static_cast<std::size_t>(src_idx);
|
||||
std::size_t idx1 = std::min(idx0 + 1, mono.size() - 1);
|
||||
double frac = src_idx - idx0;
|
||||
resampled[i] = static_cast<float>(mono[idx0] * (1.0 - frac) + mono[idx1] * frac);
|
||||
}
|
||||
mono.swap(resampled);
|
||||
}
|
||||
|
||||
std::size_t WavFileSource::read(std::vector<std::vector<float>>& out, std::size_t max_samples) {
|
||||
if (!fp_) return 0;
|
||||
|
||||
std::size_t samples_to_read = std::min(max_samples, total_samples_ - read_pos_);
|
||||
if (samples_to_read == 0) return 0;
|
||||
|
||||
int bytes_per_sample = bits_per_sample_ / 8;
|
||||
if (bytes_per_sample == 0) bytes_per_sample = 1;
|
||||
int block_align = num_channels_ * bytes_per_sample;
|
||||
bool is_float = (bits_per_sample_ == 32); // Assume 32-bit is float if audio_format==3
|
||||
|
||||
std::vector<uint8_t> raw(samples_to_read * block_align);
|
||||
fseek(fp_, static_cast<long>(data_start_ + read_pos_ * block_align), SEEK_SET);
|
||||
std::size_t read_blocks = fread(raw.data(), block_align, samples_to_read, fp_);
|
||||
if (read_blocks == 0) return 0;
|
||||
|
||||
out.resize(num_channels_);
|
||||
for (int ch = 0; ch < num_channels_; ++ch) {
|
||||
out[ch].resize(read_blocks);
|
||||
}
|
||||
|
||||
for (std::size_t i = 0; i < read_blocks; ++i) {
|
||||
for (int ch = 0; ch < num_channels_; ++ch) {
|
||||
out[ch][i] = convert_sample(&raw[i * block_align + ch * bytes_per_sample],
|
||||
bytes_per_sample, is_float);
|
||||
}
|
||||
}
|
||||
|
||||
read_pos_ += read_blocks;
|
||||
|
||||
// Resample if needed
|
||||
if (target_sample_rate_ > 0 && target_sample_rate_ != file_sample_rate_) {
|
||||
for (int ch = 0; ch < num_channels_; ++ch) {
|
||||
resample_if_needed(out[ch], file_sample_rate_, target_sample_rate_);
|
||||
}
|
||||
return out[0].size();
|
||||
}
|
||||
return read_blocks;
|
||||
}
|
||||
|
||||
} // namespace acoustic
|
||||
@ -0,0 +1,15 @@
|
||||
#include <ros/ros.h>
|
||||
#include "acoustic_analyzer/ros/acoustic_node.h"
|
||||
#include "acoustic_analyzer/ros/threat_publisher.h"
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "acoustic_analyzer_node");
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
acoustic::ThreatPublisher publisher(nh);
|
||||
auto node = acoustic::create_acoustic_node(nh, pnh, &publisher);
|
||||
|
||||
node->run();
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,202 @@
|
||||
#include "acoustic_analyzer/ros/acoustic_node.h"
|
||||
#include "acoustic_analyzer/core/pipeline.h"
|
||||
#include "acoustic_analyzer/io/wav_file_source.h"
|
||||
#ifndef _WIN32
|
||||
#include "acoustic_analyzer/io/mobile_phone_source.h"
|
||||
#endif
|
||||
#include <ros/ros.h>
|
||||
#include <ros/package.h>
|
||||
#include <std_msgs/Float32MultiArray.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <regex>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
class AcousticNode {
|
||||
public:
|
||||
AcousticNode(ros::NodeHandle& nh, ros::NodeHandle& pnh, ThreatPublisher* publisher)
|
||||
: nh_(nh), pnh_(pnh), threat_publisher_(publisher) {
|
||||
load_params();
|
||||
init_pipeline();
|
||||
init_source();
|
||||
}
|
||||
|
||||
void run() {
|
||||
ros::Rate rate(params_.publish_rate);
|
||||
while (ros::ok()) {
|
||||
ros::spinOnce();
|
||||
if (source_type_ == "wav_file" && wav_source_) {
|
||||
process_wav_source();
|
||||
}
|
||||
rate.sleep();
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle pnh_;
|
||||
ros::Subscriber audio_sub_;
|
||||
ThreatPublisher* threat_publisher_ = nullptr;
|
||||
|
||||
std::string source_type_;
|
||||
std::unique_ptr<Pipeline> pipeline_;
|
||||
std::unique_ptr<WavFileSource> wav_source_;
|
||||
#ifndef _WIN32
|
||||
std::unique_ptr<MobilePhoneSource> phone_source_;
|
||||
#endif
|
||||
|
||||
struct Params {
|
||||
int sample_rate = 16000;
|
||||
float chunk_duration = 2.0f;
|
||||
float hop_duration = 0.5f;
|
||||
int n_channels = 4;
|
||||
std::string source_type;
|
||||
std::string mobile_phone_topic;
|
||||
float mobile_phone_timeout = 10.0f;
|
||||
std::string wav_file_path;
|
||||
std::string model_path;
|
||||
std::string label_map_path;
|
||||
float publish_rate = 10.0f;
|
||||
} params_;
|
||||
|
||||
/**
|
||||
* @brief 展开 YAML 中的 $(find package_name) 语法为绝对路径
|
||||
*/
|
||||
static std::string expandFindPath(const std::string& path) {
|
||||
static const std::regex findRegex(R"(\$\(find\s+([^)]+)\))");
|
||||
std::smatch match;
|
||||
std::string result = path;
|
||||
if (std::regex_search(path, match, findRegex) && match.size() > 1) {
|
||||
std::string pkgPath = ros::package::getPath(match[1].str());
|
||||
if (!pkgPath.empty()) {
|
||||
result = match.prefix().str() + pkgPath + match.suffix().str();
|
||||
} else {
|
||||
ROS_WARN("ros::package::getPath(%s) returned empty, keeping raw path", match[1].str().c_str());
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void load_params() {
|
||||
std::string yaml_path;
|
||||
pnh_.param<std::string>("config_file", yaml_path, "");
|
||||
if (!yaml_path.empty()) {
|
||||
try {
|
||||
YAML::Node config = YAML::LoadFile(yaml_path);
|
||||
params_.sample_rate = config["audio"]["sample_rate"].as<int>(16000);
|
||||
params_.chunk_duration = config["audio"]["chunk_duration"].as<float>(2.0f);
|
||||
params_.hop_duration = config["audio"]["hop_duration"].as<float>(0.5f);
|
||||
params_.n_channels = config["audio"]["n_channels"].as<int>(4);
|
||||
params_.source_type = config["source"]["type"].as<std::string>("mic_array");
|
||||
params_.mobile_phone_topic = config["source"]["mobile_phone_topic"].as<std::string>("/mobile_phone/audio");
|
||||
params_.mobile_phone_timeout = config["source"]["mobile_phone_timeout"].as<float>(10.0f);
|
||||
params_.wav_file_path = config["source"]["wav_file_path"].as<std::string>("");
|
||||
params_.model_path = expandFindPath(config["classifier"]["model_path"].as<std::string>(""));
|
||||
params_.label_map_path = expandFindPath(config["classifier"]["label_map_path"].as<std::string>(""));
|
||||
params_.publish_rate = config["output"]["publish_rate"].as<float>(10.0f);
|
||||
} catch (const std::exception& e) {
|
||||
ROS_WARN("Failed to load YAML config: %s", e.what());
|
||||
}
|
||||
}
|
||||
source_type_ = params_.source_type;
|
||||
}
|
||||
|
||||
void init_pipeline() {
|
||||
PipelineConfig config;
|
||||
config.sample_rate = static_cast<uint32_t>(params_.sample_rate);
|
||||
config.chunk_duration = params_.chunk_duration;
|
||||
config.hop_duration = params_.hop_duration;
|
||||
config.n_mels = 64;
|
||||
config.classifier.model_path = params_.model_path;
|
||||
config.classifier.label_map_path = params_.label_map_path;
|
||||
config.classifier.threshold = 0.7f;
|
||||
config.classifier.smoothing_window = 3;
|
||||
config.mic_array.num_mics = static_cast<uint32_t>(params_.n_channels);
|
||||
config.mic_array.layout = "cross";
|
||||
config.mic_array.spacing = 0.15f;
|
||||
pipeline_ = std::make_unique<Pipeline>(config);
|
||||
}
|
||||
|
||||
void init_source() {
|
||||
if (source_type_ == "mic_array") {
|
||||
audio_sub_ = nh_.subscribe("/microphone_array/audio", 10,
|
||||
&AcousticNode::on_mic_array_audio, this);
|
||||
} else if (source_type_ == "mobile_phone") {
|
||||
audio_sub_ = nh_.subscribe(params_.mobile_phone_topic, 10,
|
||||
&AcousticNode::on_mobile_phone_audio, this);
|
||||
} else if (source_type_ == "wav_file") {
|
||||
if (!params_.wav_file_path.empty()) {
|
||||
wav_source_ = std::make_unique<WavFileSource>(params_.wav_file_path, params_.sample_rate);
|
||||
if (!wav_source_->open()) {
|
||||
ROS_ERROR("Failed to open WAV file: %s", params_.wav_file_path.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Convert multi-channel vector-of-vectors to flat interleaved format
|
||||
std::vector<float> flatten_audio(const std::vector<std::vector<float>>& audio, int channels) {
|
||||
if (audio.empty() || channels == 0) return {};
|
||||
size_t samples = audio[0].size();
|
||||
std::vector<float> flat(samples * channels);
|
||||
for (size_t s = 0; s < samples; ++s) {
|
||||
for (int ch = 0; ch < channels; ++ch) {
|
||||
flat[s * channels + ch] = (ch < static_cast<int>(audio.size()) && s < audio[ch].size())
|
||||
? audio[ch][s] : 0.0f;
|
||||
}
|
||||
}
|
||||
return flat;
|
||||
}
|
||||
|
||||
void publish_if_available(const AcousticFrame& frame) {
|
||||
if (threat_publisher_ && !frame.is_clear) {
|
||||
threat_publisher_->Publish(frame);
|
||||
}
|
||||
}
|
||||
|
||||
void on_mic_array_audio(const std_msgs::Float32MultiArray::ConstPtr& msg) {
|
||||
if (msg->layout.dim.size() < 2) return;
|
||||
int channels = static_cast<int>(msg->layout.dim[0].size);
|
||||
int samples = static_cast<int>(msg->layout.dim[1].size);
|
||||
if (channels == 0 || samples == 0) return;
|
||||
|
||||
// Assuming data is interleaved or [channels x samples] row-major
|
||||
std::vector<float> flat(msg->data.begin(), msg->data.end());
|
||||
auto frame = pipeline_->Process(flat);
|
||||
publish_if_available(frame);
|
||||
}
|
||||
|
||||
void on_mobile_phone_audio(const std_msgs::Float32MultiArray::ConstPtr& msg) {
|
||||
if (msg->data.empty()) return;
|
||||
std::vector<float> flat(msg->data.begin(), msg->data.end());
|
||||
auto frame = pipeline_->Process(flat);
|
||||
publish_if_available(frame);
|
||||
}
|
||||
|
||||
void process_wav_source() {
|
||||
if (!wav_source_ || !wav_source_->is_open()) return;
|
||||
size_t chunk_samples = static_cast<size_t>(params_.chunk_duration * params_.sample_rate);
|
||||
std::vector<std::vector<float>> audio;
|
||||
size_t got = wav_source_->read(audio, chunk_samples);
|
||||
if (got == 0) {
|
||||
ROS_INFO("WAV playback finished");
|
||||
ros::shutdown();
|
||||
return;
|
||||
}
|
||||
auto flat = flatten_audio(audio, static_cast<int>(wav_source_->num_channels()));
|
||||
auto frame = pipeline_->Process(flat);
|
||||
publish_if_available(frame);
|
||||
}
|
||||
};
|
||||
|
||||
std::unique_ptr<AcousticNode> create_acoustic_node(
|
||||
ros::NodeHandle& nh,
|
||||
ros::NodeHandle& pnh,
|
||||
ThreatPublisher* publisher) {
|
||||
return std::make_unique<AcousticNode>(nh, pnh, publisher);
|
||||
}
|
||||
|
||||
} // namespace acoustic
|
||||
@ -0,0 +1,45 @@
|
||||
#include "acoustic_analyzer/ros/threat_publisher.h"
|
||||
#include <acoustic_analyzer/AcousticThreat.h>
|
||||
|
||||
namespace acoustic {
|
||||
|
||||
struct ThreatPublisher::Impl {
|
||||
ros::Publisher pub_;
|
||||
std::size_t num_published_ = 0;
|
||||
};
|
||||
|
||||
ThreatPublisher::ThreatPublisher(ros::NodeHandle& nh, const std::string& topic)
|
||||
: impl_(std::make_unique<Impl>()) {
|
||||
impl_->pub_ = nh.advertise<acoustic_analyzer::AcousticThreatArray>(topic, 10);
|
||||
}
|
||||
|
||||
ThreatPublisher::~ThreatPublisher() = default;
|
||||
ThreatPublisher::ThreatPublisher(ThreatPublisher&&) noexcept = default;
|
||||
ThreatPublisher& ThreatPublisher::operator=(ThreatPublisher&&) noexcept = default;
|
||||
|
||||
void ThreatPublisher::Publish(const AcousticFrame& frame) {
|
||||
acoustic_analyzer::AcousticThreatArray msg;
|
||||
msg.header.stamp = ros::Time::now();
|
||||
msg.header.frame_id = "acoustic_array";
|
||||
|
||||
for (const auto& t : frame.threats) {
|
||||
acoustic_analyzer::AcousticThreat threat_msg;
|
||||
threat_msg.threat_id = t.threat_id;
|
||||
threat_msg.sound_type = t.sound_type;
|
||||
threat_msg.confidence = t.confidence;
|
||||
threat_msg.azimuth = t.azimuth;
|
||||
threat_msg.elevation = t.elevation;
|
||||
threat_msg.distance = t.distance;
|
||||
threat_msg.distance_confidence = t.distance_confidence;
|
||||
msg.threats.push_back(threat_msg);
|
||||
}
|
||||
|
||||
impl_->pub_.publish(msg);
|
||||
++impl_->num_published_;
|
||||
}
|
||||
|
||||
std::size_t ThreatPublisher::NumPublished() const noexcept {
|
||||
return impl_->num_published_;
|
||||
}
|
||||
|
||||
} // namespace acoustic
|
||||
@ -0,0 +1,245 @@
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <filesystem>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <numeric>
|
||||
#include <map>
|
||||
#include <chrono>
|
||||
|
||||
#include "acoustic_analyzer/core/feature_extractor.h"
|
||||
#include "acoustic_analyzer/core/gunshot_classifier.h"
|
||||
#include "acoustic_analyzer/core/distance_estimator.h"
|
||||
#include "acoustic_analyzer/io/wav_file_source.h"
|
||||
|
||||
namespace fs = std::filesystem;
|
||||
using namespace acoustic;
|
||||
|
||||
struct Prediction {
|
||||
std::string file_path;
|
||||
std::string true_label;
|
||||
std::string pred_label;
|
||||
float confidence = 0.0f;
|
||||
float distance = -1.0f;
|
||||
};
|
||||
|
||||
void print_usage(const char* prog) {
|
||||
std::cerr << "Usage: " << prog << " <file_or_dir> [--model <onnx>] [--label_map <json>] [--threshold <float>]" << std::endl;
|
||||
std::cerr << " --model <path> ONNX model path (default: models/gunshot_classifier.onnx)" << std::endl;
|
||||
std::cerr << " --label_map <path> Label map file (default: models/label_map.json)" << std::endl;
|
||||
std::cerr << " --threshold <float> Detection threshold (default: 0.5)" << std::endl;
|
||||
}
|
||||
|
||||
bool ends_with(const std::string& s, const std::string& suffix) {
|
||||
if (s.size() < suffix.size()) return false;
|
||||
return s.compare(s.size() - suffix.size(), suffix.size(), suffix) == 0;
|
||||
}
|
||||
|
||||
std::string get_parent_folder_name(const std::string& path) {
|
||||
fs::path p(path);
|
||||
if (p.has_parent_path()) {
|
||||
return p.parent_path().filename().string();
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
float compute_spl(const std::vector<float>& audio) {
|
||||
if (audio.empty()) return -100.0f;
|
||||
float rms = 0.0f;
|
||||
for (float s : audio) rms += s * s;
|
||||
rms = std::sqrt(rms / audio.size());
|
||||
return 20.0f * std::log10(rms + 1e-10f) + 94.0f;
|
||||
}
|
||||
|
||||
Prediction process_file(const std::string& path,
|
||||
GunshotClassifier& classifier,
|
||||
FeatureExtractor& extractor) {
|
||||
Prediction result;
|
||||
result.file_path = path;
|
||||
result.true_label = get_parent_folder_name(path);
|
||||
|
||||
WavFileSource wav(path);
|
||||
if (!wav.open()) {
|
||||
std::cerr << "[SKIP] Cannot open: " << path << std::endl;
|
||||
result.pred_label = "error";
|
||||
return result;
|
||||
}
|
||||
|
||||
int sr = wav.sample_rate();
|
||||
std::vector<std::vector<float>> audio;
|
||||
size_t chunk = static_cast<size_t>(sr * 2.0);
|
||||
size_t got = wav.read(audio, chunk);
|
||||
if (got == 0 || audio.empty()) {
|
||||
result.pred_label = "empty";
|
||||
return result;
|
||||
}
|
||||
|
||||
Eigen::MatrixXf mel = extractor.MelSpectrogram(audio[0]);
|
||||
|
||||
auto [label, confidence] = classifier.Predict(mel);
|
||||
result.pred_label = label;
|
||||
result.confidence = confidence;
|
||||
|
||||
if (label != "ambient" && confidence > 0.5f) {
|
||||
DistanceConfig dc;
|
||||
dc.ref_spl_gunshot = 150.0f;
|
||||
dc.attenuation_alpha = 0.6f;
|
||||
DistanceEstimator de(dc);
|
||||
float spl = compute_spl(audio[0]);
|
||||
float dist = de.Estimate(spl, label);
|
||||
dist = de.UpdateKalman(dist);
|
||||
result.distance = dist;
|
||||
}
|
||||
|
||||
std::cout << "File: " << fs::path(path).filename().string()
|
||||
<< " | True: " << result.true_label
|
||||
<< " | Pred: " << label
|
||||
<< " | Conf: " << std::fixed << std::setprecision(4) << confidence
|
||||
<< " | Dist: " << std::setprecision(2) << result.distance << "m" << std::endl;
|
||||
return result;
|
||||
}
|
||||
|
||||
void collect_wav_files(const std::string& target, std::vector<std::string>& out) {
|
||||
if (fs::is_regular_file(target) && ends_with(target, ".wav")) {
|
||||
out.push_back(target);
|
||||
return;
|
||||
}
|
||||
if (!fs::is_directory(target)) return;
|
||||
|
||||
for (const auto& entry : fs::recursive_directory_iterator(target)) {
|
||||
if (entry.is_regular_file() && ends_with(entry.path().string(), ".wav")) {
|
||||
out.push_back(entry.path().string());
|
||||
}
|
||||
}
|
||||
std::sort(out.begin(), out.end());
|
||||
}
|
||||
|
||||
void print_report(const std::vector<Prediction>& results) {
|
||||
std::map<std::string, int> total_by_true;
|
||||
std::map<std::string, int> correct_by_true;
|
||||
std::map<std::string, float> conf_sum_by_true;
|
||||
std::map<std::string, std::map<std::string, int>> confusion;
|
||||
|
||||
int total = 0, correct = 0;
|
||||
for (const auto& r : results) {
|
||||
if (r.pred_label == "error" || r.pred_label == "empty") continue;
|
||||
total++;
|
||||
total_by_true[r.true_label]++;
|
||||
conf_sum_by_true[r.true_label] += r.confidence;
|
||||
confusion[r.true_label][r.pred_label]++;
|
||||
if (r.true_label == r.pred_label) {
|
||||
correct++;
|
||||
correct_by_true[r.true_label]++;
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "\n==========================================" << std::endl;
|
||||
std::cout << " VALIDATION REPORT" << std::endl;
|
||||
std::cout << "==========================================" << std::endl;
|
||||
std::cout << "Total samples: " << total << std::endl;
|
||||
std::cout << "Correct: " << correct << std::endl;
|
||||
std::cout << "Accuracy: " << std::fixed << std::setprecision(2)
|
||||
<< (total > 0 ? 100.0f * correct / total : 0.0f) << "%" << std::endl;
|
||||
|
||||
std::cout << "\nPer-class breakdown:" << std::endl;
|
||||
for (const auto& kv : total_by_true) {
|
||||
const std::string& cls = kv.first;
|
||||
int cls_total = kv.second;
|
||||
int cls_correct = correct_by_true[cls];
|
||||
float avg_conf = conf_sum_by_true[cls] / cls_total;
|
||||
std::cout << " " << std::setw(10) << std::left << cls
|
||||
<< " Count: " << std::setw(3) << cls_total
|
||||
<< " Correct: " << std::setw(3) << cls_correct
|
||||
<< " Acc: " << std::setw(6) << std::fixed << std::setprecision(2)
|
||||
<< (100.0f * cls_correct / cls_total) << "%"
|
||||
<< " AvgConf: " << std::setprecision(4) << avg_conf << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "\nConfusion matrix (rows=true, cols=pred):" << std::endl;
|
||||
std::vector<std::string> labels;
|
||||
for (const auto& row : confusion) labels.push_back(row.first);
|
||||
for (const auto& row : confusion) {
|
||||
for (const auto& col : row.second) {
|
||||
if (std::find(labels.begin(), labels.end(), col.first) == labels.end()) {
|
||||
labels.push_back(col.first);
|
||||
}
|
||||
}
|
||||
}
|
||||
std::sort(labels.begin(), labels.end());
|
||||
|
||||
std::cout << std::setw(12) << " ";
|
||||
for (const auto& l : labels) std::cout << std::setw(10) << l;
|
||||
std::cout << std::endl;
|
||||
for (const auto& true_l : labels) {
|
||||
std::cout << std::setw(10) << std::left << true_l << " ";
|
||||
for (const auto& pred_l : labels) {
|
||||
int count = confusion.count(true_l) ? confusion[true_l].count(pred_l) ? confusion[true_l].at(pred_l) : 0 : 0;
|
||||
std::cout << std::setw(10) << count;
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
std::cout << "==========================================" << std::endl;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
if (argc < 2 || std::strcmp(argv[1], "--help") == 0 || std::strcmp(argv[1], "-h") == 0) {
|
||||
print_usage(argv[0]);
|
||||
return argc < 2 ? 1 : 0;
|
||||
}
|
||||
|
||||
std::string target = argv[1];
|
||||
std::string model_path = "models/gunshot_classifier.onnx";
|
||||
std::string label_map_path = "models/label_map.json";
|
||||
float threshold = 0.5f;
|
||||
|
||||
for (int i = 2; i < argc; ++i) {
|
||||
if (std::strcmp(argv[i], "--model") == 0 && i + 1 < argc) model_path = argv[++i];
|
||||
else if (std::strcmp(argv[i], "--label_map") == 0 && i + 1 < argc) label_map_path = argv[++i];
|
||||
else if (std::strcmp(argv[i], "--threshold") == 0 && i + 1 < argc) threshold = std::stof(argv[++i]);
|
||||
}
|
||||
|
||||
ClassifierConfig cc;
|
||||
cc.model_path = model_path;
|
||||
cc.label_map_path = label_map_path;
|
||||
cc.threshold = threshold;
|
||||
cc.smoothing_window = 1;
|
||||
GunshotClassifier classifier(cc);
|
||||
if (!classifier.IsLoaded()) {
|
||||
std::cerr << "[ERROR] Failed to load classifier model: " << model_path << std::endl;
|
||||
return 1;
|
||||
}
|
||||
std::cout << "Model loaded: " << model_path << std::endl;
|
||||
std::cout << "Labels: ";
|
||||
for (const auto& l : classifier.Labels()) std::cout << l << " ";
|
||||
std::cout << "\n" << std::endl;
|
||||
|
||||
std::vector<std::string> files;
|
||||
collect_wav_files(target, files);
|
||||
if (files.empty()) {
|
||||
std::cerr << "No .wav files found in: " << target << std::endl;
|
||||
return 1;
|
||||
}
|
||||
std::cout << "Found " << files.size() << " WAV file(s)." << std::endl;
|
||||
|
||||
FeatureExtractor extractor(16000, 2048, 512, 64, 0.0f, 8000.0f, 0.97f);
|
||||
|
||||
std::vector<Prediction> results;
|
||||
results.reserve(files.size());
|
||||
double total_ms = 0.0;
|
||||
for (const auto& f : files) {
|
||||
auto t0 = std::chrono::steady_clock::now();
|
||||
results.push_back(process_file(f, classifier, extractor));
|
||||
auto t1 = std::chrono::steady_clock::now();
|
||||
total_ms += std::chrono::duration<double, std::milli>(t1 - t0).count();
|
||||
}
|
||||
|
||||
std::cout << "\nTotal inference time: " << std::fixed << std::setprecision(2)
|
||||
<< total_ms << " ms"
|
||||
<< " | Avg per file: " << (files.empty() ? 0.0 : total_ms / files.size())
|
||||
<< " ms" << std::endl;
|
||||
|
||||
print_report(results);
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,331 @@
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <filesystem>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <numeric>
|
||||
#include <map>
|
||||
#include <chrono>
|
||||
#include <cstring>
|
||||
|
||||
#include "acoustic_analyzer/core/pipeline.h"
|
||||
#include "acoustic_analyzer/io/wav_file_source.h"
|
||||
|
||||
namespace fs = std::filesystem;
|
||||
using namespace acoustic;
|
||||
|
||||
struct Prediction {
|
||||
std::string file_path;
|
||||
std::string true_label;
|
||||
std::string pred_label;
|
||||
float confidence = 0.0f;
|
||||
float azimuth = 0.0f;
|
||||
float elevation = 0.0f;
|
||||
float distance = -1.0f;
|
||||
bool detected = false;
|
||||
};
|
||||
|
||||
void print_usage(const char* prog) {
|
||||
std::cerr << "Usage: " << prog << " <file_or_dir> [options]" << std::endl;
|
||||
std::cerr << "Options:" << std::endl;
|
||||
std::cerr << " --model <path> ONNX model path (default: models/gunshot_classifier.onnx)" << std::endl;
|
||||
std::cerr << " --label_map <path> Label map file (default: models/label_map.json)" << std::endl;
|
||||
std::cerr << " --threshold <float> Detection threshold (default: 0.5)" << std::endl;
|
||||
std::cerr << " --num_mics <int> Number of channels in WAV (default: 4)" << std::endl;
|
||||
std::cerr << " --spacing <float> Mic spacing in meters (default: 0.15)" << std::endl;
|
||||
std::cerr << " --layout <str> Array layout: cross/linear/circular (default: cross)" << std::endl;
|
||||
std::cerr << " --ref_spl <float> Reference SPL for distance estimation (default: 150)" << std::endl;
|
||||
std::cerr << " --ground_azimuth <float> Ground-truth azimuth for error calc (optional)" << std::endl;
|
||||
std::cerr << " --ground_distance <float> Ground-truth distance for error calc (optional)" << std::endl;
|
||||
}
|
||||
|
||||
bool ends_with(const std::string& s, const std::string& suffix) {
|
||||
if (s.size() < suffix.size()) return false;
|
||||
return s.compare(s.size() - suffix.size(), suffix.size(), suffix) == 0;
|
||||
}
|
||||
|
||||
std::string get_parent_folder_name(const std::string& path) {
|
||||
fs::path p(path);
|
||||
if (p.has_parent_path()) {
|
||||
return p.parent_path().filename().string();
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
// Convert vector-of-vectors [channels][samples] to flat interleaved [ch0_s0, ch1_s0, ...]
|
||||
std::vector<float> flatten_audio(const std::vector<std::vector<float>>& audio, int channels) {
|
||||
if (audio.empty() || channels == 0) return {};
|
||||
size_t samples = audio[0].size();
|
||||
std::vector<float> flat(samples * channels);
|
||||
for (size_t s = 0; s < samples; ++s) {
|
||||
for (int ch = 0; ch < channels; ++ch) {
|
||||
flat[s * channels + ch] = (ch < static_cast<int>(audio.size()) && s < audio[ch].size())
|
||||
? audio[ch][s] : 0.0f;
|
||||
}
|
||||
}
|
||||
return flat;
|
||||
}
|
||||
|
||||
Prediction process_file(const std::string& path,
|
||||
Pipeline& pipeline,
|
||||
int num_mics,
|
||||
float ground_azimuth,
|
||||
float ground_distance) {
|
||||
Prediction result;
|
||||
result.file_path = path;
|
||||
result.true_label = get_parent_folder_name(path);
|
||||
|
||||
WavFileSource wav(path);
|
||||
if (!wav.open()) {
|
||||
std::cerr << "[SKIP] Cannot open: " << path << std::endl;
|
||||
result.pred_label = "error";
|
||||
return result;
|
||||
}
|
||||
|
||||
int sr = wav.sample_rate();
|
||||
int file_ch = wav.num_channels();
|
||||
if (file_ch != num_mics) {
|
||||
std::cerr << "[WARN] " << path << " has " << file_ch
|
||||
<< " channels, expected " << num_mics << std::endl;
|
||||
}
|
||||
// Use actual file channels if mismatch, but pipeline config should match
|
||||
int effective_channels = (file_ch < num_mics) ? file_ch : num_mics;
|
||||
|
||||
size_t chunk_samples = static_cast<size_t>(sr * pipeline.Config().chunk_duration);
|
||||
std::vector<std::vector<float>> audio;
|
||||
size_t got = wav.read(audio, chunk_samples);
|
||||
if (got == 0 || audio.empty()) {
|
||||
result.pred_label = "empty";
|
||||
return result;
|
||||
}
|
||||
|
||||
// Process only the first chunk (like demo_offline)
|
||||
// For sliding-window analysis, call Process() on hop-sized chunks
|
||||
auto flat = flatten_audio(audio, effective_channels);
|
||||
auto frame = pipeline.Process(flat);
|
||||
|
||||
if (!frame.is_clear && !frame.threats.empty()) {
|
||||
const auto& t = frame.threats[0];
|
||||
result.detected = true;
|
||||
result.pred_label = t.sound_type;
|
||||
result.confidence = t.confidence;
|
||||
result.azimuth = t.azimuth;
|
||||
result.elevation = t.elevation;
|
||||
result.distance = t.distance;
|
||||
} else {
|
||||
result.pred_label = "ambient";
|
||||
result.confidence = 0.0f;
|
||||
}
|
||||
|
||||
std::cout << "File: " << fs::path(path).filename().string()
|
||||
<< " | True: " << result.true_label
|
||||
<< " | Pred: " << result.pred_label
|
||||
<< " | Conf: " << std::fixed << std::setprecision(4) << result.confidence;
|
||||
if (result.detected) {
|
||||
std::cout << " | Az: " << std::setprecision(2) << result.azimuth << "°"
|
||||
<< " | El: " << std::setprecision(2) << result.elevation << "°"
|
||||
<< " | Dist: " << std::setprecision(2) << result.distance << "m";
|
||||
if (ground_azimuth >= 0.0f) {
|
||||
float az_err = std::fabs(result.azimuth - ground_azimuth);
|
||||
if (az_err > 180.0f) az_err = 360.0f - az_err;
|
||||
std::cout << " | AzErr: " << az_err << "°";
|
||||
}
|
||||
if (ground_distance >= 0.0f) {
|
||||
std::cout << " | DistErr: " << std::fabs(result.distance - ground_distance) << "m";
|
||||
}
|
||||
}
|
||||
std::cout << std::endl;
|
||||
return result;
|
||||
}
|
||||
|
||||
void collect_wav_files(const std::string& target, std::vector<std::string>& out) {
|
||||
if (fs::is_regular_file(target) && ends_with(target, ".wav")) {
|
||||
out.push_back(target);
|
||||
return;
|
||||
}
|
||||
if (!fs::is_directory(target)) return;
|
||||
|
||||
for (const auto& entry : fs::recursive_directory_iterator(target)) {
|
||||
if (entry.is_regular_file() && ends_with(entry.path().string(), ".wav")) {
|
||||
out.push_back(entry.path().string());
|
||||
}
|
||||
}
|
||||
std::sort(out.begin(), out.end());
|
||||
}
|
||||
|
||||
void print_report(const std::vector<Prediction>& results,
|
||||
float ground_azimuth,
|
||||
float ground_distance) {
|
||||
std::map<std::string, int> total_by_true;
|
||||
std::map<std::string, int> correct_by_true;
|
||||
std::map<std::string, float> conf_sum_by_true;
|
||||
std::map<std::string, std::map<std::string, int>> confusion;
|
||||
|
||||
int total = 0, correct = 0;
|
||||
int detected_count = 0;
|
||||
float az_err_sum = 0.0f, dist_err_sum = 0.0f;
|
||||
int az_err_count = 0, dist_err_count = 0;
|
||||
|
||||
for (const auto& r : results) {
|
||||
if (r.pred_label == "error" || r.pred_label == "empty") continue;
|
||||
total++;
|
||||
total_by_true[r.true_label]++;
|
||||
conf_sum_by_true[r.true_label] += r.confidence;
|
||||
confusion[r.true_label][r.pred_label]++;
|
||||
if (r.true_label == r.pred_label) {
|
||||
correct++;
|
||||
correct_by_true[r.true_label]++;
|
||||
}
|
||||
if (r.detected) {
|
||||
detected_count++;
|
||||
if (ground_azimuth >= 0.0f) {
|
||||
float az_err = std::fabs(r.azimuth - ground_azimuth);
|
||||
if (az_err > 180.0f) az_err = 360.0f - az_err;
|
||||
az_err_sum += az_err;
|
||||
az_err_count++;
|
||||
}
|
||||
if (ground_distance >= 0.0f) {
|
||||
dist_err_sum += std::fabs(r.distance - ground_distance);
|
||||
dist_err_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::cout << "\n==========================================" << std::endl;
|
||||
std::cout << " MULTICHANNEL VALIDATION REPORT" << std::endl;
|
||||
std::cout << "==========================================" << std::endl;
|
||||
std::cout << "Total samples: " << total << std::endl;
|
||||
std::cout << "Correct: " << correct << std::endl;
|
||||
std::cout << "Accuracy: " << std::fixed << std::setprecision(2)
|
||||
<< (total > 0 ? 100.0f * correct / total : 0.0f) << "%" << std::endl;
|
||||
std::cout << "Detected frames: " << detected_count << std::endl;
|
||||
|
||||
if (az_err_count > 0) {
|
||||
std::cout << "Azimuth RMSE: " << std::setprecision(2)
|
||||
<< std::sqrt(az_err_sum / az_err_count) << "°" << std::endl;
|
||||
}
|
||||
if (dist_err_count > 0) {
|
||||
std::cout << "Distance MAE: " << std::setprecision(2)
|
||||
<< (dist_err_sum / dist_err_count) << "m" << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "\nPer-class breakdown:" << std::endl;
|
||||
for (const auto& kv : total_by_true) {
|
||||
const std::string& cls = kv.first;
|
||||
int cls_total = kv.second;
|
||||
int cls_correct = correct_by_true[cls];
|
||||
float avg_conf = conf_sum_by_true[cls] / cls_total;
|
||||
std::cout << " " << std::setw(10) << std::left << cls
|
||||
<< " Count: " << std::setw(3) << cls_total
|
||||
<< " Correct: " << std::setw(3) << cls_correct
|
||||
<< " Acc: " << std::setw(6) << std::fixed << std::setprecision(2)
|
||||
<< (100.0f * cls_correct / cls_total) << "%"
|
||||
<< " AvgConf: " << std::setprecision(4) << avg_conf << std::endl;
|
||||
}
|
||||
|
||||
std::cout << "\nConfusion matrix (rows=true, cols=pred):" << std::endl;
|
||||
std::vector<std::string> labels;
|
||||
for (const auto& row : confusion) labels.push_back(row.first);
|
||||
for (const auto& row : confusion) {
|
||||
for (const auto& col : row.second) {
|
||||
if (std::find(labels.begin(), labels.end(), col.first) == labels.end()) {
|
||||
labels.push_back(col.first);
|
||||
}
|
||||
}
|
||||
}
|
||||
std::sort(labels.begin(), labels.end());
|
||||
|
||||
std::cout << std::setw(12) << " ";
|
||||
for (const auto& l : labels) std::cout << std::setw(10) << l;
|
||||
std::cout << std::endl;
|
||||
for (const auto& true_l : labels) {
|
||||
std::cout << std::setw(10) << std::left << true_l << " ";
|
||||
for (const auto& pred_l : labels) {
|
||||
int count = confusion.count(true_l) ? confusion[true_l].count(pred_l) ? confusion[true_l].at(pred_l) : 0 : 0;
|
||||
std::cout << std::setw(10) << count;
|
||||
}
|
||||
std::cout << std::endl;
|
||||
}
|
||||
std::cout << "==========================================" << std::endl;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
if (argc < 2 || std::strcmp(argv[1], "--help") == 0 || std::strcmp(argv[1], "-h") == 0) {
|
||||
print_usage(argv[0]);
|
||||
return argc < 2 ? 1 : 0;
|
||||
}
|
||||
|
||||
std::string target = argv[1];
|
||||
std::string model_path = "models/gunshot_classifier.onnx";
|
||||
std::string label_map_path = "models/label_map.json";
|
||||
float threshold = 0.5f;
|
||||
int num_mics = 4;
|
||||
float spacing = 0.15f;
|
||||
std::string layout = "cross";
|
||||
float ref_spl = 150.0f;
|
||||
float ground_azimuth = -1.0f;
|
||||
float ground_distance = -1.0f;
|
||||
|
||||
for (int i = 2; i < argc; ++i) {
|
||||
if (std::strcmp(argv[i], "--model") == 0 && i + 1 < argc) model_path = argv[++i];
|
||||
else if (std::strcmp(argv[i], "--label_map") == 0 && i + 1 < argc) label_map_path = argv[++i];
|
||||
else if (std::strcmp(argv[i], "--threshold") == 0 && i + 1 < argc) threshold = std::stof(argv[++i]);
|
||||
else if (std::strcmp(argv[i], "--num_mics") == 0 && i + 1 < argc) num_mics = std::stoi(argv[++i]);
|
||||
else if (std::strcmp(argv[i], "--spacing") == 0 && i + 1 < argc) spacing = std::stof(argv[++i]);
|
||||
else if (std::strcmp(argv[i], "--layout") == 0 && i + 1 < argc) layout = argv[++i];
|
||||
else if (std::strcmp(argv[i], "--ref_spl") == 0 && i + 1 < argc) ref_spl = std::stof(argv[++i]);
|
||||
else if (std::strcmp(argv[i], "--ground_azimuth") == 0 && i + 1 < argc) ground_azimuth = std::stof(argv[++i]);
|
||||
else if (std::strcmp(argv[i], "--ground_distance") == 0 && i + 1 < argc) ground_distance = std::stof(argv[++i]);
|
||||
}
|
||||
|
||||
// Build PipelineConfig directly (no yaml-cpp needed)
|
||||
PipelineConfig config;
|
||||
config.sample_rate = 16000;
|
||||
config.chunk_duration = 2.0f;
|
||||
config.hop_duration = 0.5f;
|
||||
config.n_mels = 64;
|
||||
config.confidence_threshold = threshold;
|
||||
config.classifier.model_path = model_path;
|
||||
config.classifier.label_map_path = label_map_path;
|
||||
config.classifier.threshold = threshold;
|
||||
config.classifier.smoothing_window = 1; // offline: no temporal smoothing
|
||||
config.mic_array.num_mics = static_cast<uint32_t>(num_mics);
|
||||
config.mic_array.layout = layout;
|
||||
config.mic_array.spacing = spacing;
|
||||
config.distance.ref_spl_gunshot = ref_spl;
|
||||
config.distance.ref_spl_artillery = ref_spl;
|
||||
config.distance.ref_spl_explosion = ref_spl;
|
||||
|
||||
Pipeline pipeline(config);
|
||||
|
||||
std::vector<std::string> files;
|
||||
collect_wav_files(target, files);
|
||||
if (files.empty()) {
|
||||
std::cerr << "No .wav files found in: " << target << std::endl;
|
||||
return 1;
|
||||
}
|
||||
std::cout << "Found " << files.size() << " WAV file(s)." << std::endl;
|
||||
std::cout << "Channels: " << num_mics << ", Layout: " << layout
|
||||
<< ", Spacing: " << spacing << "m" << std::endl;
|
||||
std::cout << std::endl;
|
||||
|
||||
std::vector<Prediction> results;
|
||||
results.reserve(files.size());
|
||||
double total_ms = 0.0;
|
||||
for (const auto& f : files) {
|
||||
auto t0 = std::chrono::steady_clock::now();
|
||||
results.push_back(process_file(f, pipeline, num_mics, ground_azimuth, ground_distance));
|
||||
auto t1 = std::chrono::steady_clock::now();
|
||||
total_ms += std::chrono::duration<double, std::milli>(t1 - t0).count();
|
||||
}
|
||||
|
||||
std::cout << "\nTotal inference time: " << std::fixed << std::setprecision(2)
|
||||
<< total_ms << " ms"
|
||||
<< " | Avg per file: " << (files.empty() ? 0.0 : total_ms / files.size())
|
||||
<< " ms" << std::endl;
|
||||
|
||||
print_report(results, ground_azimuth, ground_distance);
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,51 @@
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <algorithm>
|
||||
#include "acoustic_analyzer/core/feature_extractor.h"
|
||||
#include "acoustic_analyzer/io/wav_file_source.h"
|
||||
|
||||
using namespace acoustic;
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
if (argc < 2) {
|
||||
std::cerr << "Usage: " << argv[0] << " <wav_file> [output_bin]" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
WavFileSource wav(argv[1]);
|
||||
if (!wav.open()) {
|
||||
std::cerr << "Failed to open WAV" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::vector<std::vector<float>> audio;
|
||||
wav.read(audio, wav.total_samples());
|
||||
if (audio.empty()) {
|
||||
std::cerr << "Empty audio" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
FeatureExtractor ext(wav.sample_rate(), 2048, 512, 64);
|
||||
Eigen::MatrixXf mel = ext.MelSpectrogram(audio[0]);
|
||||
|
||||
std::cout << "Extracted Mel shape: " << mel.rows() << " x " << mel.cols() << std::endl;
|
||||
|
||||
// Flatten for min/max stats
|
||||
std::vector<float> mel_flat(mel.size());
|
||||
for (int i = 0; i < mel.rows(); ++i) {
|
||||
for (int j = 0; j < mel.cols(); ++j) {
|
||||
mel_flat[i * mel.cols() + j] = mel(i, j);
|
||||
}
|
||||
}
|
||||
std::cout << "Min: " << *std::min_element(mel_flat.begin(), mel_flat.end())
|
||||
<< " Max: " << *std::max_element(mel_flat.begin(), mel_flat.end()) << std::endl;
|
||||
|
||||
if (argc >= 3) {
|
||||
std::ofstream out(argv[2], std::ios::binary);
|
||||
out.write(reinterpret_cast<const char*>(mel_flat.data()), mel_flat.size() * sizeof(float));
|
||||
std::cout << "Saved to " << argv[2] << std::endl;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,67 @@
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include "acoustic_analyzer/core/gunshot_classifier.h"
|
||||
#include "acoustic_analyzer/core/feature_extractor.h"
|
||||
#include "acoustic_analyzer/io/wav_file_source.h"
|
||||
|
||||
using namespace acoustic;
|
||||
|
||||
void print_usage(const char* prog) {
|
||||
std::cerr << "Usage: " << prog << " --model <onnx> --wav <wav_file> [--label_map <json>]" << std::endl;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
std::string model_path;
|
||||
std::string wav_path;
|
||||
std::string label_map_path;
|
||||
|
||||
for (int i = 1; i < argc; ++i) {
|
||||
if (std::strcmp(argv[i], "--model") == 0 && i + 1 < argc) {
|
||||
model_path = argv[++i];
|
||||
} else if (std::strcmp(argv[i], "--wav") == 0 && i + 1 < argc) {
|
||||
wav_path = argv[++i];
|
||||
} else if (std::strcmp(argv[i], "--label_map") == 0 && i + 1 < argc) {
|
||||
label_map_path = argv[++i];
|
||||
}
|
||||
}
|
||||
|
||||
if (model_path.empty() || wav_path.empty()) {
|
||||
print_usage(argv[0]);
|
||||
return 1;
|
||||
}
|
||||
|
||||
WavFileSource wav(wav_path);
|
||||
if (!wav.open()) {
|
||||
std::cerr << "Failed to open WAV: " << wav_path << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::vector<std::vector<float>> audio;
|
||||
size_t n = wav.read(audio, wav.total_samples());
|
||||
if (n == 0 || audio.empty()) {
|
||||
std::cerr << "Empty WAV file" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
FeatureExtractor extractor(wav.sample_rate(), 2048, 512, 64);
|
||||
Eigen::MatrixXf mel = extractor.MelSpectrogram(audio[0]);
|
||||
|
||||
ClassifierConfig cc;
|
||||
cc.model_path = model_path;
|
||||
cc.label_map_path = label_map_path;
|
||||
cc.threshold = 0.7f;
|
||||
cc.smoothing_window = 3;
|
||||
GunshotClassifier classifier(cc);
|
||||
if (!classifier.IsLoaded()) {
|
||||
std::cerr << "Classifier init failed" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
auto [label, confidence] = classifier.Predict(mel);
|
||||
std::cout << "Label: " << label << std::endl;
|
||||
std::cout << "Confidence: " << confidence << std::endl;
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,170 @@
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
#include "acoustic_analyzer/core/audio_buffer.h"
|
||||
#include "acoustic_analyzer/core/feature_extractor.h"
|
||||
#include "acoustic_analyzer/core/gcc_phat_localizer.h"
|
||||
#include "acoustic_analyzer/core/distance_estimator.h"
|
||||
#include "acoustic_analyzer/core/threat_tracker.h"
|
||||
#include "acoustic_analyzer/core/fft_utils.h"
|
||||
|
||||
using namespace acoustic;
|
||||
|
||||
bool near(float a, float b, float eps = 1e-3f) {
|
||||
return std::abs(a - b) < eps;
|
||||
}
|
||||
|
||||
void test_audio_buffer() {
|
||||
AudioBuffer buf(100, 2);
|
||||
std::vector<float> data(200);
|
||||
for (int i = 0; i < 200; ++i) data[i] = static_cast<float>(i);
|
||||
buf.Push(data);
|
||||
assert(buf.Size() == 100);
|
||||
assert(buf.NumChannels() == 2);
|
||||
|
||||
auto popped = buf.Pop(10);
|
||||
assert(popped.size() == 10 * 2);
|
||||
assert(near(popped[0], 0.0f)); // ch0, sample 0 (oldest)
|
||||
assert(near(popped[1], 1.0f)); // ch1, sample 0
|
||||
std::cout << "[PASS] audio_buffer" << std::endl;
|
||||
}
|
||||
|
||||
void test_preemphasis() {
|
||||
float in[] = {1.0f, 0.5f, 0.25f};
|
||||
float out[3];
|
||||
apply_preemphasis(in, out, 3, 0.97f);
|
||||
assert(near(out[0], 1.0f));
|
||||
assert(near(out[1], 0.5f - 0.97f * 1.0f, 1e-4f));
|
||||
std::cout << "[PASS] preemphasis" << std::endl;
|
||||
}
|
||||
|
||||
void test_gcc_phat_cross_array() {
|
||||
MicArrayConfig config;
|
||||
config.num_mics = 4;
|
||||
config.layout = "cross";
|
||||
config.spacing = 0.15f;
|
||||
GccPhatLocalizer loc(config, 16000, 0.00044f, 4);
|
||||
|
||||
// Simulate a signal arriving from 90 degrees (right)
|
||||
int n = 512;
|
||||
Eigen::MatrixXf audio(n, 4);
|
||||
for (int i = 0; i < n; ++i) {
|
||||
float s = std::sin(2.0f * static_cast<float>(M_PI) * 1000.0f * i / 16000.0f);
|
||||
audio(i, 0) = s;
|
||||
audio(i, 1) = s;
|
||||
audio(i, 2) = s;
|
||||
audio(i, 3) = s;
|
||||
}
|
||||
float delay_samples = 0.15f / 343.0f * 16000.0f;
|
||||
for (int i = 0; i < n; ++i) {
|
||||
int idx = static_cast<int>(i + delay_samples);
|
||||
if (idx < n) audio(idx, 1) = audio(i, 0);
|
||||
idx = static_cast<int>(i - delay_samples);
|
||||
if (idx >= 0) audio(idx, 3) = audio(i, 0);
|
||||
}
|
||||
|
||||
auto [azimuth, elevation] = loc.Localize(audio);
|
||||
assert(azimuth >= 0.0f && azimuth < 360.0f);
|
||||
std::cout << "[PASS] gcc_phat_cross_array (azimuth=" << azimuth << ")" << std::endl;
|
||||
}
|
||||
|
||||
void test_distance_estimator() {
|
||||
DistanceConfig config;
|
||||
config.ref_spl_gunshot = 150.0f;
|
||||
config.attenuation_alpha = 0.6f;
|
||||
DistanceEstimator est(config);
|
||||
float dist = est.Estimate(120.0f, "gunshot");
|
||||
assert(dist > 0.0f && dist < 5000.0f);
|
||||
std::cout << "[PASS] distance_estimator (dist=" << dist << "m)" << std::endl;
|
||||
}
|
||||
|
||||
void test_threat_tracker() {
|
||||
ThreatTracker tracker(0.3f, 15.0f, 5);
|
||||
AcousticThreat t1;
|
||||
t1.sound_type = "gunshot";
|
||||
t1.confidence = 0.9f;
|
||||
t1.azimuth = 45.0f;
|
||||
t1.elevation = 0.0f;
|
||||
t1.distance = 100.0f;
|
||||
t1.distance_confidence = 0.8f;
|
||||
|
||||
auto out = tracker.Update({t1});
|
||||
assert(!out.empty());
|
||||
assert(out[0].sound_type == "gunshot");
|
||||
assert(out[0].threat_id == "THREAT-0001");
|
||||
std::cout << "[PASS] threat_tracker" << std::endl;
|
||||
}
|
||||
|
||||
void test_feature_extractor_shape() {
|
||||
FeatureExtractor ext(16000, 2048, 512, 64);
|
||||
std::vector<float> audio(16000 * 2, 0.0f); // 2 seconds
|
||||
for (int i = 0; i < 32000; ++i) {
|
||||
audio[i] = std::sin(2.0f * static_cast<float>(M_PI) * 500.0f * i / 16000.0f) * 0.1f;
|
||||
}
|
||||
Eigen::MatrixXf mel = ext.MelSpectrogram(audio);
|
||||
assert(mel.rows() == 64);
|
||||
assert(mel.cols() == 63); // padded/truncated to 63 frames
|
||||
std::cout << "[PASS] feature_extractor_shape (" << mel.rows() << " x " << mel.cols() << ")" << std::endl;
|
||||
}
|
||||
|
||||
void test_audio_buffer_overflow() {
|
||||
AudioBuffer buf(10, 1);
|
||||
std::vector<float> data(25, 1.0f);
|
||||
buf.Push(data);
|
||||
assert(buf.Size() == 10); // should cap at capacity
|
||||
auto popped = buf.Pop(5);
|
||||
assert(popped.size() == 5);
|
||||
assert(buf.Size() == 5);
|
||||
std::cout << "[PASS] audio_buffer_overflow" << std::endl;
|
||||
}
|
||||
|
||||
void test_distance_estimator_multi_class() {
|
||||
DistanceConfig config;
|
||||
config.ref_spl_gunshot = 150.0f;
|
||||
config.ref_spl_artillery = 180.0f;
|
||||
config.ref_spl_explosion = 170.0f;
|
||||
config.attenuation_alpha = 0.6f;
|
||||
DistanceEstimator est(config);
|
||||
float d1 = est.Estimate(120.0f, "gunshot");
|
||||
float d2 = est.Estimate(120.0f, "artillery");
|
||||
float d3 = est.Estimate(120.0f, "explosion");
|
||||
assert(d2 > d1); // artillery has higher ref SPL -> farther estimated distance at same SPL
|
||||
assert(d3 > d1);
|
||||
std::cout << "[PASS] distance_estimator_multi_class" << std::endl;
|
||||
}
|
||||
|
||||
void test_threat_tracker_filtering() {
|
||||
ThreatTracker tracker(0.3f, 15.0f, 5);
|
||||
AcousticThreat t1;
|
||||
t1.sound_type = "gunshot";
|
||||
t1.confidence = 0.9f;
|
||||
t1.azimuth = 45.0f;
|
||||
t1.distance = 100.0f;
|
||||
|
||||
// First update should publish
|
||||
auto out1 = tracker.Update({t1});
|
||||
assert(out1.size() == 1);
|
||||
|
||||
// Immediate second update with same azimuth should be filtered (interval < 0.3s)
|
||||
auto out2 = tracker.Update({t1});
|
||||
assert(out2.empty());
|
||||
|
||||
std::cout << "[PASS] threat_tracker_filtering" << std::endl;
|
||||
}
|
||||
|
||||
int main() {
|
||||
std::cout << "Running core library tests..." << std::endl;
|
||||
test_audio_buffer();
|
||||
test_audio_buffer_overflow();
|
||||
test_preemphasis();
|
||||
test_gcc_phat_cross_array();
|
||||
test_distance_estimator();
|
||||
test_distance_estimator_multi_class();
|
||||
test_threat_tracker();
|
||||
test_threat_tracker_filtering();
|
||||
test_feature_extractor_shape();
|
||||
std::cout << "All tests passed!" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,43 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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 modules__airship_att_control
|
||||
MAIN airship_att_control
|
||||
STACK_MAX 3500
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
airship_att_control_main.cpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
)
|
||||
@ -0,0 +1,99 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
/**
|
||||
* Airship attitude control app start / stop handling function
|
||||
*/
|
||||
extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]);
|
||||
|
||||
class AirshipAttitudeControl : public ModuleBase<AirshipAttitudeControl>, public ModuleParams,
|
||||
public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
AirshipAttitudeControl();
|
||||
|
||||
virtual ~AirshipAttitudeControl();
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
|
||||
void Run() override;
|
||||
|
||||
bool init();
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Check for parameter update and handle it.
|
||||
*/
|
||||
void parameter_update_poll();
|
||||
|
||||
void publish_actuator_controls();
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< parameter updates subscription */
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
|
||||
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
|
||||
|
||||
uORB::Publication<actuator_controls_s> _actuators_0_pub;
|
||||
|
||||
struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */
|
||||
struct vehicle_status_s _vehicle_status {}; /**< vehicle status */
|
||||
struct actuator_controls_s _actuators {}; /**< actuator controls */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
};
|
||||
@ -0,0 +1,213 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 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 airship_att_control_main.cpp
|
||||
* Airship attitude controller.
|
||||
*
|
||||
* @author Anton Erasmus <anton@flycloudline.com>
|
||||
*/
|
||||
|
||||
#include "airship_att_control.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
AirshipAttitudeControl::AirshipAttitudeControl() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
|
||||
_actuators_0_pub(ORB_ID(actuator_controls_0)),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control"))
|
||||
{
|
||||
}
|
||||
|
||||
AirshipAttitudeControl::~AirshipAttitudeControl()
|
||||
{
|
||||
perf_free(_loop_perf);
|
||||
}
|
||||
|
||||
bool
|
||||
AirshipAttitudeControl::init()
|
||||
{
|
||||
if (!_vehicle_angular_velocity_sub.registerCallback()) {
|
||||
PX4_ERR("vehicle_angular_velocity callback registration failed!");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
AirshipAttitudeControl::parameter_update_poll()
|
||||
{
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
AirshipAttitudeControl::publish_actuator_controls()
|
||||
{
|
||||
// zero actuators if not armed
|
||||
if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
for (uint8_t i = 0 ; i < 4 ; i++) {
|
||||
_actuators.control[i] = 0.0f;
|
||||
}
|
||||
|
||||
} else {
|
||||
_actuators.control[0] = 0.0f;
|
||||
_actuators.control[1] = _manual_control_sp.x;
|
||||
_actuators.control[2] = _manual_control_sp.r;
|
||||
_actuators.control[3] = _manual_control_sp.z;
|
||||
}
|
||||
|
||||
// note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run()
|
||||
_actuators.timestamp = hrt_absolute_time();
|
||||
|
||||
_actuators_0_pub.publish(_actuators);
|
||||
}
|
||||
|
||||
void
|
||||
AirshipAttitudeControl::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_vehicle_angular_velocity_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
/* run controller on gyro changes */
|
||||
vehicle_angular_velocity_s angular_velocity;
|
||||
|
||||
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
|
||||
|
||||
const Vector3f rates{angular_velocity.xyz};
|
||||
|
||||
_actuators.timestamp_sample = angular_velocity.timestamp_sample;
|
||||
|
||||
/* run the rate controller immediately after a gyro update */
|
||||
publish_actuator_controls();
|
||||
|
||||
/* check for updates in manual control topic */
|
||||
if (_manual_control_sp_sub.updated()) {
|
||||
_manual_control_sp_sub.update(&_manual_control_sp);
|
||||
}
|
||||
|
||||
/* check for updates in vehicle status topic */
|
||||
if (_vehicle_status_sub.updated()) {
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
}
|
||||
|
||||
parameter_update_poll();
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
int AirshipAttitudeControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
AirshipAttitudeControl *instance = new AirshipAttitudeControl();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int AirshipAttitudeControl::print_status()
|
||||
{
|
||||
PX4_INFO("Running");
|
||||
|
||||
perf_print_counter(_loop_perf);
|
||||
|
||||
print_message(_actuators);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int AirshipAttitudeControl::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int AirshipAttitudeControl::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
This implements the airship attitude and rate controller. Ideally it would
|
||||
take attitude setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode
|
||||
via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
|
||||
|
||||
Currently it is feeding the `manual_control_setpoint` topic directly to the actuators.
|
||||
|
||||
### Implementation
|
||||
To reduce control latency, the module directly polls on the gyro topic published by the IMU driver.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("airship_att_control", "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int airship_att_control_main(int argc, char *argv[])
|
||||
{
|
||||
return AirshipAttitudeControl::main(argc, argv);
|
||||
}
|
||||
@ -0,0 +1,42 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE modules__airspeed_selector
|
||||
MAIN airspeed_selector
|
||||
SRCS
|
||||
airspeed_selector_main.cpp
|
||||
DEPENDS
|
||||
git_ecl
|
||||
ecl_airdata
|
||||
AirspeedValidator
|
||||
)
|
||||
@ -0,0 +1,675 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <ecl/airdata/WindEstimator.hpp>
|
||||
#include <matrix/math.hpp>
|
||||
#include <parameters/param.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/airspeed/airspeed.h>
|
||||
#include <AirspeedValidator.hpp>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/estimator_selector_status.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/mavlink_log.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/position_setpoint.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/vehicle_acceleration.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
#include <uORB/topics/airspeed_wind.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */
|
||||
|
||||
using matrix::Dcmf;
|
||||
using matrix::Quatf;
|
||||
using matrix::Vector2f;
|
||||
using matrix::Vector3f;
|
||||
|
||||
class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
|
||||
public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
AirspeedModule();
|
||||
|
||||
~AirspeedModule() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */
|
||||
enum airspeed_index {
|
||||
DISABLED_INDEX = -1,
|
||||
GROUND_MINUS_WIND_INDEX,
|
||||
FIRST_SENSOR_INDEX,
|
||||
SECOND_SENSOR_INDEX,
|
||||
THIRD_SENSOR_INDEX
|
||||
};
|
||||
|
||||
uORB::Publication<airspeed_validated_s> _airspeed_validated_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/
|
||||
uORB::PublicationMulti<airspeed_wind_s> _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
|
||||
orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
|
||||
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
|
||||
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
|
||||
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
|
||||
uORB::Subscription _position_setpoint_sub{ORB_ID(position_setpoint)};
|
||||
uORB::SubscriptionMultiArray<airspeed_s, MAX_NUM_AIRSPEED_SENSORS> _airspeed_subs{ORB_ID::airspeed};
|
||||
|
||||
|
||||
estimator_status_s _estimator_status {};
|
||||
vehicle_acceleration_s _accel {};
|
||||
vehicle_air_data_s _vehicle_air_data {};
|
||||
vehicle_attitude_s _vehicle_attitude {};
|
||||
vehicle_land_detected_s _vehicle_land_detected {};
|
||||
vehicle_local_position_s _vehicle_local_position {};
|
||||
vehicle_status_s _vehicle_status {};
|
||||
vtol_vehicle_status_s _vtol_vehicle_status {};
|
||||
position_setpoint_s _position_setpoint {};
|
||||
|
||||
WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */
|
||||
airspeed_wind_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */
|
||||
|
||||
int32_t _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/
|
||||
int32_t _prev_number_of_airspeed_sensors{0}; /**< number of airspeed sensors in previous loop (to detect a new added sensor)*/
|
||||
AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS] {}; /**< airspeedValidator instances (one for each sensor) */
|
||||
|
||||
hrt_abstime _time_now_usec{0};
|
||||
int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */
|
||||
int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */
|
||||
bool _initialized{false}; /**< module initialized*/
|
||||
bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
|
||||
bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
|
||||
float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
|
||||
float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */
|
||||
|
||||
bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */
|
||||
|
||||
hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
|
||||
|
||||
perf_counter_t _perf_elapsed{};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
|
||||
(ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
|
||||
(ParamFloat<px4::params::ASPD_TAS_NOISE>) _param_west_tas_noise,
|
||||
(ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
|
||||
(ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
|
||||
(ParamInt<px4::params::ASPD_BETA_GATE>) _param_west_beta_gate,
|
||||
(ParamInt<px4::params::ASPD_SCALE_EST>) _param_west_scale_estimation_on,
|
||||
(ParamFloat<px4::params::ASPD_SCALE>) _param_west_airspeed_scale,
|
||||
(ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
|
||||
(ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
|
||||
(ParamInt<px4::params::ASPD_FALLBACK_GW>) _param_airspeed_fallback_gw,
|
||||
|
||||
(ParamFloat<px4::params::ASPD_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
|
||||
(ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
|
||||
(ParamInt<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
|
||||
(ParamInt<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**< delay to declare airspeed valid again */
|
||||
|
||||
(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall
|
||||
)
|
||||
|
||||
void init(); /**< initialization of the airspeed validator instances */
|
||||
void check_for_connected_airspeed_sensors(); /**< check for airspeed sensors (airspeed topics) and get _number_of_airspeed_sensors */
|
||||
void update_params(); /**< update parameters */
|
||||
void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */
|
||||
void update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */
|
||||
void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */
|
||||
void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */
|
||||
|
||||
};
|
||||
|
||||
AirspeedModule::AirspeedModule():
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
|
||||
{
|
||||
// initialise parameters
|
||||
update_params();
|
||||
|
||||
_perf_elapsed = perf_alloc(PC_ELAPSED, MODULE_NAME": elapsed");
|
||||
}
|
||||
|
||||
AirspeedModule::~AirspeedModule()
|
||||
{
|
||||
ScheduleClear();
|
||||
|
||||
perf_free(_perf_elapsed);
|
||||
}
|
||||
|
||||
int
|
||||
AirspeedModule::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
AirspeedModule *dev = new AirspeedModule();
|
||||
|
||||
// check if the trampoline is called for the first time
|
||||
if (!dev) {
|
||||
PX4_ERR("alloc failed");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_object.store(dev);
|
||||
|
||||
dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000);
|
||||
_task_id = task_id_is_work_queue;
|
||||
return PX4_OK;
|
||||
|
||||
}
|
||||
void
|
||||
AirspeedModule::init()
|
||||
{
|
||||
check_for_connected_airspeed_sensors();
|
||||
|
||||
// Set the default sensor
|
||||
if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors) {
|
||||
// constrain the index to the number of sensors connected
|
||||
_valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors);
|
||||
|
||||
if (_number_of_airspeed_sensors == 0) {
|
||||
mavlink_log_info(&_mavlink_log_pub,
|
||||
"No airspeed sensor detected. Switch to non-airspeed mode.");
|
||||
|
||||
} else {
|
||||
mavlink_log_info(&_mavlink_log_pub,
|
||||
"Primary airspeed index bigger than number connected sensors. Take last sensor.");
|
||||
}
|
||||
|
||||
} else {
|
||||
// set index to the one provided in the parameter ASPD_PRIMARY
|
||||
_valid_airspeed_index = _param_airspeed_primary_index.get();
|
||||
}
|
||||
|
||||
_prev_airspeed_index = _valid_airspeed_index; // used to detect a sensor switching
|
||||
}
|
||||
|
||||
void
|
||||
AirspeedModule::check_for_connected_airspeed_sensors()
|
||||
{
|
||||
// check for new connected airspeed sensor
|
||||
int detected_airspeed_sensors = 0;
|
||||
|
||||
if (_param_airspeed_primary_index.get() > 0) {
|
||||
|
||||
for (int i = 0; i < _airspeed_subs.size(); i++) {
|
||||
if (!_airspeed_subs[i].advertised()) {
|
||||
break;
|
||||
}
|
||||
|
||||
detected_airspeed_sensors = i + 1;
|
||||
}
|
||||
|
||||
} else {
|
||||
// user has selected groundspeed-windspeed as primary source, or disabled airspeed
|
||||
detected_airspeed_sensors = 0;
|
||||
}
|
||||
|
||||
_number_of_airspeed_sensors = detected_airspeed_sensors;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
AirspeedModule::Run()
|
||||
{
|
||||
_time_now_usec = hrt_absolute_time(); // hrt time of the current cycle
|
||||
|
||||
// do not run the airspeed selector until 2s after system boot,
|
||||
// as data from airspeed sensor and estimator may not be valid yet
|
||||
if (_time_now_usec < 2_s) {
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_perf_elapsed);
|
||||
|
||||
if (!_initialized) {
|
||||
init(); // initialize airspeed validator instances
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
parameter_update_s update;
|
||||
|
||||
if (_parameter_update_sub.update(&update)) {
|
||||
update_params();
|
||||
}
|
||||
|
||||
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||
|
||||
// check for new connected airspeed sensors as long as we're disarmed
|
||||
if (!armed) {
|
||||
check_for_connected_airspeed_sensors();
|
||||
}
|
||||
|
||||
poll_topics();
|
||||
update_wind_estimator_sideslip();
|
||||
update_ground_minus_wind_airspeed();
|
||||
|
||||
if (_number_of_airspeed_sensors > 0) {
|
||||
|
||||
// disable checks if not a fixed-wing or the vehicle is landing/landed, as then airspeed can fall below stall speed
|
||||
// and wind estimate isn't accurate anymore. Even better would be to have a reliable "ground_contact" detection
|
||||
// for fixed-wing landings.
|
||||
const bool in_air_fixed_wing = !_vehicle_land_detected.landed &&
|
||||
_position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND &&
|
||||
_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
|
||||
|
||||
// Prepare data for airspeed_validator
|
||||
struct airspeed_validator_update_data input_data = {};
|
||||
input_data.timestamp = _time_now_usec;
|
||||
input_data.lpos_vx = _vehicle_local_position.vx;
|
||||
input_data.lpos_vy = _vehicle_local_position.vy;
|
||||
input_data.lpos_vz = _vehicle_local_position.vz;
|
||||
input_data.lpos_valid = _vehicle_local_position_valid;
|
||||
input_data.lpos_evh = _vehicle_local_position.evh;
|
||||
input_data.lpos_evv = _vehicle_local_position.evv;
|
||||
input_data.att_q[0] = _vehicle_attitude.q[0];
|
||||
input_data.att_q[1] = _vehicle_attitude.q[1];
|
||||
input_data.att_q[2] = _vehicle_attitude.q[2];
|
||||
input_data.att_q[3] = _vehicle_attitude.q[3];
|
||||
input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa;
|
||||
input_data.accel_z = _accel.xyz[2];
|
||||
input_data.vel_test_ratio = _estimator_status.vel_test_ratio;
|
||||
input_data.mag_test_ratio = _estimator_status.mag_test_ratio;
|
||||
|
||||
// iterate through all airspeed sensors, poll new data from them and update their validators
|
||||
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
|
||||
|
||||
// poll raw airspeed topic of the i-th sensor
|
||||
airspeed_s airspeed_raw;
|
||||
|
||||
if (_airspeed_subs[i].update(&airspeed_raw)) {
|
||||
|
||||
input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s;
|
||||
input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s;
|
||||
input_data.airspeed_timestamp = airspeed_raw.timestamp;
|
||||
input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;
|
||||
|
||||
// takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed
|
||||
if (_in_takeoff_situation &&
|
||||
(airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get() ||
|
||||
_ground_minus_wind_CAS > _param_fw_airspd_stall.get())) {
|
||||
_in_takeoff_situation = false;
|
||||
}
|
||||
|
||||
// reset takeoff_situation to true when not in air and not in fixed-wing mode
|
||||
if (!in_air_fixed_wing) {
|
||||
_in_takeoff_situation = true;
|
||||
}
|
||||
|
||||
input_data.in_fixed_wing_flight = (armed && in_air_fixed_wing && !_in_takeoff_situation);
|
||||
|
||||
// push input data into airspeed validator
|
||||
_airspeed_validator[i].update_airspeed_validator(input_data);
|
||||
|
||||
_time_last_airspeed_update[i] = _time_now_usec;
|
||||
|
||||
} else if (_time_now_usec - _time_last_airspeed_update[i] > 1_s) {
|
||||
// declare airspeed invalid if more then 1s since last raw airspeed update
|
||||
_airspeed_validator[i].reset_airspeed_to_invalid(_time_now_usec);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
select_airspeed_and_publish();
|
||||
|
||||
perf_end(_perf_elapsed);
|
||||
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
}
|
||||
}
|
||||
|
||||
void AirspeedModule::update_params()
|
||||
{
|
||||
updateParams();
|
||||
|
||||
_wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
|
||||
_wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
|
||||
_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
|
||||
_wind_estimator_sideslip.set_beta_noise(_param_west_beta_noise.get());
|
||||
_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
|
||||
_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
|
||||
|
||||
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
|
||||
_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
|
||||
_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
|
||||
_airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
|
||||
_airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
|
||||
_airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());
|
||||
_airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get());
|
||||
_airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());
|
||||
|
||||
// only apply manual entered airspeed scale to first airspeed measurement
|
||||
// TODO: enable multiple airspeed sensors
|
||||
_airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
|
||||
|
||||
_airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get());
|
||||
_airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
|
||||
_airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get());
|
||||
_airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get());
|
||||
_airspeed_validator[i].set_airspeed_stall(_param_fw_airspd_stall.get());
|
||||
}
|
||||
|
||||
// if the airspeed scale estimation is enabled and the airspeed is valid,
|
||||
// then set the scale inside the wind estimator to -1 such that it starts to estimate it
|
||||
if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
|
||||
if (_valid_airspeed_index > 0) {
|
||||
// set it to a negative value to start estimation inside wind estimator
|
||||
_airspeed_validator[0].set_airspeed_scale_manual(-1.0f);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
|
||||
_param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
|
||||
_param_west_scale_estimation_on.commit_no_notification();
|
||||
}
|
||||
|
||||
} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
|
||||
if (_valid_airspeed_index > 0) {
|
||||
|
||||
_param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
|
||||
_param_west_airspeed_scale.commit_no_notification();
|
||||
_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
|
||||
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f",
|
||||
(double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
|
||||
|
||||
} else {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
|
||||
}
|
||||
}
|
||||
|
||||
_scale_estimation_previously_on = _param_west_scale_estimation_on.get();
|
||||
|
||||
}
|
||||
|
||||
void AirspeedModule::poll_topics()
|
||||
{
|
||||
// use primary estimator_status
|
||||
if (_estimator_selector_status_sub.updated()) {
|
||||
estimator_selector_status_s estimator_selector_status;
|
||||
|
||||
if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
|
||||
if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
|
||||
_estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_estimator_status_sub.update(&_estimator_status);
|
||||
_vehicle_acceleration_sub.update(&_accel);
|
||||
_vehicle_air_data_sub.update(&_vehicle_air_data);
|
||||
_vehicle_attitude_sub.update(&_vehicle_attitude);
|
||||
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
_vtol_vehicle_status_sub.update(&_vtol_vehicle_status);
|
||||
_vehicle_local_position_sub.update(&_vehicle_local_position);
|
||||
_position_setpoint_sub.update(&_position_setpoint);
|
||||
|
||||
|
||||
_vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
|
||||
&& (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid;
|
||||
}
|
||||
|
||||
void AirspeedModule::update_wind_estimator_sideslip()
|
||||
{
|
||||
// update wind and airspeed estimator
|
||||
_wind_estimator_sideslip.update(_time_now_usec);
|
||||
|
||||
if (_vehicle_local_position_valid && !_vtol_vehicle_status.vtol_in_rw_mode) {
|
||||
Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
|
||||
Quatf q(_vehicle_attitude.q);
|
||||
|
||||
_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, q);
|
||||
}
|
||||
|
||||
_wind_estimate_sideslip.timestamp = _time_now_usec;
|
||||
float wind[2];
|
||||
_wind_estimator_sideslip.get_wind(wind);
|
||||
_wind_estimate_sideslip.windspeed_north = wind[0];
|
||||
_wind_estimate_sideslip.windspeed_east = wind[1];
|
||||
float wind_cov[2];
|
||||
_wind_estimator_sideslip.get_wind_var(wind_cov);
|
||||
_wind_estimate_sideslip.variance_north = wind_cov[0];
|
||||
_wind_estimate_sideslip.variance_east = wind_cov[1];
|
||||
_wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov();
|
||||
_wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var();
|
||||
_wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov();
|
||||
_wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var();
|
||||
_wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale();
|
||||
_wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY;
|
||||
}
|
||||
|
||||
void AirspeedModule::update_ground_minus_wind_airspeed()
|
||||
{
|
||||
// calculate airspeed estimate based on groundspeed-windspeed to use as fallback
|
||||
const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
|
||||
const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
|
||||
const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
|
||||
_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
|
||||
_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
|
||||
_vehicle_air_data.baro_temp_celcius);
|
||||
}
|
||||
|
||||
|
||||
void AirspeedModule::select_airspeed_and_publish()
|
||||
{
|
||||
const bool airspeed_sensor_switching_necessary = _prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX ||
|
||||
!_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid();
|
||||
const bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 &&
|
||||
_param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get();
|
||||
const bool airspeed_sensor_added = _prev_number_of_airspeed_sensors < _number_of_airspeed_sensors;
|
||||
|
||||
if (airspeed_sensor_switching_necessary && (airspeed_sensor_switching_allowed || airspeed_sensor_added)) {
|
||||
|
||||
_valid_airspeed_index = airspeed_index::DISABLED_INDEX;
|
||||
|
||||
// loop through all sensors and take the first valid one
|
||||
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
|
||||
if (_airspeed_validator[i].get_airspeed_valid()) {
|
||||
_valid_airspeed_index = i + 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// check if airspeed based on ground-wind speed is valid and can be published
|
||||
if (_param_airspeed_primary_index.get() > airspeed_index::DISABLED_INDEX &&
|
||||
(_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
|
||||
|| _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
|
||||
|
||||
// _vehicle_local_position_valid determines if ground-wind estimate is valid
|
||||
if (_vehicle_local_position_valid &&
|
||||
(_param_airspeed_fallback_gw.get() || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
|
||||
_valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX;
|
||||
|
||||
} else {
|
||||
_valid_airspeed_index = airspeed_index::DISABLED_INDEX;
|
||||
}
|
||||
}
|
||||
|
||||
// print warning or info, depending of whether airspeed got declared invalid or healthy
|
||||
if (_valid_airspeed_index != _prev_airspeed_index &&
|
||||
(_number_of_airspeed_sensors > 0 || !_vehicle_land_detected.landed) &&
|
||||
_valid_airspeed_index != _prev_airspeed_index) {
|
||||
if (_prev_airspeed_index > 0) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Airspeed sensor failure detected (%i, %i)", _prev_airspeed_index,
|
||||
_valid_airspeed_index);
|
||||
|
||||
} else if (_prev_airspeed_index == 0 && _valid_airspeed_index == -1) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation invalid");
|
||||
|
||||
} else if (_prev_airspeed_index == -1 && _valid_airspeed_index == 0) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed estimation valid");
|
||||
|
||||
} else {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed sensor healthy, start using again (%i, %i)", _prev_airspeed_index,
|
||||
_valid_airspeed_index);
|
||||
}
|
||||
}
|
||||
|
||||
_prev_airspeed_index = _valid_airspeed_index;
|
||||
_prev_number_of_airspeed_sensors = _number_of_airspeed_sensors;
|
||||
|
||||
airspeed_validated_s airspeed_validated = {};
|
||||
airspeed_validated.timestamp = _time_now_usec;
|
||||
airspeed_validated.true_ground_minus_wind_m_s = NAN;
|
||||
airspeed_validated.calibrated_ground_minus_wind_m_s = NAN;
|
||||
airspeed_validated.indicated_airspeed_m_s = NAN;
|
||||
airspeed_validated.calibrated_airspeed_m_s = NAN;
|
||||
airspeed_validated.true_airspeed_m_s = NAN;
|
||||
airspeed_validated.airspeed_sensor_measurement_valid = false;
|
||||
airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
|
||||
|
||||
switch (_valid_airspeed_index) {
|
||||
case airspeed_index::DISABLED_INDEX:
|
||||
break;
|
||||
|
||||
case airspeed_index::GROUND_MINUS_WIND_INDEX:
|
||||
airspeed_validated.indicated_airspeed_m_s = _ground_minus_wind_CAS;
|
||||
airspeed_validated.calibrated_airspeed_m_s = _ground_minus_wind_CAS;
|
||||
airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS;
|
||||
airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
|
||||
airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_IAS();
|
||||
airspeed_validated.calibrated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_CAS();
|
||||
airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index - 1].get_TAS();
|
||||
airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
|
||||
airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS;
|
||||
airspeed_validated.airspeed_sensor_measurement_valid = true;
|
||||
break;
|
||||
}
|
||||
|
||||
_airspeed_validated_pub.publish(airspeed_validated);
|
||||
|
||||
_wind_est_pub[0].publish(_wind_estimate_sideslip);
|
||||
|
||||
// publish the wind estimator states from all airspeed validators
|
||||
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
|
||||
airspeed_wind_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec);
|
||||
|
||||
if (i == 0) {
|
||||
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_1;
|
||||
|
||||
} else if (i == 1) {
|
||||
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_2;
|
||||
|
||||
} else {
|
||||
wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_3;
|
||||
}
|
||||
|
||||
_wind_est_pub[i + 1].publish(wind_est);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int AirspeedModule::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (!is_running()) {
|
||||
int ret = AirspeedModule::task_spawn(argc, argv);
|
||||
|
||||
if (ret) {
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int AirspeedModule::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
This module provides a single airspeed_validated topic, containing indicated (IAS),
|
||||
calibrated (CAS), true airspeed (TAS) and the information if the estimation currently
|
||||
is invalid and if based sensor readings or on groundspeed minus windspeed.
|
||||
Supporting the input of multiple "raw" airspeed inputs, this module automatically switches
|
||||
to a valid sensor in case of failure detection. For failure detection as well as for
|
||||
the estimation of a scale factor from IAS to CAS, it runs several wind estimators
|
||||
and also publishes those.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("airspeed_estimator", "estimator");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int airspeed_selector_main(int argc, char *argv[])
|
||||
{
|
||||
return AirspeedModule::main(argc, argv);
|
||||
}
|
||||
@ -0,0 +1,186 @@
|
||||
|
||||
/**
|
||||
* Airspeed Selector: Wind estimator wind process noise
|
||||
*
|
||||
* Wind process noise of the internal wind estimator(s) of the airspeed selector.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @unit m/s^2
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
|
||||
|
||||
/**
|
||||
* Airspeed Selector: Wind estimator true airspeed scale process noise
|
||||
*
|
||||
* Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector.
|
||||
*
|
||||
* @min 0
|
||||
* @max 0.1
|
||||
* @unit Hz
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001);
|
||||
|
||||
/**
|
||||
* Airspeed Selector: Wind estimator true airspeed measurement noise
|
||||
*
|
||||
* True airspeed measurement noise of the internal wind estimator(s) of the airspeed selector.
|
||||
*
|
||||
* @min 0
|
||||
* @max 4
|
||||
* @unit m/s
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4);
|
||||
|
||||
/**
|
||||
* Airspeed Selector: Wind estimator sideslip measurement noise
|
||||
*
|
||||
* Sideslip measurement noise of the internal wind estimator(s) of the airspeed selector.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @unit rad
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3);
|
||||
|
||||
/**
|
||||
* Airspeed Selector: Gate size for true airspeed fusion
|
||||
*
|
||||
* Sets the number of standard deviations used by the innovation consistency test.
|
||||
*
|
||||
* @min 1
|
||||
* @max 5
|
||||
* @unit SD
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3);
|
||||
|
||||
/**
|
||||
* Airspeed Selector: Gate size for sideslip angle fusion
|
||||
*
|
||||
* Sets the number of standard deviations used by the innovation consistency test.
|
||||
*
|
||||
* @min 1
|
||||
* @max 5
|
||||
* @unit SD
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ASPD_BETA_GATE, 1);
|
||||
|
||||
/**
|
||||
* Automatic airspeed scale estimation on
|
||||
*
|
||||
* Turns the automatic airspeed scale (scale from IAS to CAS) on or off. It is recommended to fly level
|
||||
* altitude while performing the estimation. Set to 1 to start estimation (best when already flying).
|
||||
* Set to 0 to end scale estimation. The estimated scale is then saved using the ASPD_SCALE parameter.
|
||||
*
|
||||
* @boolean
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ASPD_SCALE_EST, 0);
|
||||
|
||||
/**
|
||||
* Airspeed scale (scale from IAS to CAS)
|
||||
*
|
||||
* Scale can either be entered manually, or estimated in-flight by setting ASPD_SCALE_EST to 1.
|
||||
*
|
||||
* @min 0.5
|
||||
* @max 1.5
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_SCALE, 1.0f);
|
||||
|
||||
/**
|
||||
* Index or primary airspeed measurement source
|
||||
*
|
||||
* @value -1 Disabled
|
||||
* @value 0 Groundspeed minus windspeed
|
||||
* @value 1 First airspeed sensor
|
||||
* @value 2 Second airspeed sensor
|
||||
* @value 3 Third airspeed sensor
|
||||
*
|
||||
* @reboot_required true
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ASPD_PRIMARY, 1);
|
||||
|
||||
|
||||
/**
|
||||
* Enable checks on airspeed sensors
|
||||
*
|
||||
* If set to true then the data comming from the airspeed sensors is checked for validity. Only applied if ASPD_PRIMARY > 0.
|
||||
*
|
||||
* @boolean
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 1);
|
||||
|
||||
/**
|
||||
* Enable fallback to sensor-less airspeed estimation
|
||||
*
|
||||
* If set to true and airspeed checks are enabled, it will use a sensor-less airspeed estimation based on groundspeed
|
||||
* minus windspeed if no other airspeed sensor available to fall back to.
|
||||
*
|
||||
* @value 0 Disable fallback to sensor-less estimation
|
||||
* @value 1 Enable fallback to sensor-less estimation
|
||||
* @boolean
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0);
|
||||
|
||||
/**
|
||||
* Airspeed failsafe consistency threshold
|
||||
*
|
||||
* This specifies the minimum airspeed test ratio required to trigger a failsafe. Larger values make the check less sensitive,
|
||||
* smaller values make it more sensitive. Start with a value of 1.0 when tuning. When tas_test_ratio is > 1.0 it indicates the
|
||||
* inconsistency between predicted and measured airspeed is large enough to cause the wind EKF to reject airspeed measurements.
|
||||
* The time required to detect a fault when the threshold is exceeded depends on the size of the exceedance and is controlled by the ASPD_FS_INTEG parameter.
|
||||
*
|
||||
* @min 0.5
|
||||
* @max 3.0
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_FS_INNOV, 1.0f);
|
||||
|
||||
/**
|
||||
* Airspeed failsafe consistency delay
|
||||
*
|
||||
* This sets the time integral of airspeed test ratio exceedance above ASPD_FS_INNOV required to trigger a failsafe.
|
||||
* For example if ASPD_FS_INNOV is 1 and estimator_status.tas_test_ratio is 2.0, then the exceedance is 1.0 and the integral will
|
||||
* rise at a rate of 1.0/second. A negative value disables the check. Larger positive values make the check less sensitive, smaller positive values
|
||||
* make it more sensitive.
|
||||
*
|
||||
* @unit s
|
||||
* @max 30.0
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 5.0f);
|
||||
|
||||
/**
|
||||
* Airspeed failsafe stop delay
|
||||
*
|
||||
* Delay before stopping use of airspeed sensor if checks indicate sensor is bad.
|
||||
*
|
||||
* @unit s
|
||||
* @group Airspeed Validator
|
||||
* @min 1
|
||||
* @max 10
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2);
|
||||
|
||||
/**
|
||||
* Airspeed failsafe start delay
|
||||
*
|
||||
* Delay before switching back to using airspeed sensor if checks indicate sensor is good.
|
||||
* Set to a negative value to disable the re-enabling in flight.
|
||||
*
|
||||
* @unit s
|
||||
* @group Airspeed Validator
|
||||
* @min -1
|
||||
* @max 1000
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ASPD_FS_T_START, -1);
|
||||
@ -0,0 +1,115 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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 AngularVelocityControl.cpp
|
||||
*/
|
||||
|
||||
#include <AngularVelocityControl.hpp>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
void AngularVelocityControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
|
||||
{
|
||||
_gain_p = P;
|
||||
_gain_i = I;
|
||||
_gain_d = D;
|
||||
}
|
||||
|
||||
void AngularVelocityControl::setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
|
||||
const matrix::Vector<bool, 3> &saturation_negative)
|
||||
{
|
||||
_saturation_positive = saturation_positive;
|
||||
_saturation_negative = saturation_negative;
|
||||
}
|
||||
|
||||
void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_velocity_sp,
|
||||
const Vector3f &angular_acceleration, const float dt, const bool landed)
|
||||
{
|
||||
// angular rates error
|
||||
Vector3f angular_velocity_error = angular_velocity_sp - angular_velocity;
|
||||
|
||||
// P + D control
|
||||
_angular_accel_sp = _gain_p.emult(angular_velocity_error) - _gain_d.emult(angular_acceleration);
|
||||
|
||||
// I + FF control
|
||||
Vector3f torque_feedforward = _angular_velocity_int + _gain_ff.emult(angular_velocity_sp);
|
||||
|
||||
// compute torque setpoint
|
||||
_torque_sp = _inertia * _angular_accel_sp + torque_feedforward + angular_velocity.cross(_inertia * angular_velocity);
|
||||
|
||||
// update integral only if we are not landed
|
||||
if (!landed) {
|
||||
updateIntegral(angular_velocity_error, dt);
|
||||
}
|
||||
}
|
||||
|
||||
void AngularVelocityControl::updateIntegral(Vector3f &angular_velocity_error, const float dt)
|
||||
{
|
||||
for (int i = 0; i < 3; i++) {
|
||||
// prevent further positive control saturation
|
||||
if (_saturation_positive(i)) {
|
||||
angular_velocity_error(i) = math::min(angular_velocity_error(i), 0.f);
|
||||
}
|
||||
|
||||
// prevent further negative control saturation
|
||||
if (_saturation_negative(i)) {
|
||||
angular_velocity_error(i) = math::max(angular_velocity_error(i), 0.f);
|
||||
}
|
||||
|
||||
// I term factor: reduce the I gain with increasing rate error.
|
||||
// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
|
||||
// change (noticeable in a bounce-back effect after a flip).
|
||||
// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
|
||||
// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
|
||||
// and up to 200 deg error leads to <25% reduction of I.
|
||||
float i_factor = angular_velocity_error(i) / math::radians(400.f);
|
||||
i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
|
||||
|
||||
// Perform the integration using a first order method
|
||||
float angular_velocity_i = _angular_velocity_int(i) + i_factor * _gain_i(i) * angular_velocity_error(i) * dt;
|
||||
|
||||
// do not propagate the result if out of range or invalid
|
||||
if (PX4_ISFINITE(angular_velocity_i)) {
|
||||
_angular_velocity_int(i) = math::constrain(angular_velocity_i, -_lim_int(i), _lim_int(i));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AngularVelocityControl::reset()
|
||||
{
|
||||
_angular_velocity_int.zero();
|
||||
_torque_sp.zero();
|
||||
_angular_accel_sp.zero();
|
||||
}
|
||||
@ -0,0 +1,149 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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 AngularVelocityControl.hpp
|
||||
*
|
||||
* PID 3 axis angular velocity control.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
|
||||
|
||||
class AngularVelocityControl
|
||||
{
|
||||
public:
|
||||
AngularVelocityControl() = default;
|
||||
~AngularVelocityControl() = default;
|
||||
|
||||
/**
|
||||
* Set the control gains
|
||||
* @param P 3D vector of proportional gains for body x,y,z axis
|
||||
* @param I 3D vector of integral gains
|
||||
* @param D 3D vector of derivative gains
|
||||
*/
|
||||
void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
|
||||
|
||||
/**
|
||||
* Set the mximum absolute value of the integrator for all axes
|
||||
* @param integrator_limit limit value for all axes x, y, z
|
||||
*/
|
||||
void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
|
||||
|
||||
/**
|
||||
* Set direct angular velocity setpoint to torque feed forward gain
|
||||
* @see _gain_ff
|
||||
* @param FF 3D vector of feed forward gains for body x,y,z axis
|
||||
*/
|
||||
void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
|
||||
|
||||
/**
|
||||
* Set inertia matrix
|
||||
* @see _inertia
|
||||
* @param inertia inertia matrix
|
||||
*/
|
||||
void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; };
|
||||
|
||||
/**
|
||||
* Set saturation status
|
||||
* @see _saturation_positive
|
||||
* @see _saturation_negative
|
||||
* @param saturation_positive positive saturation
|
||||
* @param saturation_negative negative saturation
|
||||
*/
|
||||
void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
|
||||
const matrix::Vector<bool, 3> &saturation_negative);
|
||||
|
||||
/**
|
||||
* Run one control loop cycle calculation
|
||||
* @param angular_velocity estimation of the current vehicle angular velocity
|
||||
* @param angular_velocity_sp desired vehicle angular velocity setpoint
|
||||
* @param angular_acceleration estimation of the current vehicle angular acceleration
|
||||
* @param dt elapsed time since last update
|
||||
* @param landed whether the vehicle is on the ground
|
||||
*/
|
||||
void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_velocity_sp,
|
||||
const matrix::Vector3f &angular_acceleration, const float dt, const bool landed);
|
||||
|
||||
/**
|
||||
* Get the desired angular acceleration
|
||||
* @see _angular_accel_sp
|
||||
*/
|
||||
const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;};
|
||||
|
||||
/**
|
||||
* Get the torque vector to apply to the vehicle
|
||||
* @see _torque_sp
|
||||
*/
|
||||
const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;};
|
||||
|
||||
/**
|
||||
* Get the integral term
|
||||
* @see _torque_sp
|
||||
*/
|
||||
const matrix::Vector3f &getIntegral() {return _angular_velocity_int;};
|
||||
|
||||
/**
|
||||
* Set the integral term to 0 to prevent windup
|
||||
* @see _angular_velocity_int
|
||||
*/
|
||||
void resetIntegral() { _angular_velocity_int.zero(); }
|
||||
|
||||
/**
|
||||
* ReSet the controller state
|
||||
*/
|
||||
void reset();
|
||||
|
||||
private:
|
||||
void updateIntegral(matrix::Vector3f &angular_velocity_error, const float dt);
|
||||
|
||||
// Gains
|
||||
matrix::Vector3f _gain_p; ///< proportional gain for all axes x, y, z
|
||||
matrix::Vector3f _gain_i; ///< integral gain
|
||||
matrix::Vector3f _gain_d; ///< derivative gain
|
||||
matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
|
||||
matrix::Vector3f _gain_ff; ///< direct angular velocity to torque feed forward gain
|
||||
matrix::Matrix3f _inertia{matrix::eye<float, 3>()}; ///< inertia matrix
|
||||
|
||||
// States
|
||||
matrix::Vector3f _angular_velocity_int;
|
||||
matrix::Vector<bool, 3> _saturation_positive;
|
||||
matrix::Vector<bool, 3> _saturation_negative;
|
||||
|
||||
// Output
|
||||
matrix::Vector3f _angular_accel_sp; //< Angular acceleration setpoint computed using P and D gains
|
||||
matrix::Vector3f _torque_sp; //< Torque setpoint to apply to the vehicle
|
||||
};
|
||||
@ -0,0 +1,45 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 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 <gtest/gtest.h>
|
||||
#include <AngularVelocityControl.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
TEST(AngularVelocityControlTest, AllZeroCase)
|
||||
{
|
||||
AngularVelocityControl control;
|
||||
control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
|
||||
Vector3f torque = control.getTorqueSetpoint();
|
||||
EXPECT_EQ(torque, Vector3f());
|
||||
}
|
||||
@ -0,0 +1,41 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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_library(AngularVelocityControl
|
||||
AngularVelocityControl.cpp
|
||||
)
|
||||
target_compile_options(AngularVelocityControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(AngularVelocityControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(AngularVelocityControl PRIVATE mathlib)
|
||||
|
||||
px4_add_unit_gtest(SRC AngularVelocityControlTest.cpp LINKLIBS AngularVelocityControl)
|
||||
@ -0,0 +1,342 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 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 "AngularVelocityController.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <circuit_breaker/circuit_breaker.h>
|
||||
#include <mathlib/math/Limits.hpp>
|
||||
#include <mathlib/math/Functions.hpp>
|
||||
#include <ecl/geo/geo.h>
|
||||
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
AngularVelocityController::AngularVelocityController() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
||||
{
|
||||
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
||||
|
||||
parameters_updated();
|
||||
}
|
||||
|
||||
AngularVelocityController::~AngularVelocityController()
|
||||
{
|
||||
perf_free(_loop_perf);
|
||||
}
|
||||
|
||||
bool
|
||||
AngularVelocityController::init()
|
||||
{
|
||||
if (!_vehicle_angular_velocity_sub.registerCallback()) {
|
||||
PX4_ERR("vehicle_angular_velocity callback registration failed!");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
AngularVelocityController::parameters_updated()
|
||||
{
|
||||
// Control parameters
|
||||
// The controller gain K is used to convert the parallel (P + I/s + sD) form
|
||||
// to the ideal (K * [1 + 1/sTi + sTd]) form
|
||||
const Vector3f k_gains = Vector3f(_param_avc_x_k.get(), _param_avc_y_k.get(), _param_avc_z_k.get());
|
||||
|
||||
_control.setGains(
|
||||
k_gains.emult(Vector3f(_param_avc_x_p.get(), _param_avc_y_p.get(), _param_avc_z_p.get())),
|
||||
k_gains.emult(Vector3f(_param_avc_x_i.get(), _param_avc_y_i.get(), _param_avc_z_i.get())),
|
||||
k_gains.emult(Vector3f(_param_avc_x_d.get(), _param_avc_y_d.get(), _param_avc_z_d.get())));
|
||||
|
||||
_control.setIntegratorLimit(
|
||||
Vector3f(_param_avc_x_i_lim.get(), _param_avc_y_i_lim.get(), _param_avc_z_i_lim.get()));
|
||||
|
||||
_control.setFeedForwardGain(
|
||||
Vector3f(_param_avc_x_ff.get(), _param_avc_y_ff.get(), _param_avc_z_ff.get()));
|
||||
|
||||
// inertia matrix
|
||||
const float inertia[3][3] = {
|
||||
{_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()},
|
||||
{_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()},
|
||||
{_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()}
|
||||
};
|
||||
_control.setInertiaMatrix(matrix::Matrix3f(inertia));
|
||||
|
||||
// Hover thrust
|
||||
if (!_param_mpc_use_hte.get()) {
|
||||
_hover_thrust = _param_mpc_thr_hover.get();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
AngularVelocityController::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_vehicle_angular_velocity_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
// Check if parameters have changed
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
updateParams();
|
||||
parameters_updated();
|
||||
}
|
||||
|
||||
/* run controller on gyro changes */
|
||||
vehicle_angular_velocity_s vehicle_angular_velocity;
|
||||
|
||||
if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) {
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
_timestamp_sample = vehicle_angular_velocity.timestamp_sample;
|
||||
|
||||
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
|
||||
const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
|
||||
_last_run = now;
|
||||
|
||||
const Vector3f angular_velocity{vehicle_angular_velocity.xyz};
|
||||
|
||||
/* check for updates in other topics */
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
|
||||
|
||||
if (_vehicle_land_detected_sub.updated()) {
|
||||
vehicle_land_detected_s vehicle_land_detected;
|
||||
|
||||
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
|
||||
_landed = vehicle_land_detected.landed;
|
||||
_maybe_landed = vehicle_land_detected.maybe_landed;
|
||||
}
|
||||
}
|
||||
|
||||
// Check for updates of hover thrust
|
||||
if (_param_mpc_use_hte.get()) {
|
||||
hover_thrust_estimate_s hte;
|
||||
|
||||
if (_hover_thrust_estimate_sub.update(&hte)) {
|
||||
_hover_thrust = hte.hover_thrust;
|
||||
}
|
||||
}
|
||||
|
||||
// check angular acceleration topic
|
||||
vehicle_angular_acceleration_s vehicle_angular_acceleration;
|
||||
|
||||
if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) {
|
||||
_angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz);
|
||||
}
|
||||
|
||||
// check rates setpoint topic
|
||||
vehicle_rates_setpoint_s vehicle_rates_setpoint;
|
||||
|
||||
if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
|
||||
_angular_velocity_sp(0) = vehicle_rates_setpoint.roll;
|
||||
_angular_velocity_sp(1) = vehicle_rates_setpoint.pitch;
|
||||
_angular_velocity_sp(2) = vehicle_rates_setpoint.yaw;
|
||||
_thrust_sp = Vector3f(vehicle_rates_setpoint.thrust_body);
|
||||
|
||||
// Scale _thrust_sp in Newton, assuming _hover_thrust is equivalent to 1G
|
||||
_thrust_sp *= (_param_vm_mass.get() * CONSTANTS_ONE_G / _hover_thrust);
|
||||
}
|
||||
|
||||
// run the controller
|
||||
if (_vehicle_control_mode.flag_control_rates_enabled) {
|
||||
// reset integral if disarmed
|
||||
if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
_control.resetIntegral();
|
||||
}
|
||||
|
||||
// update saturation status from mixer feedback
|
||||
control_allocator_status_s control_allocator_status;
|
||||
|
||||
if (_control_allocator_status_sub.update(&control_allocator_status)) {
|
||||
Vector<bool, 3> saturation_positive;
|
||||
Vector<bool, 3> saturation_negative;
|
||||
|
||||
if (!control_allocator_status.torque_setpoint_achieved) {
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
|
||||
saturation_positive(i) = true;
|
||||
|
||||
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
|
||||
saturation_negative(i) = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_control.setSaturationStatus(saturation_positive, saturation_negative);
|
||||
}
|
||||
|
||||
// run rate controller
|
||||
_control.update(angular_velocity, _angular_velocity_sp, _angular_acceleration, dt, _maybe_landed || _landed);
|
||||
|
||||
// publish rate controller status
|
||||
rate_ctrl_status_s rate_ctrl_status{};
|
||||
Vector3f integral = _control.getIntegral();
|
||||
rate_ctrl_status.timestamp = hrt_absolute_time();
|
||||
rate_ctrl_status.rollspeed_integ = integral(0);
|
||||
rate_ctrl_status.pitchspeed_integ = integral(1);
|
||||
rate_ctrl_status.yawspeed_integ = integral(2);
|
||||
_rate_ctrl_status_pub.publish(rate_ctrl_status);
|
||||
|
||||
// publish controller output
|
||||
publish_angular_acceleration_setpoint();
|
||||
publish_torque_setpoint();
|
||||
publish_thrust_setpoint();
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
void
|
||||
AngularVelocityController::publish_angular_acceleration_setpoint()
|
||||
{
|
||||
Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint();
|
||||
|
||||
vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
|
||||
v_angular_accel_sp.timestamp = hrt_absolute_time();
|
||||
v_angular_accel_sp.timestamp_sample = _timestamp_sample;
|
||||
v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
|
||||
v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
|
||||
v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
|
||||
|
||||
_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
|
||||
}
|
||||
|
||||
void
|
||||
AngularVelocityController::publish_torque_setpoint()
|
||||
{
|
||||
Vector3f torque_sp = _control.getTorqueSetpoint();
|
||||
|
||||
vehicle_torque_setpoint_s v_torque_sp = {};
|
||||
v_torque_sp.timestamp = hrt_absolute_time();
|
||||
v_torque_sp.timestamp_sample = _timestamp_sample;
|
||||
v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
|
||||
v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
|
||||
v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
|
||||
|
||||
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
|
||||
}
|
||||
|
||||
void
|
||||
AngularVelocityController::publish_thrust_setpoint()
|
||||
{
|
||||
vehicle_thrust_setpoint_s v_thrust_sp = {};
|
||||
v_thrust_sp.timestamp = hrt_absolute_time();
|
||||
v_thrust_sp.timestamp_sample = _timestamp_sample;
|
||||
v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f;
|
||||
v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f;
|
||||
v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f;
|
||||
|
||||
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
|
||||
}
|
||||
|
||||
int AngularVelocityController::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
AngularVelocityController *instance = new AngularVelocityController();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int AngularVelocityController::print_status()
|
||||
{
|
||||
PX4_INFO("Running");
|
||||
|
||||
perf_print_counter(_loop_perf);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int AngularVelocityController::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int AngularVelocityController::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
This implements the angular velocity controller.
|
||||
It takes angular velocity setpoints and measured angular
|
||||
velocity as inputs and outputs actuator setpoints.
|
||||
|
||||
The controller has a PID loop for angular rate error.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Angular velocity controller app start / stop handling function
|
||||
*/
|
||||
extern "C" __EXPORT int angular_velocity_controller_main(int argc, char *argv[]);
|
||||
|
||||
int angular_velocity_controller_main(int argc, char *argv[])
|
||||
{
|
||||
return AngularVelocityController::main(argc, argv);
|
||||
}
|
||||
@ -0,0 +1,178 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <AngularVelocityControl.hpp>
|
||||
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
|
||||
#include <uORB/topics/control_allocator_status.h>
|
||||
#include <uORB/topics/hover_thrust_estimate.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/rate_ctrl_status.h>
|
||||
#include <uORB/topics/vehicle_angular_acceleration.h>
|
||||
#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_thrust_setpoint.h>
|
||||
#include <uORB/topics/vehicle_torque_setpoint.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class AngularVelocityController : public ModuleBase<AngularVelocityController>, public ModuleParams,
|
||||
public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
AngularVelocityController();
|
||||
|
||||
virtual ~AngularVelocityController();
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
|
||||
void Run() override;
|
||||
|
||||
bool init();
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* initialize some vectors/matrices from parameters
|
||||
*/
|
||||
void parameters_updated();
|
||||
|
||||
void vehicle_status_poll();
|
||||
|
||||
void publish_angular_acceleration_setpoint();
|
||||
void publish_torque_setpoint();
|
||||
void publish_thrust_setpoint();
|
||||
void publish_actuator_controls();
|
||||
|
||||
AngularVelocityControl _control; ///< class for control calculations
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; /**< motor limits subscription */
|
||||
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; /**< vehicle angular acceleration subscription */
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
|
||||
uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
|
||||
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
|
||||
|
||||
uORB::Publication<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */
|
||||
uORB::Publication<vehicle_angular_acceleration_setpoint_s> _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; /**< angular acceleration setpoint publication */
|
||||
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< thrust setpoint publication */
|
||||
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; /**< torque setpoint publication */
|
||||
|
||||
vehicle_control_mode_s _vehicle_control_mode{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
||||
bool _landed{true};
|
||||
bool _maybe_landed{true};
|
||||
|
||||
float _battery_status_scale{0.0f};
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop duration performance counter */
|
||||
|
||||
matrix::Vector3f _angular_velocity_sp; /**< angular velocity setpoint */
|
||||
matrix::Vector3f _angular_acceleration; /**< angular acceleration (estimated) */
|
||||
matrix::Vector3f _thrust_sp; /**< thrust setpoint */
|
||||
|
||||
float _hover_thrust{0.5f}; /**< Normalized hover thrust **/
|
||||
|
||||
bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */
|
||||
|
||||
hrt_abstime _task_start{hrt_absolute_time()};
|
||||
hrt_abstime _last_run{0};
|
||||
hrt_abstime _timestamp_sample{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::AVC_X_P>) _param_avc_x_p,
|
||||
(ParamFloat<px4::params::AVC_X_I>) _param_avc_x_i,
|
||||
(ParamFloat<px4::params::AVC_X_I_LIM>) _param_avc_x_i_lim,
|
||||
(ParamFloat<px4::params::AVC_X_D>) _param_avc_x_d,
|
||||
(ParamFloat<px4::params::AVC_X_FF>) _param_avc_x_ff,
|
||||
(ParamFloat<px4::params::AVC_X_K>) _param_avc_x_k,
|
||||
|
||||
(ParamFloat<px4::params::AVC_Y_P>) _param_avc_y_p,
|
||||
(ParamFloat<px4::params::AVC_Y_I>) _param_avc_y_i,
|
||||
(ParamFloat<px4::params::AVC_Y_I_LIM>) _param_avc_y_i_lim,
|
||||
(ParamFloat<px4::params::AVC_Y_D>) _param_avc_y_d,
|
||||
(ParamFloat<px4::params::AVC_Y_FF>) _param_avc_y_ff,
|
||||
(ParamFloat<px4::params::AVC_Y_K>) _param_avc_y_k,
|
||||
|
||||
(ParamFloat<px4::params::AVC_Z_P>) _param_avc_z_p,
|
||||
(ParamFloat<px4::params::AVC_Z_I>) _param_avc_z_i,
|
||||
(ParamFloat<px4::params::AVC_Z_I_LIM>) _param_avc_z_i_lim,
|
||||
(ParamFloat<px4::params::AVC_Z_D>) _param_avc_z_d,
|
||||
(ParamFloat<px4::params::AVC_Z_FF>) _param_avc_z_ff,
|
||||
(ParamFloat<px4::params::AVC_Z_K>) _param_avc_z_k,
|
||||
|
||||
(ParamFloat<px4::params::VM_MASS>) _param_vm_mass,
|
||||
(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx,
|
||||
(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
|
||||
(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
|
||||
(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
|
||||
(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
|
||||
(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz,
|
||||
|
||||
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
|
||||
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte
|
||||
)
|
||||
|
||||
};
|
||||
@ -0,0 +1,49 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(AngularVelocityControl)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__angular_velocity_control
|
||||
MAIN angular_velocity_controller
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
SRCS
|
||||
AngularVelocityController.cpp
|
||||
AngularVelocityController.hpp
|
||||
DEPENDS
|
||||
circuit_breaker
|
||||
mathlib
|
||||
AngularVelocityControl
|
||||
px4_work_queue
|
||||
)
|
||||
@ -0,0 +1,297 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 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 angular_velocity_controller_params.c
|
||||
* Parameters for angular velocity controller.
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
* @author Anton Babushkin <anton@px4.io>
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Body X axis angular velocity P gain
|
||||
*
|
||||
* Body X axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @unit 1/s
|
||||
* @min 0.0
|
||||
* @max 20.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_X_P, 18.f);
|
||||
|
||||
/**
|
||||
* Body X axis angular velocity I gain
|
||||
*
|
||||
* Body X axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @unit Nm/rad
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_X_I, 0.2f);
|
||||
|
||||
/**
|
||||
* Body X axis angular velocity integrator limit
|
||||
*
|
||||
* Body X axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes.
|
||||
*
|
||||
* @unit Nm
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_X_I_LIM, 0.3f);
|
||||
|
||||
/**
|
||||
* Body X axis angular velocity D gain
|
||||
*
|
||||
* Body X axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 2.0
|
||||
* @decimal 4
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_X_D, 0.36f);
|
||||
|
||||
/**
|
||||
* Body X axis angular velocity feedforward gain
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @unit Nm/(rad/s)
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_X_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Body X axis angular velocity controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = AVC_X_K * (AVC_X_P * error
|
||||
* + AVC_X_I * error_integral
|
||||
* + AVC_X_D * error_derivative)
|
||||
* Set AVC_X_P=1 to implement a PID in the ideal form.
|
||||
* Set AVC_X_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_X_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Body Y axis angular velocity P gain
|
||||
*
|
||||
* Body Y axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @unit 1/s
|
||||
* @min 0.0
|
||||
* @max 20.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Y_P, 18.f);
|
||||
|
||||
/**
|
||||
* Body Y axis angular velocity I gain
|
||||
*
|
||||
* Body Y axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @unit Nm/rad
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Y_I, 0.2f);
|
||||
|
||||
/**
|
||||
* Body Y axis angular velocity integrator limit
|
||||
*
|
||||
* Body Y axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes.
|
||||
*
|
||||
* @unit Nm
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Y_I_LIM, 0.3f);
|
||||
|
||||
/**
|
||||
* Body Y axis angular velocity D gain
|
||||
*
|
||||
* Body Y axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 2.0
|
||||
* @decimal 4
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Y_D, 0.36f);
|
||||
|
||||
/**
|
||||
* Body Y axis angular velocity feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @unit Nm/(rad/s)
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Y_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Body Y axis angular velocity controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = AVC_Y_K * (AVC_Y_P * error
|
||||
* + AVC_Y_I * error_integral
|
||||
* + AVC_Y_D * error_derivative)
|
||||
* Set AVC_Y_P=1 to implement a PID in the ideal form.
|
||||
* Set AVC_Y_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 20.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Y_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Body Z axis angular velocity P gain
|
||||
*
|
||||
* Body Z axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @unit 1/s
|
||||
* @min 0.0
|
||||
* @max 20.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Z_P, 7.f);
|
||||
|
||||
/**
|
||||
* Body Z axis angular velocity I gain
|
||||
*
|
||||
* Body Z axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @unit Nm/rad
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Z_I, 0.1f);
|
||||
|
||||
/**
|
||||
* Body Z axis angular velocity integrator limit
|
||||
*
|
||||
* Body Z axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes.
|
||||
*
|
||||
* @unit Nm
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Z_I_LIM, 0.30f);
|
||||
|
||||
/**
|
||||
* Body Z axis angular velocity D gain
|
||||
*
|
||||
* Body Z axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 2.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Z_D, 0.0f);
|
||||
|
||||
/**
|
||||
* Body Z axis angular velocity feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @unit Nm/(rad/s)
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @increment 0.01
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Z_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Body Z axis angular velocity controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = AVC_Z_K * (AVC_Z_P * error
|
||||
* + AVC_Z_I * error_integral
|
||||
* + AVC_Z_D * error_derivative)
|
||||
* Set AVC_Z_P=1 to implement a PID in the ideal form.
|
||||
* Set AVC_Z_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Angular Velocity Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(AVC_Z_K, 1.0f);
|
||||
@ -0,0 +1,109 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 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 vehicle_model_params.c
|
||||
* Parameters for vehicle model.
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Mass
|
||||
*
|
||||
* @unit kg
|
||||
* @decimal 5
|
||||
* @increment 0.00001
|
||||
* @group Vehicle Model
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VM_MASS, 1.f);
|
||||
|
||||
/**
|
||||
* Inertia matrix, XX component
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @decimal 5
|
||||
* @increment 0.00001
|
||||
* @group Vehicle Model
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 0.01f);
|
||||
|
||||
/**
|
||||
* Inertia matrix, YY component
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @decimal 5
|
||||
* @increment 0.00001
|
||||
* @group Vehicle Model
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 0.01f);
|
||||
|
||||
/**
|
||||
* Inertia matrix, ZZ component
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @decimal 5
|
||||
* @increment 0.00001
|
||||
* @group Vehicle Model
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 0.01f);
|
||||
|
||||
/**
|
||||
* Inertia matrix, XY component
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @decimal 5
|
||||
* @increment 0.00001
|
||||
* @group Vehicle Model
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f);
|
||||
|
||||
/**
|
||||
* Inertia matrix, XZ component
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @decimal 5
|
||||
* @increment 0.00001
|
||||
* @group Vehicle Model
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f);
|
||||
|
||||
/**
|
||||
* Inertia matrix, YZ component
|
||||
*
|
||||
* @unit kg m^2
|
||||
* @decimal 5
|
||||
* @increment 0.00001
|
||||
* @group Vehicle Model
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f);
|
||||
@ -0,0 +1,45 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
#############################################################################
|
||||
set(MODULE_CFLAGS)
|
||||
px4_add_module(
|
||||
MODULE modules__attitude_estimator_q
|
||||
MAIN attitude_estimator_q
|
||||
COMPILE_FLAGS
|
||||
STACK_MAX 1600
|
||||
SRCS
|
||||
attitude_estimator_q_main.cpp
|
||||
DEPENDS
|
||||
git_ecl
|
||||
ecl_geo_lookup
|
||||
)
|
||||
|
||||
@ -0,0 +1,615 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file attitude_estimator_q_main.cpp
|
||||
*
|
||||
* Attitude estimator (quaternion based)
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#include <float.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/ecl/geo_lookup/geo_mag_declination.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_magnetometer.h>
|
||||
#include <uORB/topics/vehicle_odometry.h>
|
||||
|
||||
using matrix::Dcmf;
|
||||
using matrix::Eulerf;
|
||||
using matrix::Quatf;
|
||||
using matrix::Vector3f;
|
||||
using matrix::wrap_pi;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class AttitudeEstimatorQ : public ModuleBase<AttitudeEstimatorQ>, public ModuleParams, public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
AttitudeEstimatorQ();
|
||||
~AttitudeEstimatorQ() override = default;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
bool init();
|
||||
|
||||
private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
void update_parameters(bool force = false);
|
||||
|
||||
bool init_attq();
|
||||
|
||||
bool update(float dt);
|
||||
|
||||
// Update magnetic declination (in rads) immediately changing yaw rotation
|
||||
void update_mag_declination(float new_declination);
|
||||
|
||||
|
||||
const float _eo_max_std_dev = 100.0f; /**< Maximum permissible standard deviation for estimated orientation */
|
||||
const float _dt_min = 0.00001f;
|
||||
const float _dt_max = 0.02f;
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)};
|
||||
uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vision_odom_sub{ORB_ID(vehicle_visual_odometry)};
|
||||
uORB::Subscription _mocap_odom_sub{ORB_ID(vehicle_mocap_odometry)};
|
||||
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
|
||||
|
||||
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
|
||||
|
||||
float _mag_decl{0.0f};
|
||||
float _bias_max{0.0f};
|
||||
|
||||
Vector3f _gyro;
|
||||
Vector3f _accel;
|
||||
Vector3f _mag;
|
||||
|
||||
Vector3f _vision_hdg;
|
||||
Vector3f _mocap_hdg;
|
||||
|
||||
Quatf _q;
|
||||
Vector3f _rates;
|
||||
Vector3f _gyro_bias;
|
||||
|
||||
Vector3f _vel_prev;
|
||||
hrt_abstime _vel_prev_t{0};
|
||||
|
||||
Vector3f _pos_acc;
|
||||
|
||||
hrt_abstime _last_time{0};
|
||||
|
||||
bool _inited{false};
|
||||
bool _data_good{false};
|
||||
bool _ext_hdg_good{false};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc,
|
||||
(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag,
|
||||
(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg,
|
||||
(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias,
|
||||
(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl,
|
||||
(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a,
|
||||
(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m,
|
||||
(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,
|
||||
(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,
|
||||
(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
|
||||
)
|
||||
};
|
||||
|
||||
AttitudeEstimatorQ::AttitudeEstimatorQ() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
|
||||
{
|
||||
_vel_prev.zero();
|
||||
_pos_acc.zero();
|
||||
|
||||
_gyro.zero();
|
||||
_accel.zero();
|
||||
_mag.zero();
|
||||
|
||||
_vision_hdg.zero();
|
||||
_mocap_hdg.zero();
|
||||
|
||||
_q.zero();
|
||||
_rates.zero();
|
||||
_gyro_bias.zero();
|
||||
|
||||
update_parameters(true);
|
||||
}
|
||||
|
||||
bool
|
||||
AttitudeEstimatorQ::init()
|
||||
{
|
||||
if (!_sensors_sub.registerCallback()) {
|
||||
PX4_ERR("sensor combined callback registration failed!");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
AttitudeEstimatorQ::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_sensors_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
sensor_combined_s sensors;
|
||||
|
||||
if (_sensors_sub.update(&sensors)) {
|
||||
|
||||
update_parameters();
|
||||
|
||||
// Feed validator with recent sensor data
|
||||
if (sensors.timestamp > 0) {
|
||||
_gyro(0) = sensors.gyro_rad[0];
|
||||
_gyro(1) = sensors.gyro_rad[1];
|
||||
_gyro(2) = sensors.gyro_rad[2];
|
||||
}
|
||||
|
||||
if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
|
||||
_accel(0) = sensors.accelerometer_m_s2[0];
|
||||
_accel(1) = sensors.accelerometer_m_s2[1];
|
||||
_accel(2) = sensors.accelerometer_m_s2[2];
|
||||
|
||||
if (_accel.length() < 0.01f) {
|
||||
PX4_ERR("degenerate accel!");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Update magnetometer
|
||||
if (_magnetometer_sub.updated()) {
|
||||
vehicle_magnetometer_s magnetometer;
|
||||
|
||||
if (_magnetometer_sub.copy(&magnetometer)) {
|
||||
_mag(0) = magnetometer.magnetometer_ga[0];
|
||||
_mag(1) = magnetometer.magnetometer_ga[1];
|
||||
_mag(2) = magnetometer.magnetometer_ga[2];
|
||||
|
||||
if (_mag.length() < 0.01f) {
|
||||
PX4_ERR("degenerate mag!");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
_data_good = true;
|
||||
|
||||
// Update vision and motion capture heading
|
||||
_ext_hdg_good = false;
|
||||
|
||||
if (_vision_odom_sub.updated()) {
|
||||
vehicle_odometry_s vision;
|
||||
|
||||
if (_vision_odom_sub.copy(&vision)) {
|
||||
// validation check for vision attitude data
|
||||
bool vision_att_valid = PX4_ISFINITE(vision.q[0])
|
||||
&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
|
||||
vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
|
||||
fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
|
||||
vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
|
||||
|
||||
if (vision_att_valid) {
|
||||
Dcmf Rvis = Quatf(vision.q);
|
||||
Vector3f v(1.0f, 0.0f, 0.4f);
|
||||
|
||||
// Rvis is Rwr (robot respect to world) while v is respect to world.
|
||||
// Hence Rvis must be transposed having (Rwr)' * Vw
|
||||
// Rrw * Vw = vn. This way we have consistency
|
||||
_vision_hdg = Rvis.transpose() * v;
|
||||
|
||||
// vision external heading usage (ATT_EXT_HDG_M 1)
|
||||
if (_param_att_ext_hdg_m.get() == 1) {
|
||||
// Check for timeouts on data
|
||||
_ext_hdg_good = vision.timestamp_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 500000);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_mocap_odom_sub.updated()) {
|
||||
vehicle_odometry_s mocap;
|
||||
|
||||
if (_mocap_odom_sub.copy(&mocap)) {
|
||||
// validation check for mocap attitude data
|
||||
bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
|
||||
&& (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
|
||||
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
|
||||
fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
|
||||
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
|
||||
|
||||
if (mocap_att_valid) {
|
||||
Dcmf Rmoc = Quatf(mocap.q);
|
||||
Vector3f v(1.0f, 0.0f, 0.4f);
|
||||
|
||||
// Rmoc is Rwr (robot respect to world) while v is respect to world.
|
||||
// Hence Rmoc must be transposed having (Rwr)' * Vw
|
||||
// Rrw * Vw = vn. This way we have consistency
|
||||
_mocap_hdg = Rmoc.transpose() * v;
|
||||
|
||||
// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
|
||||
if (_param_att_ext_hdg_m.get() == 2) {
|
||||
// Check for timeouts on data
|
||||
_ext_hdg_good = mocap.timestamp_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_gps_sub.updated()) {
|
||||
vehicle_gps_position_s gps;
|
||||
|
||||
if (_gps_sub.copy(&gps)) {
|
||||
if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
|
||||
/* set magnetic declination automatically */
|
||||
update_mag_declination(get_mag_declination_radians(gps.lat, gps.lon));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_local_position_sub.updated()) {
|
||||
vehicle_local_position_s lpos;
|
||||
|
||||
if (_local_position_sub.copy(&lpos)) {
|
||||
|
||||
if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
|
||||
&& lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {
|
||||
|
||||
/* position data is actual */
|
||||
const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
|
||||
|
||||
/* velocity updated */
|
||||
if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
|
||||
float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
|
||||
/* calculate acceleration in body frame */
|
||||
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
|
||||
}
|
||||
|
||||
_vel_prev_t = lpos.timestamp;
|
||||
_vel_prev = vel;
|
||||
|
||||
} else {
|
||||
/* position data is outdated, reset acceleration */
|
||||
_pos_acc.zero();
|
||||
_vel_prev.zero();
|
||||
_vel_prev_t = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* time from previous iteration */
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
const float dt = math::constrain((now - _last_time) / 1e6f, _dt_min, _dt_max);
|
||||
_last_time = now;
|
||||
|
||||
if (update(dt)) {
|
||||
vehicle_attitude_s att = {};
|
||||
att.timestamp_sample = sensors.timestamp;
|
||||
_q.copyTo(att.q);
|
||||
|
||||
/* the instance count is not used here */
|
||||
att.timestamp = hrt_absolute_time();
|
||||
_att_pub.publish(att);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
AttitudeEstimatorQ::update_parameters(bool force)
|
||||
{
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated() || force) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
|
||||
// disable mag fusion if the system does not have a mag
|
||||
if (_param_sys_has_mag.get() == 0) {
|
||||
_param_att_w_mag.set(0.0f);
|
||||
}
|
||||
|
||||
// if the weight is zero (=mag disabled), make sure the estimator initializes
|
||||
if (_param_att_w_mag.get() < FLT_EPSILON) {
|
||||
_mag(0) = 1.f;
|
||||
_mag(1) = 0.f;
|
||||
_mag(2) = 0.f;
|
||||
}
|
||||
|
||||
update_mag_declination(math::radians(_param_att_mag_decl.get()));
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
AttitudeEstimatorQ::init_attq()
|
||||
{
|
||||
// Rotation matrix can be easily constructed from acceleration and mag field vectors
|
||||
// 'k' is Earth Z axis (Down) unit vector in body frame
|
||||
Vector3f k = -_accel;
|
||||
k.normalize();
|
||||
|
||||
// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
|
||||
Vector3f i = (_mag - k * (_mag * k));
|
||||
i.normalize();
|
||||
|
||||
// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
|
||||
Vector3f j = k % i;
|
||||
|
||||
// Fill rotation matrix
|
||||
Dcmf R;
|
||||
R.row(0) = i;
|
||||
R.row(1) = j;
|
||||
R.row(2) = k;
|
||||
|
||||
// Convert to quaternion
|
||||
_q = R;
|
||||
|
||||
// Compensate for magnetic declination
|
||||
Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);
|
||||
_q = _q * decl_rotation;
|
||||
|
||||
_q.normalize();
|
||||
|
||||
if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
|
||||
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
|
||||
_q.length() > 0.95f && _q.length() < 1.05f) {
|
||||
_inited = true;
|
||||
|
||||
} else {
|
||||
_inited = false;
|
||||
}
|
||||
|
||||
return _inited;
|
||||
}
|
||||
|
||||
bool
|
||||
AttitudeEstimatorQ::update(float dt)
|
||||
{
|
||||
if (!_inited) {
|
||||
|
||||
if (!_data_good) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return init_attq();
|
||||
}
|
||||
|
||||
Quatf q_last = _q;
|
||||
|
||||
// Angular rate of correction
|
||||
Vector3f corr;
|
||||
float spinRate = _gyro.length();
|
||||
|
||||
if (_param_att_ext_hdg_m.get() > 0 && _ext_hdg_good) {
|
||||
if (_param_att_ext_hdg_m.get() == 1) {
|
||||
// Vision heading correction
|
||||
// Project heading to global frame and extract XY component
|
||||
Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg);
|
||||
float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
|
||||
// Project correction to body frame
|
||||
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
|
||||
}
|
||||
|
||||
if (_param_att_ext_hdg_m.get() == 2) {
|
||||
// Mocap heading correction
|
||||
// Project heading to global frame and extract XY component
|
||||
Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
|
||||
float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
|
||||
// Project correction to body frame
|
||||
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
|
||||
}
|
||||
}
|
||||
|
||||
if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
|
||||
// Magnetometer correction
|
||||
// Project mag field vector to global frame and extract XY component
|
||||
Vector3f mag_earth = _q.conjugate(_mag);
|
||||
float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
|
||||
float gainMult = 1.0f;
|
||||
const float fifty_dps = 0.873f;
|
||||
|
||||
if (spinRate > fifty_dps) {
|
||||
gainMult = math::min(spinRate / fifty_dps, 10.0f);
|
||||
}
|
||||
|
||||
// Project magnetometer correction to body frame
|
||||
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
|
||||
}
|
||||
|
||||
_q.normalize();
|
||||
|
||||
|
||||
// Accelerometer correction
|
||||
// Project 'k' unit vector of earth frame to body frame
|
||||
// Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
|
||||
// Optimized version with dropped zeros
|
||||
Vector3f k(
|
||||
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
|
||||
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
|
||||
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
|
||||
);
|
||||
|
||||
// If we are not using acceleration compensation based on GPS velocity,
|
||||
// fuse accel data only if its norm is close to 1 g (reduces drift).
|
||||
const float accel_norm_sq = _accel.norm_squared();
|
||||
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
|
||||
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
|
||||
|
||||
if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) &&
|
||||
(accel_norm_sq < upper_accel_limit * upper_accel_limit))) {
|
||||
|
||||
corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();
|
||||
}
|
||||
|
||||
// Gyro bias estimation
|
||||
if (spinRate < 0.175f) {
|
||||
_gyro_bias += corr * (_param_att_w_gyro_bias.get() * dt);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
_rates = _gyro + _gyro_bias;
|
||||
|
||||
// Feed forward gyro
|
||||
corr += _rates;
|
||||
|
||||
// Apply correction to state
|
||||
_q += _q.derivative1(corr) * dt;
|
||||
|
||||
// Normalize quaternion
|
||||
_q.normalize();
|
||||
|
||||
if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
|
||||
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
|
||||
|
||||
// Reset quaternion to last good state
|
||||
_q = q_last;
|
||||
_rates.zero();
|
||||
_gyro_bias.zero();
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
AttitudeEstimatorQ::update_mag_declination(float new_declination)
|
||||
{
|
||||
// Apply initial declination or trivial rotations without changing estimation
|
||||
if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
|
||||
_mag_decl = new_declination;
|
||||
|
||||
} else {
|
||||
// Immediately rotate current estimation to avoid gyro bias growth
|
||||
Quatf decl_rotation = Eulerf(0.0f, 0.0f, new_declination - _mag_decl);
|
||||
_q = _q * decl_rotation;
|
||||
_mag_decl = new_declination;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
AttitudeEstimatorQ::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int
|
||||
AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
AttitudeEstimatorQ *instance = new AttitudeEstimatorQ();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int
|
||||
AttitudeEstimatorQ::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
Attitude estimator q.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("AttitudeEstimatorQ", "estimator");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[])
|
||||
{
|
||||
return AttitudeEstimatorQ::main(argc, argv);
|
||||
}
|
||||
@ -0,0 +1,136 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file attitude_estimator_q_params.c
|
||||
*
|
||||
* Parameters for attitude estimator (quaternion based)
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Complimentary filter accelerometer weight
|
||||
*
|
||||
* @group Attitude Q estimator
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
|
||||
|
||||
/**
|
||||
* Complimentary filter magnetometer weight
|
||||
*
|
||||
* Set to 0 to avoid using the magnetometer.
|
||||
*
|
||||
* @group Attitude Q estimator
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
|
||||
|
||||
/**
|
||||
* Complimentary filter external heading weight
|
||||
*
|
||||
* @group Attitude Q estimator
|
||||
* @min 0
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f);
|
||||
|
||||
/**
|
||||
* Complimentary filter gyroscope bias weight
|
||||
*
|
||||
* @group Attitude Q estimator
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
|
||||
|
||||
/**
|
||||
* Magnetic declination, in degrees
|
||||
*
|
||||
* This parameter is not used in normal operation,
|
||||
* as the declination is looked up based on the
|
||||
* GPS coordinates of the vehicle.
|
||||
*
|
||||
* @group Attitude Q estimator
|
||||
* @unit deg
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
|
||||
|
||||
/**
|
||||
* Automatic GPS based declination compensation
|
||||
*
|
||||
* @group Attitude Q estimator
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
|
||||
|
||||
/**
|
||||
* External heading usage mode (from Motion capture/Vision)
|
||||
*
|
||||
* Set to 1 to use heading estimate from vision.
|
||||
* Set to 2 to use heading from motion capture.
|
||||
*
|
||||
* @group Attitude Q estimator
|
||||
* @value 0 None
|
||||
* @value 1 Vision
|
||||
* @value 2 Motion Capture
|
||||
* @min 0
|
||||
* @max 2
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
|
||||
|
||||
/**
|
||||
* Acceleration compensation based on GPS velocity.
|
||||
*
|
||||
* @group Attitude Q estimator
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ATT_ACC_COMP, 1);
|
||||
|
||||
/**
|
||||
* Gyro bias limit
|
||||
*
|
||||
* @group Attitude Q estimator
|
||||
* @unit rad/s
|
||||
* @min 0
|
||||
* @max 2
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
|
||||
@ -0,0 +1,47 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015-2020 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 modules__battery_status
|
||||
MAIN battery_status
|
||||
SRCS
|
||||
battery_status.cpp
|
||||
analog_battery.cpp
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
battery
|
||||
conversion
|
||||
drivers__device
|
||||
mathlib
|
||||
)
|
||||
@ -0,0 +1,167 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019-2021 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 <stdio.h>
|
||||
#include <lib/battery/battery.h>
|
||||
#include "analog_battery.h"
|
||||
|
||||
// Defaults to use if the parameters are not set
|
||||
#if BOARD_NUMBER_BRICKS > 0
|
||||
#if defined(BOARD_BATT_V_LIST) && defined(BOARD_BATT_I_LIST)
|
||||
static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
|
||||
static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
|
||||
#else
|
||||
static constexpr int DEFAULT_V_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
|
||||
static constexpr int DEFAULT_I_CHANNEL[BOARD_NUMBER_BRICKS] = {0};
|
||||
#endif
|
||||
#else
|
||||
static constexpr int DEFAULT_V_CHANNEL[1] = {0};
|
||||
static constexpr int DEFAULT_I_CHANNEL[1] = {0};
|
||||
#endif
|
||||
|
||||
AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us) :
|
||||
Battery(index, parent, sample_interval_us)
|
||||
{
|
||||
char param_name[17];
|
||||
|
||||
_analog_param_handles.v_offs_cur = param_find("BAT_V_OFFS_CURR");
|
||||
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_V_DIV", index);
|
||||
_analog_param_handles.v_div = param_find(param_name);
|
||||
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_A_PER_V", index);
|
||||
_analog_param_handles.a_per_v = param_find(param_name);
|
||||
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_V_CHANNEL", index);
|
||||
_analog_param_handles.v_channel = param_find(param_name);
|
||||
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_I_CHANNEL", index);
|
||||
_analog_param_handles.i_channel = param_find(param_name);
|
||||
|
||||
_analog_param_handles.v_div_old = param_find("BAT_V_DIV");
|
||||
_analog_param_handles.a_per_v_old = param_find("BAT_A_PER_V");
|
||||
_analog_param_handles.adc_channel_old = param_find("BAT_ADC_CHANNEL");
|
||||
}
|
||||
|
||||
void
|
||||
AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
|
||||
int source, int priority, float throttle_normalized)
|
||||
{
|
||||
float voltage_v = voltage_raw * _analog_params.v_div;
|
||||
float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v;
|
||||
|
||||
bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V &&
|
||||
(BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid());
|
||||
|
||||
|
||||
Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected,
|
||||
source, priority, throttle_normalized);
|
||||
}
|
||||
|
||||
bool AnalogBattery::is_valid()
|
||||
{
|
||||
#ifdef BOARD_BRICK_VALID_LIST
|
||||
bool valid[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
|
||||
return valid[_index - 1];
|
||||
#else
|
||||
// TODO: Maybe return false instead?
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
int AnalogBattery::get_voltage_channel()
|
||||
{
|
||||
if (_analog_params.v_channel >= 0) {
|
||||
return _analog_params.v_channel;
|
||||
|
||||
} else {
|
||||
return DEFAULT_V_CHANNEL[_index - 1];
|
||||
}
|
||||
}
|
||||
|
||||
int AnalogBattery::get_current_channel()
|
||||
{
|
||||
if (_analog_params.i_channel >= 0) {
|
||||
return _analog_params.i_channel;
|
||||
|
||||
} else {
|
||||
return DEFAULT_I_CHANNEL[_index - 1];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
AnalogBattery::updateParams()
|
||||
{
|
||||
if (_index == 1) {
|
||||
migrateParam<float>(_analog_param_handles.v_div_old, _analog_param_handles.v_div, &_analog_params.v_div_old,
|
||||
&_analog_params.v_div, _first_parameter_update);
|
||||
migrateParam<float>(_analog_param_handles.a_per_v_old, _analog_param_handles.a_per_v, &_analog_params.a_per_v_old,
|
||||
&_analog_params.a_per_v, _first_parameter_update);
|
||||
migrateParam<int32_t>(_analog_param_handles.adc_channel_old, _analog_param_handles.v_channel,
|
||||
&_analog_params.adc_channel_old, &_analog_params.v_channel, _first_parameter_update);
|
||||
|
||||
} else {
|
||||
param_get(_analog_param_handles.v_div, &_analog_params.v_div);
|
||||
param_get(_analog_param_handles.a_per_v, &_analog_params.a_per_v);
|
||||
param_get(_analog_param_handles.v_channel, &_analog_params.v_channel);
|
||||
}
|
||||
|
||||
param_get(_analog_param_handles.i_channel, &_analog_params.i_channel);
|
||||
param_get(_analog_param_handles.v_offs_cur, &_analog_params.v_offs_cur);
|
||||
|
||||
if (_analog_params.v_div < 0.0f) {
|
||||
/* apply scaling according to defaults if set to default */
|
||||
_analog_params.v_div = BOARD_BATTERY1_V_DIV;
|
||||
param_set_no_notification(_analog_param_handles.v_div, &_analog_params.v_div);
|
||||
|
||||
if (_index == 1) {
|
||||
_analog_params.v_div_old = BOARD_BATTERY1_V_DIV;
|
||||
param_set_no_notification(_analog_param_handles.v_div_old, &_analog_params.v_div_old);
|
||||
}
|
||||
}
|
||||
|
||||
if (_analog_params.a_per_v < 0.0f) {
|
||||
/* apply scaling according to defaults if set to default */
|
||||
|
||||
_analog_params.a_per_v = BOARD_BATTERY1_A_PER_V;
|
||||
param_set_no_notification(_analog_param_handles.a_per_v, &_analog_params.a_per_v);
|
||||
|
||||
if (_index == 1) {
|
||||
_analog_params.a_per_v_old = BOARD_BATTERY1_A_PER_V;
|
||||
param_set_no_notification(_analog_param_handles.a_per_v_old, &_analog_params.a_per_v_old);
|
||||
}
|
||||
}
|
||||
|
||||
Battery::updateParams();
|
||||
}
|
||||
@ -0,0 +1,100 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <battery/battery.h>
|
||||
#include <parameters/param.h>
|
||||
|
||||
class AnalogBattery : public Battery
|
||||
{
|
||||
public:
|
||||
AnalogBattery(int index, ModuleParams *parent, const int sample_interval_us);
|
||||
|
||||
/**
|
||||
* Update current battery status message.
|
||||
*
|
||||
* @param voltage_raw Battery voltage read from ADC, volts
|
||||
* @param current_raw Voltage of current sense resistor, volts
|
||||
* @param timestamp Time at which the ADC was read (use hrt_absolute_time())
|
||||
* @param source The source as defined by param BAT%d_SOURCE
|
||||
* @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
|
||||
* @param throttle_normalized Throttle of the vehicle, between 0 and 1
|
||||
*/
|
||||
void updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw,
|
||||
int source, int priority, float throttle_normalized);
|
||||
|
||||
/**
|
||||
* Whether the ADC channel for the voltage of this battery is valid.
|
||||
* Corresponds to BOARD_BRICK_VALID_LIST
|
||||
*/
|
||||
bool is_valid();
|
||||
|
||||
/**
|
||||
* Which ADC channel is used for voltage reading of this battery
|
||||
*/
|
||||
int get_voltage_channel();
|
||||
|
||||
/**
|
||||
* Which ADC channel is used for current reading of this battery
|
||||
*/
|
||||
int get_current_channel();
|
||||
|
||||
protected:
|
||||
|
||||
struct {
|
||||
param_t v_offs_cur;
|
||||
param_t v_div;
|
||||
param_t a_per_v;
|
||||
param_t v_channel;
|
||||
param_t i_channel;
|
||||
|
||||
param_t v_div_old;
|
||||
param_t a_per_v_old;
|
||||
param_t adc_channel_old;
|
||||
} _analog_param_handles;
|
||||
|
||||
struct {
|
||||
float v_offs_cur;
|
||||
float v_div;
|
||||
float a_per_v;
|
||||
int32_t v_channel;
|
||||
int32_t i_channel;
|
||||
|
||||
float v_div_old;
|
||||
float a_per_v_old;
|
||||
int32_t adc_channel_old;
|
||||
} _analog_params;
|
||||
|
||||
virtual void updateParams() override;
|
||||
};
|
||||
@ -0,0 +1,43 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2017 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Offset in volt as seen by the ADC input of the current sensor.
|
||||
*
|
||||
* This offset will be subtracted before calculating the battery
|
||||
* current based on the voltage.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0);
|
||||
@ -0,0 +1,39 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* This parameter is deprecated. Please use BAT1_ADC_CHANNEL.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1);
|
||||
@ -0,0 +1,321 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2019 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 sensors.cpp
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomas@px4.io>
|
||||
* @author Anton Babushkin <anton@px4.io>
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_adc.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/battery/battery.h>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/adc_report.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include "analog_battery.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
/**
|
||||
* The channel definitions (e.g., ADC_BATTERY_VOLTAGE_CHANNEL, ADC_BATTERY_CURRENT_CHANNEL, and ADC_AIRSPEED_VOLTAGE_CHANNEL) are defined in board_config.h
|
||||
*/
|
||||
|
||||
#ifndef BOARD_NUMBER_BRICKS
|
||||
#error "battery_status module requires power bricks"
|
||||
#endif
|
||||
|
||||
#if BOARD_NUMBER_BRICKS == 0
|
||||
#error "battery_status module requires power bricks"
|
||||
#endif
|
||||
|
||||
class BatteryStatus : public ModuleBase<BatteryStatus>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
BatteryStatus();
|
||||
~BatteryStatus() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
bool init();
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< notification of parameter updates */
|
||||
uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)};
|
||||
|
||||
static constexpr uint32_t SAMPLE_FREQUENCY_HZ = 100;
|
||||
static constexpr uint32_t SAMPLE_INTERVAL_US = 1_s / SAMPLE_FREQUENCY_HZ;
|
||||
|
||||
AnalogBattery _battery1;
|
||||
|
||||
#if BOARD_NUMBER_BRICKS > 1
|
||||
AnalogBattery _battery2;
|
||||
#endif
|
||||
|
||||
AnalogBattery *_analogBatteries[BOARD_NUMBER_BRICKS] {
|
||||
&_battery1,
|
||||
#if BOARD_NUMBER_BRICKS > 1
|
||||
&_battery2,
|
||||
#endif
|
||||
}; // End _analogBatteries
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
/**
|
||||
* Check for changes in parameters.
|
||||
*/
|
||||
void parameter_update_poll(bool forced = false);
|
||||
|
||||
/**
|
||||
* Poll the ADC and update readings to suit.
|
||||
*
|
||||
* @param raw Combined sensor data structure into which
|
||||
* data should be returned.
|
||||
*/
|
||||
void adc_poll();
|
||||
};
|
||||
|
||||
BatteryStatus::BatteryStatus() :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
_battery1(1, this, SAMPLE_INTERVAL_US),
|
||||
#if BOARD_NUMBER_BRICKS > 1
|
||||
_battery2(2, this, SAMPLE_INTERVAL_US),
|
||||
#endif
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME))
|
||||
{
|
||||
updateParams();
|
||||
}
|
||||
|
||||
BatteryStatus::~BatteryStatus()
|
||||
{
|
||||
ScheduleClear();
|
||||
}
|
||||
|
||||
void
|
||||
BatteryStatus::parameter_update_poll(bool forced)
|
||||
{
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated() || forced) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
BatteryStatus::adc_poll()
|
||||
{
|
||||
/* For legacy support we publish the battery_status for the Battery that is
|
||||
* associated with the Brick that is the selected source for VDD_5V_IN
|
||||
* Selection is done in HW ala a LTC4417 or similar, or may be hard coded
|
||||
* Like in the FMUv4
|
||||
*/
|
||||
|
||||
/* Per Brick readings with default unread channels at 0 */
|
||||
float bat_current_adc_readings[BOARD_NUMBER_BRICKS] {};
|
||||
float bat_voltage_adc_readings[BOARD_NUMBER_BRICKS] {};
|
||||
|
||||
int selected_source = -1;
|
||||
|
||||
adc_report_s adc_report;
|
||||
|
||||
if (_adc_report_sub.update(&adc_report)) {
|
||||
|
||||
/* Read add channels we got */
|
||||
for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
|
||||
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
|
||||
|
||||
/* Once we have subscriptions, Do this once for the lowest (highest priority
|
||||
* supply on power controller) that is valid.
|
||||
*/
|
||||
if (selected_source < 0 && _analogBatteries[b]->is_valid()) {
|
||||
/* Indicate the lowest brick (highest priority supply on power controller)
|
||||
* that is valid as the one that is the selected source for the
|
||||
* VDD_5V_IN
|
||||
*/
|
||||
selected_source = b;
|
||||
}
|
||||
|
||||
/* look for specific channels and process the raw voltage to measurement data */
|
||||
|
||||
if (adc_report.channel_id[i] == _analogBatteries[b]->get_voltage_channel()) {
|
||||
/* Voltage in volts */
|
||||
bat_voltage_adc_readings[b] = adc_report.raw_data[i] *
|
||||
adc_report.v_ref /
|
||||
adc_report.resolution;
|
||||
|
||||
} else if (adc_report.channel_id[i] == _analogBatteries[b]->get_current_channel()) {
|
||||
bat_current_adc_readings[b] = adc_report.raw_data[i] *
|
||||
adc_report.v_ref /
|
||||
adc_report.resolution;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
|
||||
|
||||
actuator_controls_s ctrl{};
|
||||
_actuator_ctrl_0_sub.copy(&ctrl);
|
||||
|
||||
_analogBatteries[b]->updateBatteryStatusADC(
|
||||
hrt_absolute_time(),
|
||||
bat_voltage_adc_readings[b],
|
||||
bat_current_adc_readings[b],
|
||||
battery_status_s::BATTERY_SOURCE_POWER_MODULE,
|
||||
b,
|
||||
ctrl.control[actuator_controls_s::INDEX_THROTTLE]
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
BatteryStatus::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
/* check parameters for updates */
|
||||
parameter_update_poll();
|
||||
|
||||
/* check battery voltage */
|
||||
adc_poll();
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
int
|
||||
BatteryStatus::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
BatteryStatus *instance = new BatteryStatus();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
bool
|
||||
BatteryStatus::init()
|
||||
{
|
||||
return _adc_report_sub.registerCallback();
|
||||
}
|
||||
|
||||
int BatteryStatus::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int BatteryStatus::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
|
||||
The provided functionality includes:
|
||||
- Read the output from the ADC driver (via ioctl interface) and publish `battery_status`.
|
||||
|
||||
|
||||
### Implementation
|
||||
It runs in its own thread and polls on the currently selected gyro topic.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("battery_status", "system");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int battery_status_main(int argc, char *argv[])
|
||||
{
|
||||
return BatteryStatus::main(argc, argv);
|
||||
}
|
||||
@ -0,0 +1,63 @@
|
||||
__max_num_config_instances: &max_num_config_instances 2
|
||||
|
||||
module_name: battery_status
|
||||
|
||||
parameters:
|
||||
- group: Battery Calibration
|
||||
definitions:
|
||||
BAT${i}_V_DIV:
|
||||
description:
|
||||
short: Battery ${i} voltage divider (V divider)
|
||||
long: |
|
||||
This is the divider from battery ${i} voltage to ADC voltage.
|
||||
If using e.g. Mauch power modules the value from the datasheet
|
||||
can be applied straight here. A value of -1 means to use
|
||||
the board default.
|
||||
|
||||
type: float
|
||||
decimal: 8
|
||||
reboot_required: true
|
||||
num_instances: *max_num_config_instances
|
||||
instance_start: 1
|
||||
default: [-1.0, -1.0]
|
||||
|
||||
BAT${i}_A_PER_V:
|
||||
description:
|
||||
short: Battery ${i} current per volt (A/V)
|
||||
long: |
|
||||
The voltage seen by the ADC multiplied by this factor
|
||||
will determine the battery current. A value of -1 means to use
|
||||
the board default.
|
||||
|
||||
type: float
|
||||
decimal: 8
|
||||
reboot_required: true
|
||||
num_instances: *max_num_config_instances
|
||||
instance_start: 1
|
||||
default: [-1.0, -1.0]
|
||||
|
||||
BAT${i}_V_CHANNEL:
|
||||
description:
|
||||
short: Battery ${i} Voltage ADC Channel
|
||||
long: |
|
||||
This parameter specifies the ADC channel used to monitor voltage of main power battery.
|
||||
A value of -1 means to use the board default.
|
||||
|
||||
type: int32
|
||||
reboot_required: true
|
||||
num_instances: *max_num_config_instances
|
||||
instance_start: 1
|
||||
default: [-1, -1]
|
||||
|
||||
BAT${i}_I_CHANNEL:
|
||||
description:
|
||||
short: Battery ${i} Current ADC Channel
|
||||
long: |
|
||||
This parameter specifies the ADC channel used to monitor current of main power battery.
|
||||
A value of -1 means to use the board default.
|
||||
|
||||
type: int32
|
||||
reboot_required: true
|
||||
num_instances: *max_num_config_instances
|
||||
instance_start: 1
|
||||
default: [-1, -1]
|
||||
@ -0,0 +1,41 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2017-2019 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 modules__camera_feedback
|
||||
MAIN camera_feedback
|
||||
SRCS
|
||||
CameraFeedback.cpp
|
||||
CameraFeedback.hpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
)
|
||||
@ -0,0 +1,168 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2019 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 "CameraFeedback.hpp"
|
||||
|
||||
CameraFeedback::CameraFeedback() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
|
||||
{
|
||||
}
|
||||
|
||||
bool
|
||||
CameraFeedback::init()
|
||||
{
|
||||
if (!_trigger_sub.registerCallback()) {
|
||||
PX4_ERR("camera_trigger callback registration failed!");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
CameraFeedback::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_trigger_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
camera_trigger_s trig{};
|
||||
|
||||
if (_trigger_sub.update(&trig)) {
|
||||
|
||||
// update geotagging subscriptions
|
||||
vehicle_global_position_s gpos{};
|
||||
_gpos_sub.copy(&gpos);
|
||||
|
||||
vehicle_attitude_s att{};
|
||||
_att_sub.copy(&att);
|
||||
|
||||
if (trig.timestamp == 0 ||
|
||||
gpos.timestamp == 0 ||
|
||||
att.timestamp == 0) {
|
||||
|
||||
// reject until we have valid data
|
||||
return;
|
||||
}
|
||||
|
||||
camera_capture_s capture{};
|
||||
|
||||
// Fill timestamps
|
||||
capture.timestamp = trig.timestamp;
|
||||
capture.timestamp_utc = trig.timestamp_utc;
|
||||
|
||||
// Fill image sequence
|
||||
capture.seq = trig.seq;
|
||||
|
||||
// Fill position data
|
||||
capture.lat = gpos.lat;
|
||||
capture.lon = gpos.lon;
|
||||
capture.alt = gpos.alt;
|
||||
|
||||
if (gpos.terrain_alt_valid) {
|
||||
capture.ground_distance = gpos.alt - gpos.terrain_alt;
|
||||
|
||||
} else {
|
||||
capture.ground_distance = -1.0f;
|
||||
}
|
||||
|
||||
// Fill attitude data
|
||||
// TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
|
||||
capture.q[0] = att.q[0];
|
||||
capture.q[1] = att.q[1];
|
||||
capture.q[2] = att.q[2];
|
||||
capture.q[3] = att.q[3];
|
||||
capture.result = 1;
|
||||
|
||||
_capture_pub.publish(capture);
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
CameraFeedback::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
CameraFeedback *instance = new CameraFeedback();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int
|
||||
CameraFeedback::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int
|
||||
CameraFeedback::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("camera_feedback", "system");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int camera_feedback_main(int argc, char *argv[])
|
||||
{
|
||||
return CameraFeedback::main(argc, argv);
|
||||
}
|
||||
@ -0,0 +1,86 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
*
|
||||
* Online and offline geotagging from camera feedback
|
||||
*
|
||||
* @author Mohammed Kabir <kabir@uasys.io>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/camera_capture.h>
|
||||
#include <uORB/topics/camera_trigger.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
||||
class CameraFeedback : public ModuleBase<CameraFeedback>, public ModuleParams, public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
CameraFeedback();
|
||||
~CameraFeedback() override = default;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
bool init();
|
||||
|
||||
private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _trigger_sub{this, ORB_ID(camera_trigger)};
|
||||
|
||||
uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
|
||||
|
||||
uORB::Publication<camera_capture_s> _capture_pub{ORB_ID(camera_capture)};
|
||||
};
|
||||
@ -0,0 +1,298 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017 Intel Corporation. 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 "ArmAuthorization.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static orb_advert_t *mavlink_log_pub;
|
||||
static int command_ack_sub = -1;
|
||||
|
||||
static hrt_abstime auth_timeout;
|
||||
static hrt_abstime auth_req_time;
|
||||
|
||||
static hrt_abstime _param_com_arm_auth_timout;
|
||||
static enum arm_auth_methods _param_com_arm_auth_method;
|
||||
static int32_t _param_com_arm_auth_id;
|
||||
|
||||
static enum {
|
||||
ARM_AUTH_IDLE = 0,
|
||||
ARM_AUTH_WAITING_AUTH,
|
||||
ARM_AUTH_WAITING_AUTH_WITH_ACK,
|
||||
ARM_AUTH_MISSION_APPROVED
|
||||
} state = ARM_AUTH_IDLE;
|
||||
|
||||
static uint8_t *system_id;
|
||||
|
||||
static uint8_t _auth_method_arm_req_check();
|
||||
static uint8_t _auth_method_two_arm_check();
|
||||
|
||||
static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
|
||||
_auth_method_arm_req_check,
|
||||
_auth_method_two_arm_check,
|
||||
};
|
||||
|
||||
void arm_auth_param_update()
|
||||
{
|
||||
float timeout = 0;
|
||||
param_get(param_find("COM_ARM_AUTH_TO"), &timeout);
|
||||
_param_com_arm_auth_timout = timeout * 1_s;
|
||||
|
||||
int32_t auth_method = ARM_AUTH_METHOD_ARM_REQ;
|
||||
param_get(param_find("COM_ARM_AUTH_MET"), &auth_method);
|
||||
|
||||
if (auth_method >= 0 && auth_method < ARM_AUTH_METHOD_LAST) {
|
||||
_param_com_arm_auth_method = (arm_auth_methods)auth_method;
|
||||
|
||||
} else {
|
||||
_param_com_arm_auth_method = ARM_AUTH_METHOD_ARM_REQ;
|
||||
}
|
||||
|
||||
param_get(param_find("COM_ARM_AUTH_ID"), &_param_com_arm_auth_id);
|
||||
}
|
||||
|
||||
static void arm_auth_request_msg_send()
|
||||
{
|
||||
vehicle_command_s vcmd{};
|
||||
vcmd.timestamp = hrt_absolute_time();
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST;
|
||||
vcmd.target_system = _param_com_arm_auth_id;
|
||||
|
||||
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
|
||||
vcmd_pub.publish(vcmd);
|
||||
}
|
||||
|
||||
static uint8_t _auth_method_arm_req_check()
|
||||
{
|
||||
switch (state) {
|
||||
case ARM_AUTH_IDLE:
|
||||
/* no authentication in process? handle bellow */
|
||||
break;
|
||||
|
||||
case ARM_AUTH_MISSION_APPROVED:
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
||||
|
||||
default:
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
|
||||
}
|
||||
|
||||
/* handling ARM_AUTH_IDLE */
|
||||
arm_auth_request_msg_send();
|
||||
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
auth_req_time = now;
|
||||
auth_timeout = now + _param_com_arm_auth_timout;
|
||||
state = ARM_AUTH_WAITING_AUTH;
|
||||
|
||||
while (now < auth_timeout) {
|
||||
arm_auth_update(now);
|
||||
|
||||
if (state != ARM_AUTH_WAITING_AUTH && state != ARM_AUTH_WAITING_AUTH_WITH_ACK) {
|
||||
break;
|
||||
}
|
||||
|
||||
/* 0.5ms */
|
||||
px4_usleep(500);
|
||||
now = hrt_absolute_time();
|
||||
}
|
||||
|
||||
switch (state) {
|
||||
case ARM_AUTH_WAITING_AUTH:
|
||||
case ARM_AUTH_WAITING_AUTH_WITH_ACK:
|
||||
state = ARM_AUTH_IDLE;
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return state == ARM_AUTH_MISSION_APPROVED ?
|
||||
vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
|
||||
}
|
||||
|
||||
static uint8_t _auth_method_two_arm_check()
|
||||
{
|
||||
switch (state) {
|
||||
case ARM_AUTH_IDLE:
|
||||
/* no authentication in process? handle bellow */
|
||||
break;
|
||||
|
||||
case ARM_AUTH_MISSION_APPROVED:
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
||||
|
||||
case ARM_AUTH_WAITING_AUTH:
|
||||
case ARM_AUTH_WAITING_AUTH_WITH_ACK:
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
|
||||
|
||||
default:
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
|
||||
}
|
||||
|
||||
/* handling ARM_AUTH_IDLE */
|
||||
arm_auth_request_msg_send();
|
||||
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
auth_req_time = now;
|
||||
auth_timeout = now + _param_com_arm_auth_timout;
|
||||
state = ARM_AUTH_WAITING_AUTH;
|
||||
|
||||
mavlink_log_info(mavlink_log_pub, "Arm auth: Requesting authorization...");
|
||||
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
|
||||
}
|
||||
|
||||
uint8_t arm_auth_check()
|
||||
{
|
||||
if (_param_com_arm_auth_method < ARM_AUTH_METHOD_LAST) {
|
||||
return arm_check_method[_param_com_arm_auth_method]();
|
||||
}
|
||||
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
|
||||
}
|
||||
|
||||
void arm_auth_update(hrt_abstime now, bool param_update)
|
||||
{
|
||||
if (param_update) {
|
||||
arm_auth_param_update();
|
||||
}
|
||||
|
||||
switch (state) {
|
||||
case ARM_AUTH_WAITING_AUTH:
|
||||
case ARM_AUTH_WAITING_AUTH_WITH_ACK:
|
||||
/* handle bellow */
|
||||
break;
|
||||
|
||||
case ARM_AUTH_MISSION_APPROVED:
|
||||
if (now > auth_timeout) {
|
||||
state = ARM_AUTH_IDLE;
|
||||
}
|
||||
|
||||
return;
|
||||
|
||||
case ARM_AUTH_IDLE:
|
||||
default:
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* handling ARM_AUTH_WAITING_AUTH, ARM_AUTH_WAITING_AUTH_WITH_ACK
|
||||
*/
|
||||
vehicle_command_ack_s command_ack;
|
||||
bool updated = false;
|
||||
|
||||
orb_check(command_ack_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_command_ack), command_ack_sub, &command_ack);
|
||||
}
|
||||
|
||||
if (updated
|
||||
&& command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST
|
||||
&& command_ack.target_system == *system_id
|
||||
&& command_ack.timestamp > auth_req_time) {
|
||||
switch (command_ack.result) {
|
||||
case vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS:
|
||||
state = ARM_AUTH_WAITING_AUTH_WITH_ACK;
|
||||
break;
|
||||
|
||||
case vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED:
|
||||
mavlink_log_info(mavlink_log_pub,
|
||||
"Arm auth: Authorized for the next %" PRId32 " seconds",
|
||||
command_ack.result_param2);
|
||||
state = ARM_AUTH_MISSION_APPROVED;
|
||||
auth_timeout = command_ack.timestamp + (command_ack.result_param2 * 1000000);
|
||||
return;
|
||||
|
||||
case vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED:
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: Temporarily rejected");
|
||||
state = ARM_AUTH_IDLE;
|
||||
return;
|
||||
|
||||
case vehicle_command_ack_s::VEHICLE_RESULT_DENIED:
|
||||
default:
|
||||
switch (command_ack.result_param1) {
|
||||
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_NONE:
|
||||
/* Authorizer will send reason to ground station */
|
||||
break;
|
||||
|
||||
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT:
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied, waypoint %" PRId32 " have a invalid value",
|
||||
command_ack.result_param2);
|
||||
break;
|
||||
|
||||
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_TIMEOUT:
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied by timeout in authorizer");
|
||||
break;
|
||||
|
||||
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE:
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because airspace is in use");
|
||||
break;
|
||||
|
||||
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_BAD_WEATHER:
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because of bad weather");
|
||||
break;
|
||||
|
||||
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_GENERIC:
|
||||
default:
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied");
|
||||
}
|
||||
|
||||
state = ARM_AUTH_IDLE;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (now > auth_timeout) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
|
||||
state = ARM_AUTH_IDLE;
|
||||
}
|
||||
}
|
||||
|
||||
void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *sys_id)
|
||||
{
|
||||
arm_auth_param_update();
|
||||
system_id = sys_id;
|
||||
command_ack_sub = orb_subscribe(ORB_ID(vehicle_command_ack));
|
||||
mavlink_log_pub = mav_log_pub;
|
||||
}
|
||||
@ -0,0 +1,49 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017 Intel Corporation. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
enum arm_auth_methods {
|
||||
ARM_AUTH_METHOD_ARM_REQ = 0,
|
||||
ARM_AUTH_METHOD_TWO_ARM_REQ,
|
||||
ARM_AUTH_METHOD_LAST
|
||||
};
|
||||
|
||||
void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *system_id);
|
||||
void arm_auth_update(hrt_abstime now, bool param_update = true);
|
||||
uint8_t arm_auth_check();
|
||||
@ -0,0 +1,40 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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_library(ArmAuthorization
|
||||
ArmAuthorization.cpp
|
||||
)
|
||||
target_include_directories(ArmAuthorization
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
)
|
||||
@ -0,0 +1,36 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(PreFlightCheck)
|
||||
add_subdirectory(ArmAuthorization)
|
||||
add_subdirectory(HealthFlags)
|
||||
@ -0,0 +1,37 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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_library(HealthFlags
|
||||
HealthFlags.cpp
|
||||
)
|
||||
target_include_directories(HealthFlags PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@ -0,0 +1,128 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2020 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 HealthFlags.cpp
|
||||
*
|
||||
* Contains helper functions to efficiently set the system health flags from commander and preflight check.
|
||||
*
|
||||
* @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
|
||||
*/
|
||||
|
||||
#include "HealthFlags.h"
|
||||
|
||||
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
|
||||
{
|
||||
PX4_DEBUG("set_health_flags: Type %llu pres=%u enabl=%u ok=%u", (long long unsigned int)subsystem_type, present,
|
||||
enabled, ok);
|
||||
|
||||
if (present) {
|
||||
status.onboard_control_sensors_present |= (uint32_t)subsystem_type;
|
||||
|
||||
} else {
|
||||
status.onboard_control_sensors_present &= ~(uint32_t)subsystem_type;
|
||||
}
|
||||
|
||||
if (enabled) {
|
||||
status.onboard_control_sensors_enabled |= (uint32_t)subsystem_type;
|
||||
|
||||
} else {
|
||||
status.onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type;
|
||||
}
|
||||
|
||||
if (ok) {
|
||||
status.onboard_control_sensors_health |= (uint32_t)subsystem_type;
|
||||
|
||||
} else {
|
||||
status.onboard_control_sensors_health &= ~(uint32_t)subsystem_type;
|
||||
}
|
||||
}
|
||||
|
||||
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status)
|
||||
{
|
||||
set_health_flags(subsystem_type, present, status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy,
|
||||
status);
|
||||
}
|
||||
|
||||
void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status)
|
||||
{
|
||||
set_health_flags(subsystem_type, status.onboard_control_sensors_present & (uint32_t)subsystem_type,
|
||||
status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy, status);
|
||||
}
|
||||
|
||||
void _print_sub(const char *name, const vehicle_status_s &status, uint32_t bit)
|
||||
{
|
||||
PX4_INFO("%s:\t\t%s\t%s", name,
|
||||
(status.onboard_control_sensors_enabled & bit) ? "EN" : " ",
|
||||
(status.onboard_control_sensors_present & bit) ?
|
||||
((status.onboard_control_sensors_health & bit) ? "OK" : "ERR") :
|
||||
(status.onboard_control_sensors_enabled & bit) ? "OFF" : "");
|
||||
}
|
||||
|
||||
void print_health_flags(const vehicle_status_s &status)
|
||||
{
|
||||
#ifndef CONSTRAINED_FLASH
|
||||
PX4_INFO("DEVICE\t\tSTATUS");
|
||||
PX4_INFO("----------------------------------");
|
||||
_print_sub("GYRO", status, subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
|
||||
_print_sub("ACC", status, subsystem_info_s::SUBSYSTEM_TYPE_ACC);
|
||||
_print_sub("MAG", status, subsystem_info_s::SUBSYSTEM_TYPE_MAG);
|
||||
_print_sub("PRESS", status, subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE);
|
||||
_print_sub("AIRSP", status, subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE);
|
||||
_print_sub("GPS", status, subsystem_info_s::SUBSYSTEM_TYPE_GPS);
|
||||
_print_sub("OPT", status, subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW);
|
||||
_print_sub("VIO", status, subsystem_info_s::SUBSYSTEM_TYPE_CVPOSITION);
|
||||
_print_sub("LASER", status, subsystem_info_s::SUBSYSTEM_TYPE_LASERPOSITION);
|
||||
_print_sub("GTRUTH", status, subsystem_info_s::SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH);
|
||||
_print_sub("RATES", status, subsystem_info_s::SUBSYSTEM_TYPE_ANGULARRATECONTROL);
|
||||
_print_sub("ATT", status, subsystem_info_s::SUBSYSTEM_TYPE_ATTITUDESTABILIZATION);
|
||||
_print_sub("YAW", status, subsystem_info_s::SUBSYSTEM_TYPE_YAWPOSITION);
|
||||
_print_sub("ALTCTL", status, subsystem_info_s::SUBSYSTEM_TYPE_ALTITUDECONTROL);
|
||||
_print_sub("POS", status, subsystem_info_s::SUBSYSTEM_TYPE_POSITIONCONTROL);
|
||||
_print_sub("MOT", status, subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL);
|
||||
_print_sub("RC ", status, subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER);
|
||||
_print_sub("GYRO2", status, subsystem_info_s::SUBSYSTEM_TYPE_GYRO2);
|
||||
_print_sub("ACC2", status, subsystem_info_s::SUBSYSTEM_TYPE_ACC2);
|
||||
_print_sub("MAG2", status, subsystem_info_s::SUBSYSTEM_TYPE_MAG2);
|
||||
_print_sub("GEOFENCE", status, subsystem_info_s::SUBSYSTEM_TYPE_GEOFENCE);
|
||||
_print_sub("AHRS", status, subsystem_info_s::SUBSYSTEM_TYPE_AHRS);
|
||||
_print_sub("TERRAIN", status, subsystem_info_s::SUBSYSTEM_TYPE_TERRAIN);
|
||||
_print_sub("REVMOT", status, subsystem_info_s::SUBSYSTEM_TYPE_REVERSEMOTOR);
|
||||
_print_sub("LOGGIN", status, subsystem_info_s::SUBSYSTEM_TYPE_LOGGING);
|
||||
_print_sub("BATT", status, subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY);
|
||||
_print_sub("PROX", status, subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY);
|
||||
_print_sub("SATCOM", status, subsystem_info_s::SUBSYSTEM_TYPE_SATCOM);
|
||||
_print_sub("PREARM", status, subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK);
|
||||
_print_sub("OBSAVD", status, subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE);
|
||||
#endif
|
||||
}
|
||||
@ -0,0 +1,84 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 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 HealthFlags.h
|
||||
*
|
||||
* Contains helper functions to efficiently set the system health flags from commander and preflight check.
|
||||
*
|
||||
* @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
struct subsystem_info_s {
|
||||
// keep in sync with mavlink MAV_SYS_STATUS_SENSOR
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_GYRO = 1 << 0;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_ACC = 1 << 1;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_MAG = 1 << 2;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_ABSPRESSURE = 1 << 3;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_DIFFPRESSURE = 1 << 4;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_GPS = 1 << 5;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_OPTICALFLOW = 1 << 6;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_CVPOSITION = 1 << 7;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_LASERPOSITION = 1 << 8;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 1 << 9;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1 << 10;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 1 << 11;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_YAWPOSITION = 1 << 12;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_ALTITUDECONTROL = 1 << 13;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_POSITIONCONTROL = 1 << 14;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_MOTORCONTROL = 1 << 15;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_RCRECEIVER = 1 << 16;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_GYRO2 = 1 << 17;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_ACC2 = 1 << 18;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_MAG2 = 1 << 19;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_GEOFENCE = 1 << 20;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_AHRS = 1 << 21;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_TERRAIN = 1 << 22;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_REVERSEMOTOR = 1 << 23;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_LOGGING = 1 << 24;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_SENSORBATTERY = 1 << 25;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_SENSORPROXIMITY = 1 << 26;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_SATCOM = 1 << 27;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_PREARM_CHECK = 1 << 28;
|
||||
static constexpr uint64_t SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 1 << 29;
|
||||
};
|
||||
|
||||
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status);
|
||||
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status);
|
||||
void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status);
|
||||
void print_health_flags(const vehicle_status_s &status);
|
||||
@ -0,0 +1,54 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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_library(PreFlightCheck
|
||||
PreFlightCheck.cpp
|
||||
checks/preArmCheck.cpp
|
||||
checks/magnetometerCheck.cpp
|
||||
checks/magConsistencyCheck.cpp
|
||||
checks/accelerometerCheck.cpp
|
||||
checks/gyroCheck.cpp
|
||||
checks/baroCheck.cpp
|
||||
checks/imuConsistencyCheck.cpp
|
||||
checks/airspeedCheck.cpp
|
||||
checks/rcCalibrationCheck.cpp
|
||||
checks/powerCheck.cpp
|
||||
checks/airframeCheck.cpp
|
||||
checks/ekf2Check.cpp
|
||||
checks/failureDetectorCheck.cpp
|
||||
checks/manualControlCheck.cpp
|
||||
checks/cpuResourceCheck.cpp
|
||||
checks/sdcardCheck.cpp
|
||||
)
|
||||
target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags sensor_calibration)
|
||||
@ -0,0 +1,267 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019-2020 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 PreFlightCheck.cpp
|
||||
*/
|
||||
|
||||
#include "PreFlightCheck.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <HealthFlags.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr unsigned max_mandatory_gyro_count = 1;
|
||||
static constexpr unsigned max_optional_gyro_count = 4;
|
||||
static constexpr unsigned max_mandatory_accel_count = 1;
|
||||
static constexpr unsigned max_optional_accel_count = 4;
|
||||
static constexpr unsigned max_mandatory_mag_count = 1;
|
||||
static constexpr unsigned max_optional_mag_count = 4;
|
||||
static constexpr unsigned max_mandatory_baro_count = 1;
|
||||
static constexpr unsigned max_optional_baro_count = 4;
|
||||
|
||||
bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
||||
vehicle_status_flags_s &status_flags, bool report_failures, const bool prearm,
|
||||
const hrt_abstime &time_since_boot)
|
||||
{
|
||||
report_failures = (report_failures && status_flags.condition_system_hotplug_timeout
|
||||
&& !status_flags.condition_calibration_enabled);
|
||||
|
||||
bool failed = false;
|
||||
|
||||
failed = failed || !airframeCheck(mavlink_log_pub, status);
|
||||
failed = failed || !sdcardCheck(mavlink_log_pub, status_flags.sd_card_detected_once, report_failures);
|
||||
|
||||
/* ---- MAG ---- */
|
||||
{
|
||||
int32_t sys_has_mag = 1;
|
||||
param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
|
||||
|
||||
if (sys_has_mag == 1) {
|
||||
|
||||
/* check all sensors individually, but fail only for mandatory ones */
|
||||
for (unsigned i = 0; i < max_optional_mag_count; i++) {
|
||||
const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
|
||||
bool report_fail = report_failures;
|
||||
|
||||
int32_t device_id = -1;
|
||||
|
||||
if (!magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
|
||||
if (required) {
|
||||
failed = true;
|
||||
}
|
||||
|
||||
report_fail = false; // only report the first failure
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: highest priority mag
|
||||
|
||||
/* mag consistency checks (need to be performed after the individual checks) */
|
||||
if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) {
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- ACCEL ---- */
|
||||
{
|
||||
/* check all sensors individually, but fail only for mandatory ones */
|
||||
for (unsigned i = 0; i < max_optional_accel_count; i++) {
|
||||
const bool required = (i < max_mandatory_accel_count);
|
||||
bool report_fail = report_failures;
|
||||
|
||||
int32_t device_id = -1;
|
||||
|
||||
if (!accelerometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
|
||||
if (required) {
|
||||
failed = true;
|
||||
}
|
||||
|
||||
report_fail = false; // only report the first failure
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: highest priority (from params)
|
||||
}
|
||||
|
||||
/* ---- GYRO ---- */
|
||||
{
|
||||
/* check all sensors individually, but fail only for mandatory ones */
|
||||
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
|
||||
const bool required = (i < max_mandatory_gyro_count);
|
||||
bool report_fail = report_failures;
|
||||
|
||||
int32_t device_id = -1;
|
||||
|
||||
if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
|
||||
if (required) {
|
||||
failed = true;
|
||||
}
|
||||
|
||||
report_fail = false; // only report the first failure
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: highest priority (from params)
|
||||
}
|
||||
|
||||
/* ---- BARO ---- */
|
||||
{
|
||||
int32_t sys_has_baro = 1;
|
||||
param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
|
||||
|
||||
bool baro_fail_reported = false;
|
||||
|
||||
/* check all sensors, but fail only for mandatory ones */
|
||||
for (unsigned i = 0; i < max_optional_baro_count; i++) {
|
||||
const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1);
|
||||
bool report_fail = (required && report_failures && !baro_fail_reported);
|
||||
|
||||
int32_t device_id = -1;
|
||||
|
||||
if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
|
||||
if (required) {
|
||||
baro_fail_reported = true;
|
||||
}
|
||||
|
||||
report_fail = false; // only report the first failure
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- IMU CONSISTENCY ---- */
|
||||
// To be performed after the individual sensor checks have completed
|
||||
{
|
||||
if (!imuConsistencyCheck(mavlink_log_pub, status, report_failures)) {
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- AIRSPEED ---- */
|
||||
/* Perform airspeed check only if circuit breaker is not engaged and it's not a rotary wing */
|
||||
if (!status_flags.circuit_breaker_engaged_airspd_check &&
|
||||
(status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
|
||||
|
||||
int32_t airspeed_mode = 0;
|
||||
param_get(param_find("FW_ARSP_MODE"), &airspeed_mode);
|
||||
const bool optional = (airspeed_mode == 1);
|
||||
|
||||
int32_t max_airspeed_check_en = 0;
|
||||
param_get(param_find("COM_ARM_ARSP_EN"), &max_airspeed_check_en);
|
||||
|
||||
float airspeed_trim = 10.0f;
|
||||
param_get(param_find("FW_AIRSPD_TRIM"), &airspeed_trim);
|
||||
|
||||
const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed
|
||||
|
||||
if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm, (bool)max_airspeed_check_en,
|
||||
arming_max_airspeed_allowed)
|
||||
&& !(bool)optional) {
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- RC CALIBRATION ---- */
|
||||
if (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT) {
|
||||
if (rcCalibrationCheck(mavlink_log_pub, report_failures, status.is_vtol) != OK) {
|
||||
if (report_failures) {
|
||||
mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
|
||||
}
|
||||
|
||||
failed = true;
|
||||
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, status);
|
||||
status_flags.rc_calibration_valid = false;
|
||||
|
||||
} else {
|
||||
// The calibration is fine, but only set the overall health state to true if the signal is not currently lost
|
||||
status_flags.rc_calibration_valid = true;
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true,
|
||||
!status.rc_signal_lost, status);
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- SYSTEM POWER ---- */
|
||||
if (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check) {
|
||||
if (!powerCheck(mavlink_log_pub, status, report_failures, prearm)) {
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* ---- Navigation EKF ---- */
|
||||
// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
|
||||
int32_t estimator_type = -1;
|
||||
|
||||
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) {
|
||||
param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type);
|
||||
|
||||
} else {
|
||||
// EKF2 is currently the only supported option for FW & VTOL
|
||||
estimator_type = 2;
|
||||
}
|
||||
|
||||
if (estimator_type == 2) {
|
||||
|
||||
const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) &&
|
||||
ekf2CheckSensorBias(mavlink_log_pub, report_failures);
|
||||
|
||||
// For the first 10 seconds the ekf2 can be unhealthy, and we just mark it
|
||||
// as not present.
|
||||
// After that or if report_failures is true, we'll set the flags as is.
|
||||
|
||||
if (!ekf_healthy && time_since_boot < 10_s && !report_failures) {
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status);
|
||||
|
||||
} else {
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
|
||||
}
|
||||
|
||||
failed |= !ekf_healthy;
|
||||
}
|
||||
|
||||
/* ---- Failure Detector ---- */
|
||||
if (!failureDetectorCheck(mavlink_log_pub, status, report_failures, prearm)) {
|
||||
failed = true;
|
||||
}
|
||||
|
||||
failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
|
||||
failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);
|
||||
|
||||
/* Report status */
|
||||
return !failed;
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue