Rev 4 of the flight controller i guess

src/rov/rov_flight_controller/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.16)
 project(rov_flight_controller)
 
 add_compile_options(-g)
@@ -33,18 +33,9 @@ find_package(Eigen3 REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
-if(BUILD_DISPLAY)
-  find_package(SFML 2.5 REQUIRED COMPONENTS system window graphics)
-  find_package(Boost 1.71.0 REQUIRED)
-endif()
-
 add_library(flight_controller_lib STATIC src/flight_controller.cpp)
 add_executable(flight_controller src/main.cpp)
 
-if(BUILD_DISPLAY)
-  add_executable(flight_display src/flight_display.cpp)
-endif()
-
 target_include_directories(flight_controller_lib PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
@@ -56,30 +47,13 @@ ament_target_dependencies(
   "std_srvs"
   "sensor_msgs"
 )
-if(BUILD_DISPLAY)
-  target_include_directories(flight_display PUBLIC
-    ${Boost_INCLUDE_DIRS})
-endif()
 
 target_link_libraries(flight_controller_lib Eigen3::Eigen)
 target_link_libraries(flight_controller flight_controller_lib)
-if(BUILD_DISPLAY)
-  target_link_libraries(flight_display flight_controller_lib sfml-system sfml-window sfml-graphics yaml-cpp ${Boost_LIBRARIES})
-endif()
 
 install(TARGETS flight_controller flight_controller_lib
   DESTINATION lib/${PROJECT_NAME})
 
-if(BUILD_DISPLAY)
-  install(TARGETS flight_display
-  DESTINATION lib/${PROJECT_NAME})
-endif()
-
-if(BUILD_DISPLAY)
-  install(TARGETS flight_display
-    DESTINATION lib/${PROJECT_NAME})
-endif()
-
 install(DIRECTORY launch/
   DESTINATION share/${PROJECT_NAME})
 

src/rov/rov_flight_controller/config/params.yaml

@@ -2,6 +2,10 @@ flight_controller:
   ros__parameters:
     Debug: False
     Use_PCA9685: True
+    # TODO: TUNE
+    kpd: 1.0
+    kppq: 1.0
+    kdpq: 0.0
     Thruster0:
       Position: [-1.0, 0.5, 0.0]
       Thrust: [0.0, 0.0, 1.0]

src/rov/rov_flight_controller/include/flight_controller/flight_controller.hpp → src/rov/rov_flight_controller/include/rov_flight_controller/flight_controller.hpp

@@ -10,6 +10,8 @@
 #include "Thruster.hpp"
 #include "rov_interfaces/msg/bno055_data.hpp"
 #include "rov_interfaces/msg/thruster_setpoints.hpp"
+#include "rov_interfaces/msg/bar30_data.hpp"
+#include "rov_interfaces/msg/tsys01_data.hpp"
 #include "rov_interfaces/msg/pwm.hpp"
 
 #include "rov_interfaces/srv/create_continuous_servo.hpp"
@@ -27,39 +29,64 @@ class FlightController : public rclcpp::Node {
     void toggle_PID(const std_srvs::srv::Empty_Request::SharedPtr request, std_srvs::srv::Empty_Response::SharedPtr response);
     void setpoint_callback(const rov_interfaces::msg::ThrusterSetpoints::SharedPtr setpoints);
     void bno_callback(const rov_interfaces::msg::BNO055Data::SharedPtr bno_data);
+    void bar_callback(const rov_interfaces::msg::Bar30Data::SharedPtr bar_data);
+    void tsys_callback(const rov_interfaces::msg::TSYS01Data::SharedPtr tsys_data);
     void updateNone();
     void updateSimple();
-    void updatePID();
+    void updatePID() {}; // disabled for now
     void clampthrottles(Eigen::Matrix<double,NUM_THRUSTERS,1>* throttles);
 
+    void safe(const std_srvs::srv::Empty_Request::SharedPtr request, std_srvs::srv::Empty_Response::SharedPtr response);
+    void desafe(const std_srvs::srv::Empty_Request::SharedPtr request, std_srvs::srv::Empty_Response::SharedPtr response);
+
+    void updateDepth(Eigen::Matrix<double, 6, 1>& desired_forces_torques);
+    void depthTare(const std_srvs::srv::Empty_Request::SharedPtr request, std_srvs::srv::Empty_Response::SharedPtr response);
+
+    void updatePitchRoll(Eigen::Matrix<double, 6, 1>& desired_forces_torques);
+    void updateStab();
+
     rclcpp::Subscription<rov_interfaces::msg::ThrusterSetpoints>::SharedPtr thruster_setpoint_subscription;
     rclcpp::Subscription<rov_interfaces::msg::BNO055Data>::SharedPtr bno_data_subscription;
+    rclcpp::Subscription<rov_interfaces::msg::TSYS01Data>::SharedPtr tsys_data_subscription;
+    rclcpp::Subscription<rov_interfaces::msg::Bar30Data>::SharedPtr bar_data_subscription;
     rclcpp::Publisher<rov_interfaces::msg::PWM>::SharedPtr pwm_publisher;
     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr toggle_PID_service;
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr depth_tare_service;
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr safe_service;
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr desafe_service;
     rclcpp::CallbackGroup::SharedPtr pca9685_registration_callbackgroup;
     rclcpp::Client<rov_interfaces::srv::CreateContinuousServo>::SharedPtr pca9685_client;
     std::array<rclcpp::Client<rov_interfaces::srv::CreateContinuousServo>::SharedFutureWithRequest, NUM_THRUSTERS> pca9685_requests;
 
-    std::function<void(void)> _update = std::bind(&FlightController::updateSimple, this);
-    std::function<void(void)> _update2 = std::bind(&FlightController::updatePID, this);
-
-    rclcpp::TimerBase::SharedPtr pid_control_loop;
+    std::function<void(void)> _update_simple = std::bind(&FlightController::updateSimple, this);
+    std::function<void(void)> _update_pid = std::bind(&FlightController::updatePID, this);
+    std::function<void(void)> _update_stab = std::bind(&FlightController::updateStab, this);
+    rclcpp::TimerBase::SharedPtr control_loop;
 
     std::chrono::time_point<std::chrono::high_resolution_clock> last_updated;
 
     std::mutex stall_mutex;
 
-    rov_interfaces::msg::BNO055Data bno_data;
+    rov_interfaces::msg::BNO055Data::SharedPtr bno_data;
     std::mutex bno_mutex;
+    rov_interfaces::msg::TSYS01Data::SharedPtr tsys_data;
+    std::mutex tsys_mutex;
+    rov_interfaces::msg::Bar30Data::SharedPtr bar30_data;
+    std::mutex bar30_mutex;
+
     Eigen::Vector3d translation_setpoints = Eigen::Vector3d(0,0,0);
     Eigen::Vector3d attitude_setpoints = Eigen::Vector3d(0,0,0);
+
+    float tare_depth;
+    float depth_setpoint;
+    bool keep_depth = false;
+    bool free_orientation = false;
+
     std::mutex setpoint_mutex;
     Eigen::Quaterniond quaternion_reference;
     Eigen::Vector3d linear_accel_last = Eigen::Vector3d(0,0,0);
     Eigen::Vector3d linear_integral = Eigen::Vector3d(0,0,0);
     Eigen::Vector3d linear_velocity = Eigen::Vector3d(0,0,0);
-    Eigen::Vector3d linear_velocity_err = Eigen::Vector3d(0,0,0);
-    Eigen::Vector3d linear_velocity_err_last = Eigen::Vector3d(0,0,0);
 
     std::chrono::time_point<std::chrono::high_resolution_clock> startUpdateLoopTime;