add dronecan sim interface, incremental improvements in uav dynamics,…

… alpha release v4

.gitmodules

@@ -31,3 +31,6 @@
 [submodule "src/mavlink_c_library_v2/mavlink"]
 	path = src/mavlink_c_library_v2/mavlink
 	url = https://github.com/mavlink/c_library_v2.git
+[submodule "src/DronecanInterface"]
+	path = src/DronecanInterface
+	url = git@github.com:PonomarevDA/dronecan_sim_interface.git

configs/px4/dronecan.yaml

@@ -15,15 +15,15 @@ UAVCAN_PUB_ARM:      1
 # bus=ID%8, node_id=((ID/256)%256), devtype=ID/65536
 
 # 2.1. Gyro
-CAL_GYRO0_ID:     8793603  # node_id=46
+CAL_GYRO0_ID:     8792579  # node_id=42
 CAL_GYRO0_PRIO:   50
 CAL_GYRO0_ROT:    0
 CAL_GYRO0_XOFF:   -0.001928219571709633
 CAL_GYRO0_YOFF:   0.001404458540491760
 CAL_GYRO0_ZOFF:   -0.000455024652183056
 
 # 2.2. Accelerometer
-CAL_ACC0_ID:      8400387  # node_id=46
+CAL_ACC0_ID:      8399363  # node_id=42
 CAL_ACC0_PRIO:    50
 CAL_ACC0_ROT:     0
 

configs/px4/v1.14/standard_vtol/airframe.yaml

@@ -20,6 +20,4 @@ FW_P_RMAX_NEG: 30.0       # Default 60.0
 FW_P_RMAX_POS: 30.0       # Default 60.0
 FW_PR_P: 0.12             # Default 0.08
 
-MC_YAWRATE_P: 1.0         # Yaw rate P gain. Default 0.2. Max 0.6.
-
 VT_B_TRANS_RAMP: 10.0     # Back transition MC motor ramp up time. Default 3.0 sec.

src/Simulator/CMakeLists.txt

@@ -2,11 +2,14 @@
 # Distributed under the terms of the GPL v3 license, available in the file LICENSE.
 
 cmake_minimum_required(VERSION 3.22)
-project(UavDynamics VERSION 1.0 LANGUAGES CXX)
+project(UavDynamics VERSION 1.0 LANGUAGES CXX C)
 set(CMAKE_CXX_STANDARD 17)
 find_package(Eigen3 REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
+include(../DronecanInterface/dronecan_interface.cmake)
+add_definitions(-DLIBPARAMS_PARAMS_DIR="${CMAKE_CURRENT_BINARY_DIR}")
+
 include_directories(
     ${EIGEN3_INCLUDE_DIRS}
     ../UavDynamics/include
@@ -16,12 +19,16 @@ include_directories(
     ../websocketpp
     ../nlohmann_json/include
     ../ArdupilotInterface/include
+    ${DRONECAN_INTERFACE_HEADERS}
 )
+
 add_executable(UavDynamics
     src/main.cpp
     ../MavlinkSimInterface/src/mavlink_sim_interface.cpp
+    ${DRONECAN_INTERFACE_SOURCES}
 )
 
+
 target_link_libraries(UavDynamics PRIVATE
     yaml-cpp
 )

src/Simulator/src/main.cpp

@@ -19,6 +19,7 @@
 #include "MavlinkSimInterface/mavlink_sim_interface.hpp"
 #include "ArdupilotInterface/ardupilot_json_interface.hpp"
 #include "RosInterface/ros_interface.hpp"
+#include "DronecanInterface/dronecan_interface.hpp"
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -86,7 +87,7 @@ void threadSim() {
     }
 }
 
-void communicate() {
+void px4MavlinkCommunicate() {
     float temperatureKelvin;
     float staticPressureHpa;
     float diffPressureHPa;
@@ -107,13 +108,13 @@ void communicate() {
     );
 }
 
-void threadFlightStack() {
+void threadPX4MavlinkFlightStack() {
     const auto period = std::chrono::microseconds(1000000 / 500); // 500 Hz
 
     while (true) {
         auto start_time = std::chrono::high_resolution_clock::now();
 
-        communicate();
+        px4MavlinkCommunicate();
 
         auto end_time = std::chrono::high_resolution_clock::now();
         std::this_thread::sleep_for(period - (end_time - start_time));
@@ -148,14 +149,17 @@ int main(int argc, char* argv[]) {
     std::thread thread1 = std::thread(&threadSim);
 
     std::cout << "Running the flight stack communicator..." << std::endl;
-    std::thread thread2 = std::thread(&threadFlightStack);
+    std::thread thread2 = std::thread(&threadPX4MavlinkFlightStack);
 
-    std::cout << "Running RVIZ communicator..." << std::endl;
+    std::cout << "Running ROS communicator..." << std::endl;
     std::thread thread3 = std::thread(&threadRos);
 
     std::cout << "Running ArduPilot JSON communicator..." << std::endl;
     std::thread thread4 = std::thread(&threadArdupilotJson);
 
+    std::cout << "Running DroneCAN interface..." << std::endl;
+    DronecanInterface::runInSeparateThread(sim, &setpoints);
+
     thread1.join();
     thread2.join();