From 3c1227934c272fdc36ec3defe4cc6b6f0d11732e Mon Sep 17 00:00:00 2001 From: Zac Stanton Date: Fri, 2 May 2025 13:33:13 -0600 Subject: [PATCH] remove old flight display. update to use new control loop functions. implements experimental pid controller with simple mode for fallback. many different services available to configure the node live --- src/rov/rov_flight_controller/CMakeLists.txt | 28 +- .../rov_flight_controller/config/params.yaml | 4 + .../Thruster.hpp | 0 .../flight_controller.hpp | 43 ++- .../src/flight_controller.cpp | 327 ++++++++++++------ .../src/flight_display.cpp | 120 ------- src/rov/rov_flight_controller/src/main.cpp | 2 +- .../rov_interfaces/msg/ThrusterSetpoints.msg | 4 +- 8 files changed, 265 insertions(+), 263 deletions(-) rename src/rov/rov_flight_controller/include/{flight_controller => rov_flight_controller}/Thruster.hpp (100%) rename src/rov/rov_flight_controller/include/{flight_controller => rov_flight_controller}/flight_controller.hpp (58%) delete mode 100644 src/rov/rov_flight_controller/src/flight_display.cpp diff --git a/src/rov/rov_flight_controller/CMakeLists.txt b/src/rov/rov_flight_controller/CMakeLists.txt index e6edbe6..c6662aa 100644 --- a/src/rov/rov_flight_controller/CMakeLists.txt +++ b/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 $ $) @@ -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}) diff --git a/src/rov/rov_flight_controller/config/params.yaml b/src/rov/rov_flight_controller/config/params.yaml index 3e7b103..cb8a1ad 100644 --- a/src/rov/rov_flight_controller/config/params.yaml +++ b/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] diff --git a/src/rov/rov_flight_controller/include/flight_controller/Thruster.hpp b/src/rov/rov_flight_controller/include/rov_flight_controller/Thruster.hpp similarity index 100% rename from src/rov/rov_flight_controller/include/flight_controller/Thruster.hpp rename to src/rov/rov_flight_controller/include/rov_flight_controller/Thruster.hpp diff --git a/src/rov/rov_flight_controller/include/flight_controller/flight_controller.hpp b/src/rov/rov_flight_controller/include/rov_flight_controller/flight_controller.hpp similarity index 58% rename from src/rov/rov_flight_controller/include/flight_controller/flight_controller.hpp rename to src/rov/rov_flight_controller/include/rov_flight_controller/flight_controller.hpp index 3ce18cf..9ee490f 100644 --- a/src/rov/rov_flight_controller/include/flight_controller/flight_controller.hpp +++ b/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* 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& desired_forces_torques); + void depthTare(const std_srvs::srv::Empty_Request::SharedPtr request, std_srvs::srv::Empty_Response::SharedPtr response); + + void updatePitchRoll(Eigen::Matrix& desired_forces_torques); + void updateStab(); + rclcpp::Subscription::SharedPtr thruster_setpoint_subscription; rclcpp::Subscription::SharedPtr bno_data_subscription; + rclcpp::Subscription::SharedPtr tsys_data_subscription; + rclcpp::Subscription::SharedPtr bar_data_subscription; rclcpp::Publisher::SharedPtr pwm_publisher; rclcpp::Service::SharedPtr toggle_PID_service; + rclcpp::Service::SharedPtr depth_tare_service; + rclcpp::Service::SharedPtr safe_service; + rclcpp::Service::SharedPtr desafe_service; rclcpp::CallbackGroup::SharedPtr pca9685_registration_callbackgroup; rclcpp::Client::SharedPtr pca9685_client; std::array::SharedFutureWithRequest, NUM_THRUSTERS> pca9685_requests; - std::function _update = std::bind(&FlightController::updateSimple, this); - std::function _update2 = std::bind(&FlightController::updatePID, this); - - rclcpp::TimerBase::SharedPtr pid_control_loop; + std::function _update_simple = std::bind(&FlightController::updateSimple, this); + std::function _update_pid = std::bind(&FlightController::updatePID, this); + std::function _update_stab = std::bind(&FlightController::updateStab, this); + rclcpp::TimerBase::SharedPtr control_loop; std::chrono::time_point 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 startUpdateLoopTime; diff --git a/src/rov/rov_flight_controller/src/flight_controller.cpp b/src/rov/rov_flight_controller/src/flight_controller.cpp index c644ba5..3d33f0d 100644 --- a/src/rov/rov_flight_controller/src/flight_controller.cpp +++ b/src/rov/rov_flight_controller/src/flight_controller.cpp @@ -1,4 +1,4 @@ -#include "flight_controller/flight_controller.hpp" +#include "rov_flight_controller/flight_controller.hpp" #include #include @@ -40,13 +40,9 @@ void declare_params(rclcpp::Node* node) { // declare gain constant parameters // TODO: tune these constants - node->declare_parameter("Pq", 1.0f); - node->declare_parameter("Pw", 1.0f); - node->declare_parameter("kp", 1.0f); - node->declare_parameter("ki", 1.0f); - node->declare_parameter("kd", 1.0f); - node->declare_parameter("pc_1", 1.0f); - node->declare_parameter("pc_2", 1.0f); + node->declare_parameter("kpd", 1.0f); + node->declare_parameter("kppq", 1.0f); + node->declare_parameter("kdpq", 0.0f); } } @@ -108,6 +104,10 @@ FlightController::FlightController() : Node(std::string("flight_controller")) { thruster_setpoint_subscription = this->create_subscription("thruster_setpoints", 10, std::bind(&FlightController::setpoint_callback, this, std::placeholders::_1)); // receives orientation and acceleration data from bno055 bno_data_subscription = this->create_subscription("bno055_data", rclcpp::SensorDataQoS(), std::bind(&FlightController::bno_callback, this, std::placeholders::_1)); + // depth data from bar30 + bar_data_subscription = this->create_subscription("bar30_data", rclcpp::SensorDataQoS(), std::bind(&FlightController::bar_callback, this, std::placeholders::_1)); + // temperature data from tsys01 + tsys_data_subscription = this->create_subscription("tsys01_data", rclcpp::SensorDataQoS(), std::bind(&FlightController::tsys_callback, this, std::placeholders::_1)); // publishes PWM commands to PCA9685 pwm_publisher = this->create_publisher("pwm", 10); @@ -115,11 +115,17 @@ FlightController::FlightController() : Node(std::string("flight_controller")) { // about 60 hz update rate // TODO: Check that service changes the timer callback this->startUpdateLoopTime = std::chrono::high_resolution_clock::now() + std::chrono::seconds(5); - pid_control_loop = this->create_wall_timer(std::chrono::milliseconds(UPDATE_MS), std::bind(&FlightController::updateNone, this)); + control_loop = this->create_wall_timer(std::chrono::milliseconds(UPDATE_MS), std::bind(&FlightController::updateNone, this)); // Creates service responsible for toggling between updateSimple and updatePID toggle_PID_service = this->create_service("toggle_pid", std::bind(&FlightController::toggle_PID, this, std::placeholders::_1, std::placeholders::_2)); + depth_tare_service = this->create_service("tare_depth", + std::bind(&FlightController::depthTare, this, std::placeholders::_1, std::placeholders::_2)); + safe_service = this->create_service("safe", + std::bind(&FlightController::depthTare, this, std::placeholders::_1, std::placeholders::_2)); + desafe_service = this->create_service("desafe", + std::bind(&FlightController::depthTare, this, std::placeholders::_1, std::placeholders::_2)); } void FlightController::registerThrusters() { @@ -158,31 +164,61 @@ void FlightController::toggle_PID(const std_srvs::srv::Empty_Request::SharedPtr std_srvs::srv::Empty_Response::SharedPtr response) { (void) request; (void) response; - std::swap(_update, _update2); this->last_updated = std::chrono::high_resolution_clock::now(); - pid_control_loop = this->create_wall_timer(std::chrono::milliseconds(UPDATE_MS), FlightController::_update); + control_loop = this->create_wall_timer(std::chrono::milliseconds(UPDATE_MS), FlightController::_update_stab); } +// TODO: update to new mapping void FlightController::setpoint_callback(const rov_interfaces::msg::ThrusterSetpoints::SharedPtr setpoints) { - std::lock_guard(this->setpoint_mutex); - std::lock_guard(this->stall_mutex); + const std::lock_guard l1(this->setpoint_mutex); + const std::lock_guard l2(this->stall_mutex); translation_setpoints(0,0) = setpoints->vx; // [-1,1] translation_setpoints(1,0) = setpoints->vy; // ^ translation_setpoints(2,0) = setpoints->vz; // ^ attitude_setpoints(0,0) = setpoints->omegax; // [-1,1] attitude_setpoints(1,0) = setpoints->omegay; // ^ attitude_setpoints(2,0) = setpoints->omegaz; // ^ + keep_depth = setpoints->keep_depth; + free_orientation = setpoints->free_orientation; } void FlightController::bno_callback(const rov_interfaces::msg::BNO055Data::SharedPtr bno_data) { - std::lock_guard(this->bno_mutex); - this->bno_data = *bno_data.get(); + const std::lock_guard l1(this->bno_mutex); + this->bno_data = bno_data; +} + +void FlightController::bar_callback(const rov_interfaces::msg::Bar30Data::SharedPtr bar_data) { + const std::lock_guard l1(this->bar30_mutex); + this->bar30_data = bar_data; +} + +void FlightController::tsys_callback(const rov_interfaces::msg::TSYS01Data::SharedPtr tsys_data) { + const std::lock_guard l1(this->tsys_mutex); + this->tsys_data = tsys_data; } void FlightController::updateNone() { + // wait 5 seconds before starting if(std::chrono::high_resolution_clock::now() >= this->startUpdateLoopTime) { - pid_control_loop = this->create_wall_timer(std::chrono::milliseconds(UPDATE_MS), FlightController::_update); + control_loop = this->create_wall_timer(std::chrono::milliseconds(UPDATE_MS), FlightController::_update_simple); } + + // safe the thrusters by turning them to 0 pwm + rov_interfaces::msg::PWM msg; + msg.angle_or_throttle = 0; + + for(int i=0; i < NUM_THRUSTERS; i++) { + msg.channel = i; + this->pwm_publisher->publish(msg); + } +} +void FlightController::safe(const std_srvs::srv::Empty_Request::SharedPtr request, std_srvs::srv::Empty_Response::SharedPtr response) { + (void) request; + (void) response; + // change the update loop to do nothing + control_loop = this->create_wall_timer(std::chrono::milliseconds(UPDATE_MS), [](){}); + + // safe the thrusters by turning them to 0 pwm rov_interfaces::msg::PWM msg; msg.angle_or_throttle = 0; @@ -192,6 +228,13 @@ void FlightController::updateNone() { } } +void FlightController::desafe(const std_srvs::srv::Empty_Request::SharedPtr request, std_srvs::srv::Empty_Response::SharedPtr response) { + (void) request; + (void) response; + // restart the update loop to be update_simple + control_loop = this->create_wall_timer(std::chrono::milliseconds(UPDATE_MS), FlightController::_update_simple); +} + void FlightController::updateSimple() { // convert setpoints to actuations that the controller must apply Eigen::Matrix unnormalized_actuations = Eigen::Matrix::Zero(); @@ -251,92 +294,176 @@ void FlightController::updateSimple() { } } -void FlightController::updatePID() { - Eigen::Vector3d desired_force; - Eigen::Vector3d desired_torque; - - // update dt - auto now = std::chrono::high_resolution_clock::now(); - int dt_ms = std::chrono::duration_cast(now - this->last_updated).count(); - this->last_updated = now; - - // fill the matrixes based on bno and setpoint data - this->setpoint_mutex.lock(); - this->bno_mutex.lock(); - Eigen::Vector3d linear_accel(bno_data.imu.linear_acceleration.x, bno_data.imu.linear_acceleration.y,bno_data.imu.linear_acceleration.z); // m/s^2 - linear_velocity += linear_accel * dt_ms / 1000; // get an approximation of linear velocity - linear_velocity_err_last = linear_velocity_err; - linear_velocity_err[0] = translation_setpoints(0,0) - linear_velocity[0]; - linear_velocity_err[1] = translation_setpoints(1,0) - linear_velocity[1]; - linear_velocity_err[2] = translation_setpoints(2,0) - linear_velocity[2]; - Eigen::Vector3d ha = dt_ms * 0.5 * attitude_setpoints; // vector of half angle - // Gyroscope vector from the sensor is stored in angular_velcoity, but shouldn't it be "angular_acceleration"? - Eigen::Vector3d omega = Eigen::Vector3d(bno_data.imu.angular_velocity.x, bno_data.imu.angular_velocity.y, bno_data.imu.angular_velocity.z); - auto quaternion_measured = Eigen::Quaterniond(bno_data.imu.orientation.w, bno_data.imu.orientation.x, bno_data.imu.orientation.y, bno_data.imu.orientation.z); - this->setpoint_mutex.unlock(); - this->bno_mutex.unlock(); - - // update desired force - // Linear, discrete-time PID controller - // lambdas for linear PID loop - auto PID_params = this->get_parameters(std::vector{"kp", "ki", "kd"}); - auto P = [](rclcpp::Parameter kp, Eigen::Vector3d& linear_velocity_err)->Eigen::Vector3d { - return kp.as_double() * linear_velocity_err; - }; - // TODO: anti-windup - auto I = [this](rclcpp::Parameter ki, Eigen::Vector3d& linear_velocity_err, int& dt_ms)->Eigen::Vector3d { - this->linear_integral += ki.as_double() * linear_velocity_err * dt_ms / 1000; - return this->linear_integral; - }; - auto D = [](rclcpp::Parameter kd, Eigen::Vector3d& linear_velocity_err, Eigen::Vector3d& linear_velocity_err_last, int& dt_ms)->Eigen::Vector3d { - return kd.as_double() * (linear_velocity_err - linear_velocity_err_last) / (static_cast(dt_ms) / 1000); - }; +// void FlightController::updatePID() { +// Eigen::Vector3d desired_force; +// Eigen::Vector3d desired_torque; + +// // update dt +// auto now = std::chrono::high_resolution_clock::now(); +// int dt_ms = std::chrono::duration_cast(now - this->last_updated).count(); +// this->last_updated = now; + +// // fill the matrixes based on bno and setpoint data +// this->setpoint_mutex.lock(); +// this->bno_mutex.lock(); +// Eigen::Vector3d linear_accel(bno_data->imu.linear_acceleration.x, bno_data->imu.linear_acceleration.y,bno_data->imu.linear_acceleration.z); // m/s^2 +// linear_velocity += linear_accel * dt_ms / 1000; // get an approximation of linear velocity +// Eigen::Vector3d ha = dt_ms * 0.5 * attitude_setpoints; // vector of half angle + +// Eigen::Vector3d omega = Eigen::Vector3d(bno_data->imu.angular_velocity.x, bno_data->imu.angular_velocity.y, bno_data->imu.angular_velocity.z); +// auto quaternion_measured = Eigen::Quaterniond(bno_data->imu.orientation.w, bno_data->imu.orientation.x, bno_data->imu.orientation.y, bno_data->imu.orientation.z); +// this->setpoint_mutex.unlock(); +// this->bno_mutex.unlock(); + +// // update desired force +// // Linear, discrete-time PID controller +// // lambdas for linear PID loop +// auto PID_params = this->get_parameters(std::vector{"kp", "ki", "kd"}); +// auto P = [](rclcpp::Parameter kp, Eigen::Vector3d& linear_velocity_err)->Eigen::Vector3d { +// return kp.as_double() * linear_velocity_err; +// }; +// // TODO: anti-windup +// auto I = [this](rclcpp::Parameter ki, Eigen::Vector3d& linear_velocity_err, int& dt_ms)->Eigen::Vector3d { +// this->linear_integral += ki.as_double() * linear_velocity_err * dt_ms / 1000; +// return this->linear_integral; +// }; +// auto D = [](rclcpp::Parameter kd, Eigen::Vector3d& linear_velocity_err, Eigen::Vector3d& linear_velocity_err_last, int& dt_ms)->Eigen::Vector3d { +// return kd.as_double() * (linear_velocity_err - linear_velocity_err_last) / (static_cast(dt_ms) / 1000); +// }; + +// // P + I + D +// desired_force = P(PID_params.at(0),linear_velocity_err) +// + I(PID_params.at(1), linear_velocity_err, dt_ms) +// + D(PID_params.at(2), linear_velocity_err, linear_velocity_err_last, dt_ms); + +// // update desired torque +// // TODO: look at this if performance needs to be improved (loses accuracy tho) +// // see https://stackoverflow.com/questions/24197182/efficient-quaternion-angular-velocity/24201879#24201879 +// double l = ha.norm(); // magnitude +// if (l > 0) { +// ha *= sin(l) / l; +// quaternion_reference = Eigen::Quaterniond(cos(l), ha.x(), ha.y(), ha.z()); +// } else { +// quaternion_reference = Eigen::Quaterniond(1.0, ha.x(), ha.y(), ha.z()); +// } +// // desired orientation is only known by the pilot, quaternion reference defines a reference from a unity quaternion (1,0,0,0) +// // to get actual reference, this must be multiplied with the measured quaternion to get desired orientation +// // TODO: ensure that when w = 0, this is close to, or equal to unity +// quaternion_reference = quaternion_reference * quaternion_measured; + +// // Non Linear P^2 Quaternion based control scheme +// // For derivation of controller see: http://www.diva-portal.org/smash/get/diva2:1010947/FULLTEXT01.pdf +// // For derivation of dynamics and control allocation see: https://flex.flinders.edu.au/file/27aa0064-9de2-441c-8a17-655405d5fc2e/1/ThesisWu2018.pdf +// auto q_err = quaternion_reference * quaternion_measured.conjugate(); // hamilton product (hopefully) +// Eigen::Vector3d axis_err; + +// if(q_err.w() < 0) { // this could be simplified to a negation but eh +// axis_err = q_err.conjugate().vec(); +// } else { +// axis_err = q_err.vec(); +// } + +// auto P2_params = this->get_parameters(std::vector{"Pq", "Pw"}); +// desired_torque = (-P2_params.at(0).as_double() * axis_err) - (P2_params.at(1).as_double() * omega); + +// // control allocation +// // u = K^-1 * T^+ * t +// // 8x1 = 8x8 * 8x6 * 6x1 :) +// Eigen::Matrix forcesAndTorques; +// forcesAndTorques << desired_force, desired_torque; + +// //TODO:test +// // solve Ax = b and normalize thrust such that it satisfies MIN_THRUST_VALUE <= throttles[j] <= MAX_THRUST_VALUE +// // while scaling thrusters to account for large thrust demands on a single thruster +// Eigen::Matrix actuations = this->thruster_coefficient_matrix_times_geometry * forcesAndTorques; +// clampthrottles(&actuations); + +// // publish PWM values +// // TODO: consider refactoring this to have 8 separate publishers? +// // publishers would be created upon successful return from registerThrusters() if Use_PCA9685 == true +// // else defaults would be created from params.yaml +// // this method would allow for rqt_topic to more accurately collect PWM values +// for(int i = 0; i < NUM_THRUSTERS; i++) { +// rov_interfaces::msg::PWM msg; +// msg.angle_or_throttle = static_cast(actuations(i,0)); // this is a source of noise in output signals, may cause system instability?? +// msg.channel = thruster_index_to_PWM_pin.at(i); +// pwm_publisher->publish(msg); +// #if DEBUG_OUTPUT +// RCLCPP_DEBUG(this->get_logger(), "PIN:%i THROTTLE:%f", thruster_index_to_PWM_pin.at(i), actuations(i,0)); +// #endif +// } +// } - // P + I + D - desired_force = P(PID_params.at(0),linear_velocity_err) - + I(PID_params.at(1), linear_velocity_err, dt_ms) - + D(PID_params.at(2), linear_velocity_err, linear_velocity_err_last, dt_ms); - - // update desired torque - // TODO: look at this if performance needs to be improved (loses accuracy tho) - // see https://stackoverflow.com/questions/24197182/efficient-quaternion-angular-velocity/24201879#24201879 - double l = ha.norm(); // magnitude - if (l > 0) { - ha *= sin(l) / l; - quaternion_reference = Eigen::Quaterniond(cos(l), ha.x(), ha.y(), ha.z()); - } else { - quaternion_reference = Eigen::Quaterniond(1.0, ha.x(), ha.y(), ha.z()); - } - // desired orientation is only known by the pilot, quaternion reference defines a reference from a unity quaternion (1,0,0,0) - // to get actual reference, this must be multiplied with the measured quaternion to get desired orientation - // TODO: ensure that when w = 0, this is close to, or equal to unity - quaternion_reference = quaternion_reference * quaternion_measured; - - // Non Linear P^2 Quaternion based control scheme - // For derivation of controller see: http://www.diva-portal.org/smash/get/diva2:1010947/FULLTEXT01.pdf - // For derivation of dynamics and control allocation see: https://flex.flinders.edu.au/file/27aa0064-9de2-441c-8a17-655405d5fc2e/1/ThesisWu2018.pdf - auto q_err = quaternion_reference * quaternion_measured.conjugate(); // hamilton product (hopefully) - Eigen::Vector3d axis_err; - - if(q_err.w() < 0) { // this could be simplified to a negation but eh - axis_err = q_err.conjugate().vec(); +void FlightController::clampthrottles(Eigen::Matrix* throttles) { + Eigen::Index loc; + throttles->minCoeff(&loc); + double ratioA = std::abs(-1.0 / std::min((*throttles)(loc), -1.0)); + throttles->maxCoeff(&loc); + double ratioB = std::abs(1.0 / std::max((*throttles)(loc), 1.0)); + if(ratioA < ratioB) { + *throttles = *throttles * ratioA; } else { - axis_err = q_err.vec(); + *throttles = *throttles * ratioB; } +} - auto P2_params = this->get_parameters(std::vector{"Pq", "Pw"}); - desired_torque = (-P2_params.at(0).as_double() * axis_err) - (P2_params.at(1).as_double() * omega); - // control allocation - // u = K^-1 * T^+ * t - // 8x1 = 8x8 * 8x6 * 6x1 :) - Eigen::Matrix forcesAndTorques; - forcesAndTorques << desired_force, desired_torque; +void FlightController::updateDepth(Eigen::Matrix& desired_forces_torques) { + const std::lock_guard l1(bar30_mutex); + const std::lock_guard l2(bno_mutex); + const Eigen::Vector3d sensor_offset(-0.36,0,0); + float depth = tare_depth - bar30_data->depth; + // correct for pitch + Eigen::Quaterniond quat(bno_data->imu.orientation.w, bno_data->imu.orientation.x, bno_data->imu.orientation.y, bno_data->imu.orientation.z); + Eigen::Vector3d grav(bno_data->gravity.i, bno_data->gravity.j, bno_data->gravity.k); + depth += (quat * sensor_offset).dot(grav); + float depth_error = depth_setpoint - depth; + float kp = static_cast(this->get_parameter("kpd").as_double()); + + // we want to stabilize towards no depth delta between setpoint and where we are + desired_forces_torques.segment<3>(0) += kp * depth_error * grav.normalized(); +} + +void FlightController::depthTare(const std_srvs::srv::Empty_Request::SharedPtr request, std_srvs::srv::Empty_Response::SharedPtr response) { + (void) request; + (void) response; + const std::lock_guard l1(bar30_mutex); + const std::lock_guard l2(bno_mutex); + const Eigen::Vector3d sensor_offset(-0.36,0,0); + // correct for pitch + Eigen::Quaterniond quat(bno_data->imu.orientation.w, bno_data->imu.orientation.x, bno_data->imu.orientation.y, bno_data->imu.orientation.z); + Eigen::Vector3d grav(bno_data->gravity.i, bno_data->gravity.j, bno_data->gravity.k); + tare_depth = bar30_data->depth - (quat * sensor_offset).dot(grav); +} + +void FlightController::updatePitchRoll(Eigen::Matrix& desired_forces_torques) { + const std::lock_guard l1(bno_mutex); + // remove impact from pitch and roll (!!!!!EULER WARNING!!!!!) + Eigen::AngleAxisd yaw_only(bno_data->euler.k, Eigen::Vector3d::UnitZ()); + Eigen::Quaterniond q_yaw_only(yaw_only); + Eigen::Quaterniond quat(bno_data->imu.orientation.w, bno_data->imu.orientation.x, bno_data->imu.orientation.y, bno_data->imu.orientation.z); + + Eigen::Quaterniond q_err = q_yaw_only * quat.conjugate(); + Eigen::Vector3d angular_velocity(bno_data->imu.angular_velocity.x, bno_data->imu.angular_velocity.y, bno_data->imu.angular_velocity.z); + desired_forces_torques.segment<3>(3) -= this->get_parameter("kppq").as_double() * q_err.vec() + this->get_parameter("kdpq").as_double() * angular_velocity; +} + +void FlightController::updateStab() { + Eigen::Matrix forces_and_torques; + + // will attempt to drive orientation to horizontal unless disabled + if (!free_orientation) { + updatePitchRoll(forces_and_torques); + } + // TODO: will be a button on the controller that you press to activate depth controller + if (keep_depth) { + updateDepth(forces_and_torques); + } //TODO:test - // solve Ax = b and normalize thrust such that it satisfies MIN_THRUST_VALUE <= throttles[j] <= MAX_THRUST_VALUE + // solve Ax = b with x = A^+ b and normalize thrust such that it satisfies MIN_THRUST_VALUE <= throttles[j] <= MAX_THRUST_VALUE // while scaling thrusters to account for large thrust demands on a single thruster - Eigen::Matrix actuations = this->thruster_coefficient_matrix_times_geometry * forcesAndTorques; + Eigen::Matrix actuations = this->thruster_coefficient_matrix_times_geometry * forces_and_torques; clampthrottles(&actuations); // publish PWM values @@ -355,15 +482,3 @@ void FlightController::updatePID() { } } -void FlightController::clampthrottles(Eigen::Matrix* throttles) { - Eigen::Index loc; - throttles->minCoeff(&loc); - double ratioA = std::abs(-1.0 / std::min((*throttles)(loc), -1.0)); - throttles->maxCoeff(&loc); - double ratioB = std::abs(1.0 / std::max((*throttles)(loc), 1.0)); - if(ratioA < ratioB) { - *throttles = *throttles * ratioA; - } else { - *throttles = *throttles * ratioB; - } -} diff --git a/src/rov/rov_flight_controller/src/flight_display.cpp b/src/rov/rov_flight_controller/src/flight_display.cpp deleted file mode 100644 index 6b9a1c9..0000000 --- a/src/rov/rov_flight_controller/src/flight_display.cpp +++ /dev/null @@ -1,120 +0,0 @@ -#include - -#include "flight_controller/flight_controller.hpp" - -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include - -#include -#include - -struct ThrusterDisplay : public sf::Drawable { - ThrusterDisplay() { - std::filesystem::path prefix(ament_index_cpp::get_package_share_directory("flight_controller")); - - thrust_vec_tex.loadFromFile((prefix / "assets/" / "thrust_vec_tex.png").string()); - thrust_vec.setTexture(thrust_vec_tex, true); - } - - sf::Text pos; - sf::Text thrust; - sf::Text pin; - sf::Text throttle; - sf::Text pwm; - sf::Sprite thrust_vec; - sf::Texture thrust_vec_tex; -protected: - void draw(sf::RenderTarget& target, sf::RenderStates states) const { - target.draw(thrust_vec, states); - }; -}; - -class FlightDisplay : public rclcpp::Node { -public: - FlightDisplay() : rclcpp::Node("flight_display") { - declare_params(); - - std::stringstream ss; - for(int i=0; i<8; i++) { - ss.str(""); - ss << i; - thruster_displays[i] = ThrusterDisplay(); - - auto to_string = static_cast(std::to_string); - thruster_displays[i].pin.setString(std::to_string(get_parameter("Thruster"+ss.str()+".Pin").as_int())); - thruster_displays[i].pos.setString(boost::algorithm::join(get_parameter("Thruster"+ss.str()+".Position").as_double_array() | boost::adaptors::transformed(to_string), ", ")); - thruster_displays[i].thrust.setString(boost::algorithm::join(get_parameter("Thruster"+ss.str()+".Thrust").as_double_array() | boost::adaptors::transformed(to_string), ", ")); - RCLCPP_INFO(get_logger(), "Thruster%s\nPin: %s\nPos: %s\nThrust: %s", ss.str().c_str(), thruster_displays[i].pin.getString().toAnsiString().c_str(), thruster_displays[i].pos.getString().toAnsiString().c_str(), thruster_displays[i].thrust.getString().toAnsiString().c_str()); - } - } -private: - void declare_params() { - std::stringstream ss; - for(int i=0; i<8; i++) { - ss.str(""); - ss << i; - declare_parameter("Thruster" + ss.str() + ".Position", std::vector{0,0,0}); - declare_parameter("Thruster" + ss.str() + ".Thrust", std::vector{0,0,0}); - declare_parameter("Thruster" + ss.str() + ".Pin", NULL); - } - } - - std::array thruster_displays; -}; - -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - - FlightDisplay::SharedPtr display = std::make_shared(); - - // FlightController::SharedPtr controller = std::make_shared(); - - rclcpp::executors::MultiThreadedExecutor exec; - // exec.add_node(controller); - exec.add_node(display); - - sf::RenderWindow window(sf::VideoMode(800,600), "Flight Display"); - - while(window.isOpen()) { - // process window events - sf::Event ev; - while(window.pollEvent(ev)) { - switch(ev.type) { - case sf::Event::Closed: - window.close(); - break; - case sf::Event::KeyPressed: - if(ev.key.code == sf::Keyboard::Q || ev.key.code == sf::Keyboard::Escape) { - window.close(); - } - break; - default: - break; - } - } - - // update nodes - exec.spin_all(std::chrono::milliseconds(1000/60)); - - // clear window - window.clear(); - - // draw thrusters - - } - - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/src/rov/rov_flight_controller/src/main.cpp b/src/rov/rov_flight_controller/src/main.cpp index a695321..d804052 100644 --- a/src/rov/rov_flight_controller/src/main.cpp +++ b/src/rov/rov_flight_controller/src/main.cpp @@ -1,5 +1,5 @@ #include -#include +#include int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/src/rov/rov_interfaces/msg/ThrusterSetpoints.msg b/src/rov/rov_interfaces/msg/ThrusterSetpoints.msg index e49ee33..8face5f 100644 --- a/src/rov/rov_interfaces/msg/ThrusterSetpoints.msg +++ b/src/rov/rov_interfaces/msg/ThrusterSetpoints.msg @@ -3,4 +3,6 @@ float64 vy float64 vz float64 omegax float64 omegay -float64 omegaz \ No newline at end of file +float64 omegaz +bool keep_depth +bool free_orientation \ No newline at end of file