diff --git a/CMakeLists.txt b/CMakeLists.txt index 2735b4a..b523fae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,7 +84,7 @@ target_include_directories(bno_node $) ament_target_dependencies(bno_node rclcpp std_msgs sensor_msgs geometry_msgs) -ament_target_dependencies(thrusters rclcpp std_msgs sensor_msgs ament_index_cpp) +ament_target_dependencies(thrusters rclcpp std_msgs sensor_msgs geometry_msgs ament_index_cpp) install(TARGETS sh2lib diff --git a/README.md b/README.md index 2b6a722..728445e 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,54 @@ ## Build 1. `cd [YOUR WORKSPACE]` (Don't build inside the package) 2. `colcon build` + +## Launch / Run +After building and sourcing the workspace, you can run nodes individually or use the provided launch file to start both IMU and thruster nodes. + +Run nodes individually: +```bash +source install/local_setup.bash +ros2 run wu25 bno_node +ros2 run wu25 thrusters +``` + +Run both nodes with the packaged launch file (this launch uses a `sudo -E` prefix to preserve environment variables when running nodes that require root access): +```bash +source install/local_setup.bash +ros2 launch wu25 start_nodes.launch.py +``` + +Systemd (run server on boot) +-------------------------------- +You can register the control server (the FastAPI app in `tools/rov_control_server.py`) as a systemd service on the Orange Pi so it starts on boot. + +Create `/etc/systemd/system/rov-control.service` with the following contents (adjust paths and user): + +```ini +[Unit] +Description=ROV Control Server +After=network.target + +[Service] +Type=simple +User=pi +WorkingDirectory=/home/pi/ros2_ws/src/WU25 +Environment=PYTHONUNBUFFERED=1 +ExecStart=/home/pi/ros2_ws/src/WU25/.venv/bin/python3 tools/rov_control_server.py +Restart=on-failure + +[Install] +WantedBy=multi-user.target +``` + +Then enable and start: +```bash +sudo systemctl daemon-reload +sudo systemctl enable rov-control.service +sudo systemctl start rov-control.service +sudo journalctl -u rov-control.service -f +``` + ## Enabling `spidev` (Orange Pi 5) 1. Copy device tree blob file from firmware files diff --git a/include/thrusters/thrusters.hpp b/include/thrusters/thrusters.hpp index 27c7dc0..6d8bbea 100644 --- a/include/thrusters/thrusters.hpp +++ b/include/thrusters/thrusters.hpp @@ -171,6 +171,12 @@ class Thrusters { void SetDesiredRotation(const Eigen::Quaternionf &q); + void SetRotationPIDGains(float p, float i, float d, + float i_zone = std::numeric_limits::max(), + float max_output = std::numeric_limits::max()); + + void SetDepthPIDGains(float kp, float ki, float kd); + static Eigen::Vector3f CalculateAngVel(const Eigen::Quaternionf &q1, const Eigen::Quaternionf &q2, float dt); static float Thrusters::CalculateInclination(const Eigen::Quaternionf& rot); @@ -228,9 +234,9 @@ class Thrusters { ThrusterDecomp decomp_horizontal_; ThrusterDecomp decomp_vertical_; - QuatPIDController::PIDParams x_params_; - QuatPIDController::PIDParams y_params_; - QuatPIDController::PIDParams z_params_; + QuatPIDController::PIDParams x_params_{.p = 1.0f}; + QuatPIDController::PIDParams y_params_{.p = 1.0f}; + QuatPIDController::PIDParams z_params_{.p = 1.0f}; QuatPIDController idle_rotation_controller_; PIDController x_omega_controller_{0, 0, 0}; @@ -238,8 +244,8 @@ class Thrusters { PIDController z_omega_controller_{0, 0, 0}; PIDController idle_depth_controller_{0, 0, 0}; - std::chrono::system_clock::time_point rotation_recieved_time_ns_; - std::chrono::system_clock::time_point previous_rotation_recieved_time_; + std::chrono::high_resolution_clock::time_point rotation_recieved_time_ns_; + std::chrono::high_resolution_clock::time_point previous_rotation_recieved_time_; std::array pwm_outputs_{}; diff --git a/include/util/pid_controller.hpp b/include/util/pid_controller.hpp index dd2755f..3628ef9 100644 --- a/include/util/pid_controller.hpp +++ b/include/util/pid_controller.hpp @@ -14,6 +14,8 @@ class PIDController { void SetSetpoint(float setpoint); + void SetGains(float kP, float kI, float kD, float kFF = 0.0f); + float Calculate(float current_state); private: diff --git a/include/util/quaternion_pid.hpp b/include/util/quaternion_pid.hpp index 31ba17a..f7eacd1 100644 --- a/include/util/quaternion_pid.hpp +++ b/include/util/quaternion_pid.hpp @@ -29,6 +29,11 @@ class QuatPIDController { void SetSetpoint(const Eigen::Quaternionf &setpoint); + void SetParams(float p, float i, float d, + float i_zone = std::numeric_limits::max(), + float i_max_accum = std::numeric_limits::max(), + float max_output = std::numeric_limits::max()); + Eigen::Vector3f Calculate(const Eigen::Quaternionf ¤t_rotation); Eigen::Quaternionf GetError() const; diff --git a/launch/start_nodes.launch.py b/launch/start_nodes.launch.py new file mode 100644 index 0000000..382782e --- /dev/null +++ b/launch/start_nodes.launch.py @@ -0,0 +1,38 @@ +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_ros.actions import Node + + +def generate_launch_description(): + # Prefix to run nodes under sudo while preserving environment variables + sudo_prefix = [ + "sudo -E env \"PYTHONPATH=$PYTHONPATH\" \"LD_LIBRARY_PATH=$LD_LIBRARY_PATH\" \"PATH=$PATH\" \"USER=$USER\" bash -c " + ] + + return LaunchDescription([ + Node( + package='wu25', + executable='bno_node', + name='bno_node', + output='screen', + prefix=sudo_prefix, + shell=True, + ), + Node( + package='wu25', + executable='thrusters', + name='thrusters', + output='screen', + prefix=sudo_prefix, + shell=True, + ), + # Start the ROS->HTTP bridge script as part of the launch (with sudo -E env) + ExecuteProcess( + cmd=[ + 'sudo', '-E', 'env', 'PYTHONPATH=$PYTHONPATH', 'LD_LIBRARY_PATH=$LD_LIBRARY_PATH', + 'PATH=$PATH', 'USER=$USER', 'bash', '-lc', + "source ~/ros2_ws/install/local_setup.bash >/dev/null 2>&1; export ROS_DOMAIN_ID=42; nohup python3 /home/pi/ros2_ws/src/WU25/tools/ros_to_http.py >/tmp/ros_to_http.log 2>&1 &" + ], + output='screen', + ), + ]) diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..8666491 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,3 @@ +fastapi +uvicorn[standard] +paramiko diff --git a/src/thruster_node.cpp b/src/thruster_node.cpp index 023a4e0..4d8356b 100644 --- a/src/thruster_node.cpp +++ b/src/thruster_node.cpp @@ -3,12 +3,15 @@ #include #include #include -#include +#include #include #include #include +#include #include +#include +#include #include "thrusters/thrusters.hpp" #include "util/controller_map.hpp" @@ -17,6 +20,8 @@ using namespace std::chrono_literals; +static constexpr float MAX_THRUST_KGF = 2.4f; + class ThrusterNode final : public rclcpp::Node { public: ThrusterNode() : Node("thruster_node"), log_(this->get_logger()) { @@ -27,6 +32,50 @@ class ThrusterNode final : public rclcpp::Node { this->create_subscription( "joy", 3, std::bind(&ThrusterNode::JoyCallback, this, std::placeholders::_1)); + quat_sub_ = this->create_subscription( + "bno/quat", 10, std::bind(&ThrusterNode::QuatCallback, this, std::placeholders::_1)); + + thruster_pub_ = this->create_publisher("/thrusters/output", 10); + mode_pub_ = this->create_publisher("/thrusters/control_mode", 10); + + // PID parameters (runtime-tunable via ros2 param set) + this->declare_parameter("rot_kp", 1.0); + this->declare_parameter("rot_ki", 0.0); + this->declare_parameter("rot_kd", 0.0); + this->declare_parameter("rot_i_zone", static_cast(std::numeric_limits::max())); + this->declare_parameter("rot_max_output", static_cast(MAX_THRUST_KGF)); + this->declare_parameter("depth_kp", 0.0); + this->declare_parameter("depth_ki", 0.0); + this->declare_parameter("depth_kd", 0.0); + + param_cb_ = this->add_on_set_parameters_callback( + [this](const std::vector& params) { + bool update_rot = false, update_depth = false; + for (const auto& p : params) { + const auto& name = p.get_name(); + if (name == "rot_kp") { rot_kp_ = static_cast(p.as_double()); update_rot = true; } + else if (name == "rot_ki") { rot_ki_ = static_cast(p.as_double()); update_rot = true; } + else if (name == "rot_kd") { rot_kd_ = static_cast(p.as_double()); update_rot = true; } + else if (name == "rot_i_zone") { rot_i_zone_ = static_cast(p.as_double()); update_rot = true; } + else if (name == "rot_max_output"){ rot_max_output_ = static_cast(p.as_double()); update_rot = true; } + else if (name == "depth_kp") { depth_kp_ = static_cast(p.as_double()); update_depth = true; } + else if (name == "depth_ki") { depth_ki_ = static_cast(p.as_double()); update_depth = true; } + else if (name == "depth_kd") { depth_kd_ = static_cast(p.as_double()); update_depth = true; } + } + if (update_rot && thrusters_) { + thrusters_->SetRotationPIDGains(rot_kp_, rot_ki_, rot_kd_, rot_i_zone_, rot_max_output_); + RCLCPP_INFO(log_, "Rotation PID updated: kp=%.3f ki=%.3f kd=%.3f", rot_kp_, rot_ki_, rot_kd_); + } + if (update_depth && thrusters_) { + thrusters_->SetDepthPIDGains(depth_kp_, depth_ki_, depth_kd_); + RCLCPP_INFO(log_, "Depth PID updated: kp=%.3f ki=%.3f kd=%.3f", depth_kp_, depth_ki_, depth_kd_); + } + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; + } + ); + thrusters_ = std::make_unique(); thrusters_->Init(); @@ -43,17 +92,15 @@ class ThrusterNode final : public rclcpp::Node { std::this_thread::sleep_for(2s); RCLCPP_INFO(log_, "Initialize finished\n"); -// -// Vector v{1, 0, 0, 0, 0, 0}; -// std::cout << "Calling constructor" << std::endl; -// Thrusters::ThrustVector tvec(v); -// thrusters_->SetThrustVector(tvec); -// Quaternionf desired = -// AngleAxisf(0, Vector3f::UnitX()) * -// AngleAxisf(0, Vector3f::UnitY()) * -// AngleAxisf(90.f * M_PIf / 180.f, Vector3f::UnitZ()); -// -// thrusters_->SetDesiredRotation(desired); + } + + void QuatCallback(const geometry_msgs::msg::QuaternionStamped::UniquePtr &msg) { + const auto &q = msg->quaternion; + Eigen::Quaternionf qf(static_cast(q.w), static_cast(q.x), static_cast(q.y), static_cast(q.z)); + last_quat_ = qf; + if (thrusters_) { + thrusters_->SetRotation(qf); + } } uint16_t negatePeriod(uint16_t period) { @@ -64,25 +111,11 @@ class ThrusterNode final : public rclcpp::Node { void Loop() { Thrusters::ThrusterOutputs applied_outputs = thrusters_->Update(); -// RCLCPP_INFO(log_, "%.02f\t%.02f\t%.02f\t%.02f\t%.02f\t%.02f\t%.02f\t%.02f", -// applied_outputs[0], applied_outputs[1], applied_outputs[2], applied_outputs[3], -// applied_outputs[4], applied_outputs[5], applied_outputs[6], applied_outputs[7]); - Thrusters::PCAOutputs pca_outputs = thrusters_->GetPWMOutputs(applied_outputs); RCLCPP_INFO(log_, "%u\t%u\t%u\t%u\t%u\t%u\t%u\t%u", pca_outputs[0], pca_outputs[1], pca_outputs[2], pca_outputs[3], pca_outputs[4], pca_outputs[5], pca_outputs[6], pca_outputs[7]); -// const int FLH = 4; -// const int FRH = 3; -// const int BLH = 7; -// const int BRH = 2; -// -// const int FLV = 6; -// const int FRV = 1; -// const int BLV = 5; -// const int BRV = 0; // 0 - const int FLH = 6; const int FRH = 1; const int BLH = 7; @@ -91,12 +124,12 @@ class ThrusterNode final : public rclcpp::Node { const int FLV = 4; const int FRV = 3; const int BLV = 5; - const int BRV = 0; // 0 + const int BRV = 0; for (auto &period : pca_outputs) { if (period > 1490 && period < 1510) { period = 1505; - }else if (period == 1505) { + } else if (period == 1505) { period = 1495; } else if (period == 1495) { period = 1505; @@ -114,27 +147,26 @@ class ThrusterNode final : public rclcpp::Node { remapped[BLV] = pca_outputs[6]; remapped[BRV] = pca_outputs[7]; - -// std::vector outs(pca_outputs.begin(), pca_outputs.end()); PCAError ret = pca_->set_period(0, remapped); -// PCAError ret = pca_->set_period(6, 1700); if (ret != PCA_OK) { RCLCPP_ERROR(log_, "Failed to set PCA outputs"); } + // Publish normalized thruster forces (-1 to 1) + std_msgs::msg::Float32MultiArray thruster_msg; + thruster_msg.data.resize(8); + for (size_t i = 0; i < 8; i++) { + thruster_msg.data[i] = applied_outputs[i] / MAX_THRUST_KGF; + } + thruster_pub_->publish(thruster_msg); + + // Publish control mode state + PublishControlMode(); } void JoyCallback(const sensor_msgs::msg::Joy::UniquePtr &msg) { using namespace XBoxController; -// const float x_translation = GetExponential(msg->axes[std::to_underlying(Axis::LEFT_Y)], 1, 1, STICK_DEADBAND); -// const float y_translation = GetExponential(msg->axes[std::to_underlying(Axis::LEFT_X)], 1, 1, STICK_DEADBAND); -// const float z_rotation = GetExponential(-msg->axes[std::to_underlying(Axis::RIGHT_X)], 1, 1, STICK_DEADBAND); -// const float y_rotation = GetExponential(-msg->axes[std::to_underlying(Axis::RIGHT_Y)], 1, 1, STICK_DEADBAND); -// -// float left_trigger = GetExponential(msg->axes[std::to_underlying(Axis::LEFT_TRIGGER)], 1, 1, TRIGGER_DEADBAND); -// float right_trigger = GetExponential(msg->axes[std::to_underlying(Axis::RIGHT_TRIGGER)], 1, 1, -// TRIGGER_DEADBAND); const float x_translation = Deadband(msg->axes[std::to_underlying(Axis::LEFT_Y)], STICK_DEADBAND); const float y_translation = Deadband(-msg->axes[std::to_underlying(Axis::LEFT_X)], STICK_DEADBAND); const float z_rotation = Deadband(-msg->axes[std::to_underlying(Axis::RIGHT_X)], STICK_DEADBAND); @@ -145,9 +177,7 @@ class ThrusterNode final : public rclcpp::Node { int32_t right_bumper = msg->buttons[std::to_underlying(Button::RIGHT_SHOULDER)]; int32_t left_bumper = msg->buttons[std::to_underlying(Button::LEFT_SHOULDER)]; - -// int32_t - + int32_t y_button = msg->buttons[std::to_underlying(Button::Y)]; // Remap triggers [1:-1] -> [0:1] left_trigger = -(left_trigger - 1.f); @@ -155,17 +185,34 @@ class ThrusterNode final : public rclcpp::Node { const float z_translation = -(right_trigger - left_trigger) / 2.f; - RCLCPP_INFO(log_, "X: %.03f\tY: %.03f\t Z: %.03f, Yaw: %.03f\t Pitch: %.03f", + RCLCPP_INFO(log_, "X: %.03f\tY: %.03f\t Z: %.03f, Yaw: %.03f\t Pitch: %.03f", x_translation, y_translation, z_translation, z_rotation, y_rotation); thrusters_->SetThrustVector({ - {x_translation * 3.f, y_translation * 3.f, z_translation * 4.5f}, - {0, -y_rotation, z_rotation}}); + {x_translation * 3.f, y_translation * 3.f, z_translation * 4.5f}, + {0, -y_rotation, z_rotation}}); + + // Toggle hold-idle-rotation on Y button rising edge + if (y_button && !prev_y_button_) { + hold_idle_rotation_enabled_ = !hold_idle_rotation_enabled_; + thrusters_->SetHoldIdleRotation(hold_idle_rotation_enabled_); + if (hold_idle_rotation_enabled_) { + if (last_quat_.coeffs().norm() != 0.0f) { + thrusters_->SetDesiredRotation(last_quat_); + } + RCLCPP_INFO(log_, "Y pressed: enabled hold idle rotation"); + } else { + RCLCPP_INFO(log_, "Y pressed: disabled hold idle rotation"); + } + } + if (right_bumper) { pca_->set_period(8, 1700); } else if (left_bumper) { pca_->set_period(8, 1300); } + + prev_y_button_ = y_button; } template @@ -177,7 +224,6 @@ class ThrusterNode final : public rclcpp::Node { if (std::fabs(input) < deadband) { return 0.f; } - return input; } @@ -197,17 +243,46 @@ class ThrusterNode final : public rclcpp::Node { } private: + void PublishControlMode() { + std_msgs::msg::String msg; + msg.data = std::string("{") + + "\"hold_rotation\":" + (hold_idle_rotation_enabled_ ? "true" : "false") + + ",\"hold_depth\":" + (hold_idle_depth_enabled_ ? "true" : "false") + + ",\"ang_vel_control\":" + (ang_vel_control_enabled_ ? "true" : "false") + + ",\"depth_lock\":" + (depth_lock_enabled_ ? "true" : "false") + + "}"; + mode_pub_->publish(msg); + } + const float STICK_DEADBAND = 0.1; const float TRIGGER_DEADBAND = 0.1; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr thruster_pub_; + rclcpp::Publisher::SharedPtr mode_pub_; rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Subscription::SharedPtr quat_sub_; + OnSetParametersCallbackHandle::SharedPtr param_cb_; const rclcpp::Logger log_; std::unique_ptr thrusters_; std::unique_ptr pca_; size_t count_{}; Thrusters::ThrustVector joy_tvec_; + Eigen::Quaternionf last_quat_{1, 0, 0, 0}; + int prev_y_button_{0}; + + // Control mode state + bool hold_idle_rotation_enabled_{false}; + bool hold_idle_depth_enabled_{false}; + bool ang_vel_control_enabled_{false}; + bool depth_lock_enabled_{false}; + + // PID gain tracking (mirrors ROS params) + float rot_kp_{1.0f}, rot_ki_{0.0f}, rot_kd_{0.0f}; + float rot_i_zone_{std::numeric_limits::max()}; + float rot_max_output_{MAX_THRUST_KGF}; + float depth_kp_{0.0f}, depth_ki_{0.0f}, depth_kd_{0.0f}; }; int main(const int argc, char *argv[]) { diff --git a/src/thrusters/thrusters.cpp b/src/thrusters/thrusters.cpp index f81be17..37cfe89 100644 --- a/src/thrusters/thrusters.cpp +++ b/src/thrusters/thrusters.cpp @@ -13,6 +13,10 @@ Thrusters::Thrusters(): thruster_data_(DATA_PATH), idle_rotation_controller_(x_params_, y_params_, z_params_) { + const auto now = std::chrono::high_resolution_clock::now(); + rotation_recieved_time_ns_ = now; + previous_rotation_recieved_time_ = now; + Eigen::Vector4f horizontal_angles( THRUSTER_ANGLE_RAD, -THRUSTER_ANGLE_RAD, 2.f * M_PIf - THRUSTER_ANGLE_RAD, -(2.f * M_PIf - THRUSTER_ANGLE_RAD) ); @@ -48,10 +52,8 @@ Thrusters::Thrusters(): thruster_data_(DATA_PATH), idle_rotation_controller_(x_p decomp_horizontal_ = ThrusterDecomp(thruster_config_horizontal); decomp_vertical_ = ThrusterDecomp(thruster_config_vertical); - x_params_ = {.p = 1}; - y_params_ = {.p = 1}; - z_params_ = {.p = 1}; - // rotation_controller_ = {x_params_, y_params_, z_params_}; + // x_params_, y_params_, z_params_ are now initialized in the header (.p = 1.0f); + // nothing to do here // X, Y, Yaw const Eigen::Vector3f horizontal_vector(1, 0, 0); @@ -83,11 +85,11 @@ Thrusters::ThrusterOutputs Thrusters::Update() { y_omega_controller_.SetSetpoint(thrust_vector_.angular_.y()); z_omega_controller_.SetSetpoint(thrust_vector_.angular_.z()); - auto dt = std::chrono::duration_cast - (rotation_recieved_time_ns_ - previous_rotation_recieved_time_).count(); + const float dt_s = std::chrono::duration( + rotation_recieved_time_ns_ - previous_rotation_recieved_time_).count(); // Calculate current angular velocity - Eigen::Vector3f angVel = CalculateAngVel(previous_rotation_, current_rotation_, static_cast(dt)); + Eigen::Vector3f angVel = CalculateAngVel(previous_rotation_, current_rotation_, dt_s); // Set angular thrust vector to pid calculated kgf. thrust_vector_.angular_ = { @@ -201,6 +203,7 @@ void Thrusters::SetThrustVector(const ThrustVector &thrust_vector) { void Thrusters::SetRotation(const Eigen::Quaternionf &q) { previous_rotation_recieved_time_ = rotation_recieved_time_ns_; rotation_recieved_time_ns_ = std::chrono::high_resolution_clock::now(); + previous_rotation_ = current_rotation_; current_rotation_ = q; } @@ -209,6 +212,14 @@ void Thrusters::SetDesiredRotation(const Eigen::Quaternionf &q) { idle_rotation_controller_.SetSetpoint(q); } +void Thrusters::SetRotationPIDGains(float p, float i, float d, float i_zone, float max_output) { + idle_rotation_controller_.SetParams(p, i, d, i_zone, max_output * 2.0f, max_output); +} + +void Thrusters::SetDepthPIDGains(float kp, float ki, float kd) { + idle_depth_controller_.SetGains(kp, ki, kd); +} + Eigen::Vector3f Thrusters::CalculateAngVel(const Eigen::Quaternionf &q1, const Eigen::Quaternionf &q2, const float dt) { // Calculate relative rotation (error) auto q_delta = q1.inverse() * q2; @@ -309,16 +320,16 @@ Thrusters::ThrusterOutputs::ThrusterOutputs(const Eigen::Solve max_thrust_kgf) { + const float ratio = real_max_thrust_kgf / max_thrust_kgf; for (float &value: horizontal_) { value /= ratio; } diff --git a/src/util/pid_controller.cpp b/src/util/pid_controller.cpp index 0680508..4115a00 100644 --- a/src/util/pid_controller.cpp +++ b/src/util/pid_controller.cpp @@ -3,33 +3,65 @@ // #include "util/pid_controller.hpp" +#include PIDController::PIDController(const float kP, const float kI, const float kD, const float kFF, const float max_output) : kP_(kP), kI_(kI), kD_(kD), kFF_(kFF), max_output_(max_output) { - prev_time_ms_ = std::chrono::high_resolution_clock::now(); + prev_time_ms_ = std::chrono::system_clock::now(); } void PIDController::SetSetpoint(const float setpoint) { setpoint_ = setpoint; } +void PIDController::SetGains(const float kP, const float kI, const float kD, const float kFF) { + kP_ = kP; + kI_ = kI; + kD_ = kD; + kFF_ = kFF; + i_accum_ = 0.0f; + prev_error_ = 0.0f; +} + float PIDController::Calculate(const float current_state) { - const auto now = std::chrono::high_resolution_clock::now(); - const auto dt = std::chrono::duration_cast(now - prev_time_ms_).count(); + const auto now = std::chrono::system_clock::now(); + const std::chrono::duration dt_duration = now - prev_time_ms_; + const float dt_s = dt_duration.count(); prev_time_ms_ = now; - const auto error = setpoint_ - current_state; - i_accum_ += error * static_cast(dt); + // Guard: if dt is zero or extremely small, don't update derivative/integral + if (dt_s <= 0.0f) { + return 0.0f; + } + + const float error = setpoint_ - current_state; + + // Integrate with seconds units + i_accum_ += error * dt_s; - const auto p_out = kP_ * error; - const auto i_out = kI_ * i_accum_; - float d_out = 0; - if (dt != 0) { - d_out = kD_ * (error / static_cast(dt)); + // Anti-windup: clamp integral term if kI_ non-zero and max_output_ finite + if (kI_ != 0.0f && max_output_ < std::numeric_limits::max()) { + const float i_limit = std::abs(max_output_ / kI_); + i_accum_ = std::clamp(i_accum_, -i_limit, i_limit); } - const auto ff_out = kFF_ * setpoint_; + + const float p_out = kP_ * error; + const float i_out = kI_ * i_accum_; + + float d_out = 0.0f; + if (dt_s > 1e-6f) { + d_out = kD_ * ((error - prev_error_) / dt_s); + } + const float ff_out = kFF_ * setpoint_; prev_error_ = error; - return p_out + i_out + d_out + ff_out; + float output = p_out + i_out + d_out + ff_out; + + // Final output clamp + if (max_output_ < std::numeric_limits::max()) { + output = std::clamp(output, -max_output_, max_output_); + } + + return output; } diff --git a/src/util/quaternion_pid.cpp b/src/util/quaternion_pid.cpp index e7a859e..6e452ca 100644 --- a/src/util/quaternion_pid.cpp +++ b/src/util/quaternion_pid.cpp @@ -10,48 +10,104 @@ QuatPIDController::QuatPIDController(const PIDParams &x_params, const PIDParams x_params_ = x_params; y_params_ = y_params; z_params_ = z_params; - prev_time_ = std::chrono::high_resolution_clock::now(); + prev_time_ = std::chrono::system_clock::now(); } void QuatPIDController::SetSetpoint(const Eigen::Quaternionf &setpoint) { setpoint_ = setpoint; } +void QuatPIDController::SetParams(float p, float i, float d, float i_zone, float i_max_accum, float max_output) { + PIDParams params; + params.p = p; + params.i = i; + params.d = d; + params.i_zone = i_zone; + params.i_max_accum = i_max_accum; + params.max_output = max_output; + params.i_accum = 0.0f; + x_params_ = params; + y_params_ = params; + z_params_ = params; + prev_x_error_ = 0.0f; + prev_y_error_ = 0.0f; + prev_z_error_ = 0.0f; +} + Eigen::Vector3f QuatPIDController::Calculate(const Eigen::Quaternionf ¤t_rotation) { current_ = current_rotation; - Eigen::Quaternionf error = GetError(); + // Compute error quaternion: rotation from current -> setpoint + Eigen::Quaternionf q_err = setpoint_ * current_.inverse(); + + // Ensure shortest rotation + if (q_err.w() < 0.0f) { + q_err.coeffs() = -q_err.coeffs(); + } - // Try?: if (current {dot} error < 0) then current = -current (might be error instead) - if (error.w() < 0) { - error = Eigen::Quaternionf{-error.w(), -error.x(), -error.y(), -error.z()}; + // Convert to angle-axis (angle in radians) + const float w = std::clamp(q_err.w(), -1.0f, 1.0f); + const float angle = 2.0f * std::acos(w); + const float sin_half = std::sqrt(std::max(0.0f, 1.0f - w * w)); + + Eigen::Vector3f axis; + if (sin_half < 1e-6f) { + axis = Eigen::Vector3f::Zero(); + } else { + axis = q_err.vec() / sin_half; } - const auto current_time = std::chrono::high_resolution_clock::now(); + // axis * angle gives an angle-axis vector (radians) suitable as error signal + Eigen::Vector3f angle_axis = axis * angle; - const auto dt = std::chrono::duration_cast(current_time - prev_time_); - const auto dtf = static_cast(dt.count()); + const auto current_time = std::chrono::system_clock::now(); + const std::chrono::duration dt_duration = current_time - prev_time_; + const float dtf = dt_duration.count(); prev_time_ = current_time; + if (dtf <= 0.0f) { + return Eigen::Vector3f::Zero(); + } return { - CalculateAxisOutput(error.x(), prev_x_error_, dtf, x_params_), - CalculateAxisOutput(error.y(), prev_y_error_, dtf, y_params_), - CalculateAxisOutput(error.z(), prev_z_error_, dtf, z_params_) + CalculateAxisOutput(angle_axis.x(), prev_x_error_, dtf, x_params_), + CalculateAxisOutput(angle_axis.y(), prev_y_error_, dtf, y_params_), + CalculateAxisOutput(angle_axis.z(), prev_z_error_, dtf, z_params_) }; } Eigen::Quaternionf QuatPIDController::GetError() const { - return current_.inverse() * setpoint_; + return setpoint_ * current_.inverse(); } float QuatPIDController::CalculateAxisOutput(const float error, float &prev_error, const float dt, PIDParams ¶ms) { - params.i_accum += error * dt; - const float de = (error - prev_error) / dt; + // Guard dt + if (dt <= 0.0f) { + return 0.0f; + } - const float p_out = error * params.p; - const float i_out = params.i_accum * params.i; - const float d_out = de * params.d; + // Proportional + const float p_out = params.p * error; + + // Integral (only accumulate inside i_zone) + if (std::abs(error) <= params.i_zone) { + params.i_accum += error * dt; + if (params.i_accum > params.i_max_accum) params.i_accum = params.i_max_accum; + if (params.i_accum < -params.i_max_accum) params.i_accum = -params.i_max_accum; + } + const float i_out = params.i * params.i_accum; + + // Derivative + const float de = (error - prev_error) / dt; + const float d_out = params.d * de; prev_error = error; - return p_out + i_out + d_out; + + float out = p_out + i_out + d_out; + + // Clamp by symmetric max output if provided + if (params.max_output < std::numeric_limits::max()) { + out = std::clamp(out, -params.max_output, params.max_output); + } + + return out; } diff --git a/tools/ros_to_http.py b/tools/ros_to_http.py new file mode 100644 index 0000000..b0d72d3 --- /dev/null +++ b/tools/ros_to_http.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 +""" +Simple ROS2 -> HTTP telemetry bridge + +Subscribes to ROS topics and forwards their data as JSON to an HTTP endpoint. + +Configure via environment variables: + TELEMETRY_ENDPOINT HTTP endpoint to POST JSON payloads (default: http://localhost:8080/api/telemetry) + +Run on Orange Pi (same environment as ROS2). +""" +import os +import time +import json +import threading + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import QuaternionStamped +from sensor_msgs.msg import Joy +from std_msgs.msg import Float32MultiArray, String + +import requests + + +ENDPOINT = os.environ.get('TELEMETRY_ENDPOINT', 'http://localhost:8080/api/telemetry') + +# Minimum seconds between HTTP posts per topic (rate limiting) +RATE_IMU = 0.05 # 20 Hz max +RATE_JOY = 0.1 # 10 Hz max +RATE_THRUSTERS = 0.05 # 20 Hz max +RATE_MODE = 0.2 # 5 Hz max + + +def post_json(payload: dict): + try: + requests.post(ENDPOINT, json=payload, timeout=2) + except Exception as e: + print('telemetry post failed:', e) + + +class TelemetryBridge(Node): + def __init__(self): + super().__init__('ros_to_http_bridge') + + self._last_imu = 0.0 + self._last_joy = 0.0 + self._last_thrusters = 0.0 + self._last_mode = 0.0 + + self.create_subscription(QuaternionStamped, '/bno/quat', self.cb_quat, 10) + self.create_subscription(Joy, '/joy', self.cb_joy, 10) + self.create_subscription(Float32MultiArray, '/thrusters/output', self.cb_thrusters, 10) + self.create_subscription(String, '/thrusters/control_mode', self.cb_control_mode, 10) + + self.get_logger().info('TelemetryBridge ready, posting to ' + ENDPOINT) + + def cb_quat(self, msg: QuaternionStamped): + now = time.time() + if now - self._last_imu < RATE_IMU: + return + self._last_imu = now + payload = { + 'topic': 'bno/quat', + 'ts': now, + 'quat': { + 'w': float(msg.quaternion.w), + 'x': float(msg.quaternion.x), + 'y': float(msg.quaternion.y), + 'z': float(msg.quaternion.z), + } + } + threading.Thread(target=post_json, args=(payload,), daemon=True).start() + + def cb_joy(self, msg: Joy): + now = time.time() + if now - self._last_joy < RATE_JOY: + return + self._last_joy = now + payload = { + 'topic': 'joy', + 'ts': now, + 'axes': [float(a) for a in msg.axes], + 'buttons': [int(b) for b in msg.buttons] + } + threading.Thread(target=post_json, args=(payload,), daemon=True).start() + + def cb_thrusters(self, msg: Float32MultiArray): + now = time.time() + if now - self._last_thrusters < RATE_THRUSTERS: + return + self._last_thrusters = now + payload = { + 'topic': 'thrusters/output', + 'ts': now, + 'thrusters': [float(v) for v in msg.data] + } + threading.Thread(target=post_json, args=(payload,), daemon=True).start() + + def cb_control_mode(self, msg: String): + now = time.time() + if now - self._last_mode < RATE_MODE: + return + self._last_mode = now + try: + mode = json.loads(msg.data) + except Exception: + mode = {} + payload = {'topic': 'thrusters/control_mode', 'ts': now, **mode} + threading.Thread(target=post_json, args=(payload,), daemon=True).start() + + +def main(): + rclpy.init() + node = TelemetryBridge() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/tools/rov_control_server.py b/tools/rov_control_server.py new file mode 100644 index 0000000..73e30b6 --- /dev/null +++ b/tools/rov_control_server.py @@ -0,0 +1,187 @@ +#!/usr/bin/env python3 +""" +Prototype ROV control server +- REST endpoints to start/stop ROS nodes on remote hosts via SSH +- WebSocket /ws/telemetry to push telemetry to connected UIs +- Serves static web UI from ../web + +Configure remote hosts via environment variables: +ORANGEPI_HOST, ORANGEPI_USER, ORANGEPI_KEY +WINDOWS_HOST, WINDOWS_USER, WINDOWS_KEY + +Run: python3 tools/rov_control_server.py +""" +import os +import asyncio +from fastapi import FastAPI, WebSocket, WebSocketDisconnect, Request +from fastapi.responses import JSONResponse, HTMLResponse +from fastapi.staticfiles import StaticFiles +import paramiko +from typing import List + +app = FastAPI() + +WEB_ROOT = os.path.join(os.path.dirname(__file__), '..', 'web') +app.mount('/static', StaticFiles(directory=WEB_ROOT), name='static') + +ORANGEPI_HOST = os.environ.get('ORANGEPI_HOST', '192.168.1.100') # update the correct IP +ORANGEPI_USER = os.environ.get('ORANGEPI_USER', 'pi') # update the correct username +ORANGEPI_KEY = os.environ.get('ORANGEPI_KEY') # path to private key + +WINDOWS_HOST = os.environ.get('WINDOWS_HOST', 'windows-host') # Note: Windows SSH server must be configured and running, or use WSL with an SSH server inside it +WINDOWS_USER = os.environ.get('WINDOWS_USER', 'user') # For Windows, key-based auth is less common; you may need to set up an SSH server that supports it, or use password auth (not implemented here for security reasons). If using WSL, you can set up key-based auth inside the WSL environment and point WINDOWS_KEY to that. +WINDOWS_KEY = os.environ.get('WINDOWS_KEY') # path to private key for Windows SSH, if applicable. If using WSL, this would be the key for the WSL SSH server. + +ROS_DOMAIN = os.environ.get('ROS_DOMAIN_ID', '42') # ensure both Orange Pi and Windows host use the same ROS_DOMAIN_ID for communication + + +class ConnectionError(Exception): + pass + + +async def ssh_exec(host: str, user: str, key_path: str, cmd: str, timeout: int = 10): + """Execute cmd over SSH using paramiko in a thread to avoid blocking async loop.""" + def _run(): + client = paramiko.SSHClient() + client.set_missing_host_key_policy(paramiko.AutoAddPolicy()) + try: + if key_path: + client.connect(hostname=host, username=user, key_filename=key_path, timeout=5) + else: + client.connect(hostname=host, username=user, timeout=5) + stdin, stdout, stderr = client.exec_command(cmd) + out = stdout.read().decode(errors='ignore') + err = stderr.read().decode(errors='ignore') + client.close() + return out, err + except Exception as e: + raise ConnectionError(str(e)) + + return await asyncio.to_thread(_run) + + +@app.get('/') +async def index(): + path = os.path.join(WEB_ROOT, 'index.html') + with open(path, 'r', encoding='utf-8') as f: + return HTMLResponse(f.read()) + + +@app.post('/api/orangepi/start_nodes') +async def start_orangepi_nodes(): + # start nodes using ros2 launch under bash -lc so environment sourcing works + cmd = f"bash -lc 'source ~/ros2_ws/install/local_setup.bash >/dev/null 2>&1; export ROS_DOMAIN_ID={ROS_DOMAIN}; nohup ros2 launch wu25 start_nodes.launch.py >/tmp/rov_nodes.log 2>&1 &'" + try: + out, err = await ssh_exec(ORANGEPI_HOST, ORANGEPI_USER, ORANGEPI_KEY, cmd) + return JSONResponse({'ok': True, 'out': out, 'err': err}) + except ConnectionError as e: + return JSONResponse({'ok': False, 'error': str(e)}, status_code=500) + + +@app.post('/api/orangepi/stop_nodes') +async def stop_orangepi_nodes(): + # attempt to kill thrusters and bno_node processes + cmd = "bash -lc 'pkill -f bno_node || true; pkill -f thrusters || true; pkill -f ros2 || true'" + try: + out, err = await ssh_exec(ORANGEPI_HOST, ORANGEPI_USER, ORANGEPI_KEY, cmd) + return JSONResponse({'ok': True, 'out': out, 'err': err}) + except ConnectionError as e: + return JSONResponse({'ok': False, 'error': str(e)}, status_code=500) + + +@app.post('/api/windows/start_joy') +async def start_windows_joy(): + # Example: run joy node inside WSL on the Windows host + cmd = f"wsl -d Ubuntu-22.04 -- bash -lc 'export ROS_DOMAIN_ID={ROS_DOMAIN}; source /opt/ros/jazzy/setup.bash >/dev/null 2>&1; nohup ros2 run joy joy_node >/tmp/joy.log 2>&1 &'" + try: + out, err = await ssh_exec(WINDOWS_HOST, WINDOWS_USER, WINDOWS_KEY, cmd) + return JSONResponse({'ok': True, 'out': out, 'err': err}) + except ConnectionError as e: + return JSONResponse({'ok': False, 'error': str(e)}, status_code=500) + + +@app.post('/api/windows/stop_joy') +async def stop_windows_joy(): + cmd = "wsl -d Ubuntu-22.04 -- bash -lc 'pkill -f joy_node || true'" + try: + out, err = await ssh_exec(WINDOWS_HOST, WINDOWS_USER, WINDOWS_KEY, cmd) + return JSONResponse({'ok': True, 'out': out, 'err': err}) + except ConnectionError as e: + return JSONResponse({'ok': False, 'error': str(e)}, status_code=500) + + +# telemetry WebSocket manager +class TelemetryManager: + def __init__(self): + self.clients: List[WebSocket] = [] + + async def connect(self, ws: WebSocket): + await ws.accept() + self.clients.append(ws) + + def disconnect(self, ws: WebSocket): + if ws in self.clients: + self.clients.remove(ws) + + async def broadcast(self, message: str): + for ws in list(self.clients): + try: + await ws.send_text(message) + except Exception: + self.disconnect(ws) + + +tm = TelemetryManager() + + +@app.websocket('/ws/telemetry') +async def websocket_telemetry(ws: WebSocket): + await tm.connect(ws) + try: + while True: + # this server only broadcasts telemetry pushed via REST POST; keep connection open + await ws.receive_text() + except WebSocketDisconnect: + tm.disconnect(ws) + + +@app.post('/api/telemetry') +async def post_telemetry(request: Request): + data = await request.body() + await tm.broadcast(data.decode()) + return JSONResponse({'ok': True}) + + +@app.post('/api/pid') +async def set_pid_params(request: Request): + """Set PID gains on the running thruster_node via ros2 param set over SSH.""" + try: + data = await request.json() + except Exception: + return JSONResponse({'ok': False, 'error': 'Invalid JSON'}, status_code=400) + + allowed = ['rot_kp', 'rot_ki', 'rot_kd', 'rot_i_zone', 'rot_max_output', + 'depth_kp', 'depth_ki', 'depth_kd'] + set_cmds = [] + for name in allowed: + if name in data: + val = data[name] + if isinstance(val, (int, float)): + set_cmds.append(f'ros2 param set /thruster_node {name} {float(val)}') + + if not set_cmds: + return JSONResponse({'ok': False, 'error': 'No valid parameters provided'}, status_code=400) + + joined = ' && '.join(set_cmds) + cmd = (f"bash -lc 'source ~/ros2_ws/install/local_setup.bash >/dev/null 2>&1; " + f"export ROS_DOMAIN_ID={ROS_DOMAIN}; {joined}'") + try: + out, err = await ssh_exec(ORANGEPI_HOST, ORANGEPI_USER, ORANGEPI_KEY, cmd) + return JSONResponse({'ok': True, 'out': out, 'err': err}) + except ConnectionError as e: + return JSONResponse({'ok': False, 'error': str(e)}, status_code=500) + + +if __name__ == '__main__': + import uvicorn + uvicorn.run('tools.rov_control_server:app', host='0.0.0.0', port=8080, log_level='info') diff --git a/web/app.js b/web/app.js new file mode 100644 index 0000000..2308126 --- /dev/null +++ b/web/app.js @@ -0,0 +1,160 @@ +// WebSocket connection +const ws = new WebSocket((location.protocol === 'https:' ? 'wss://' : 'ws://') + location.host + '/ws/telemetry'); +const connEl = document.getElementById('conn-status'); + +ws.onopen = () => { connEl.textContent = '● Connected'; connEl.className = 'badge connected'; }; +ws.onclose = () => { connEl.textContent = '● Disconnected'; connEl.className = 'badge disconnected'; }; +ws.onerror = () => { connEl.textContent = '● Error'; connEl.className = 'badge disconnected'; }; + +// ── Thruster bars ────────────────────────────────────────────────── +const THRUSTER_NAMES = ['FLH', 'FRH', 'BLH', 'BRH', 'FLV', 'FRV', 'BLV', 'BRV']; +const barsContainer = document.getElementById('thruster-bars'); + +THRUSTER_NAMES.forEach((name, i) => { + const div = document.createElement('div'); + div.className = 'tbar'; + div.id = `tbar-${i}`; + div.innerHTML = + `${name}` + + `
` + + `
` + + `
` + + `
` + + `
` + + `0.00`; + barsContainer.appendChild(div); +}); + +function updateThrusterBar(i, value) { + const v = Math.max(-1, Math.min(1, value)); + const pct = Math.abs(v) * 50; + document.getElementById(`tpos-${i}`).style.width = (v >= 0 ? pct : 0) + '%'; + document.getElementById(`tneg-${i}`).style.width = (v < 0 ? pct : 0) + '%'; + document.getElementById(`tval-${i}`).textContent = v.toFixed(2); +} + +// ── IMU quaternion → Euler ──────────────────────────────────────── +function quatToEulerDeg(w, x, y, z) { + const sinr = 2 * (w * x + y * z); + const cosr = 1 - 2 * (x * x + y * y); + const roll = Math.atan2(sinr, cosr) * 180 / Math.PI; + + const sinp = 2 * (w * y - z * x); + const pitch = (Math.abs(sinp) >= 1) + ? Math.sign(sinp) * 90 + : Math.asin(sinp) * 180 / Math.PI; + + const siny = 2 * (w * z + x * y); + const cosy = 1 - 2 * (y * y + z * z); + const yaw = Math.atan2(siny, cosy) * 180 / Math.PI; + + return { roll, pitch, yaw }; +} + +// ── Control mode indicators ──────────────────────────────────────── +function setLed(id, active) { + const el = document.getElementById(id); + if (el) el.className = 'led' + (active ? ' active' : ''); +} + +// ── WebSocket message handler ────────────────────────────────────── +ws.onmessage = (ev) => { + let data; + try { data = JSON.parse(ev.data); } catch { return; } + + if (data.topic === 'thrusters/output' && Array.isArray(data.thrusters)) { + data.thrusters.forEach((v, i) => { if (i < 8) updateThrusterBar(i, v); }); + } + + if (data.topic === 'thrusters/control_mode') { + setLed('led-rot', !!data.hold_rotation); + setLed('led-depth', !!data.hold_depth); + setLed('led-angvel', !!data.ang_vel_control); + setLed('led-deplock',!!data.depth_lock); + } + + if (data.topic === 'bno/quat' && data.quat) { + const { w, x, y, z } = data.quat; + const { roll, pitch, yaw } = quatToEulerDeg(w, x, y, z); + document.getElementById('imu-roll').textContent = roll.toFixed(1); + document.getElementById('imu-pitch').textContent = pitch.toFixed(1); + document.getElementById('imu-yaw').textContent = yaw.toFixed(1); + } +}; + +// ── PID sliders ──────────────────────────────────────────────────── +function bindSlider(id, valId, decimals = 2) { + const input = document.getElementById(id); + const label = document.getElementById(valId); + input.addEventListener('input', () => { + label.textContent = parseFloat(input.value).toFixed(decimals); + }); +} + +bindSlider('rot-kp', 'rot-kp-val'); +bindSlider('rot-ki', 'rot-ki-val'); +bindSlider('rot-kd', 'rot-kd-val'); +bindSlider('rot-max', 'rot-max-val'); +bindSlider('depth-kp', 'depth-kp-val'); +bindSlider('depth-ki', 'depth-ki-val'); +bindSlider('depth-kd', 'depth-kd-val'); + +async function post(path, body) { + const opts = { method: 'POST' }; + if (body) { + opts.headers = { 'Content-Type': 'application/json' }; + opts.body = JSON.stringify(body); + } + const r = await fetch(path, opts); + return r.json(); +} + +function showPidStatus(msg, ok) { + const el = document.getElementById('pid-status'); + el.textContent = msg; + el.style.color = ok ? '#81c784' : '#e57373'; + setTimeout(() => { el.textContent = ''; }, 3000); +} + +document.getElementById('apply-rot-pid').onclick = async () => { + const params = { + rot_kp: parseFloat(document.getElementById('rot-kp').value), + rot_ki: parseFloat(document.getElementById('rot-ki').value), + rot_kd: parseFloat(document.getElementById('rot-kd').value), + rot_max_output:parseFloat(document.getElementById('rot-max').value), + }; + try { + const r = await post('/api/pid', params); + showPidStatus(r.ok ? '✓ Rotation PID applied' : ('Error: ' + r.error), r.ok); + } catch { showPidStatus('Network error', false); } +}; + +document.getElementById('apply-depth-pid').onclick = async () => { + const params = { + depth_kp: parseFloat(document.getElementById('depth-kp').value), + depth_ki: parseFloat(document.getElementById('depth-ki').value), + depth_kd: parseFloat(document.getElementById('depth-kd').value), + }; + try { + const r = await post('/api/pid', params); + showPidStatus(r.ok ? '✓ Depth PID applied' : ('Error: ' + r.error), r.ok); + } catch { showPidStatus('Network error', false); } +}; + +// ── Node control buttons ─────────────────────────────────────────── +document.getElementById('start-rovs').onclick = async () => { + const r = await post('/api/orangepi/start_nodes'); + alert(r.ok ? 'OrangePi nodes started' : 'Error: ' + r.error); +}; +document.getElementById('stop-rovs').onclick = async () => { + const r = await post('/api/orangepi/stop_nodes'); + alert(r.ok ? 'OrangePi nodes stopped' : 'Error: ' + r.error); +}; +document.getElementById('start-joy').onclick = async () => { + const r = await post('/api/windows/start_joy'); + alert(r.ok ? 'Joy node started' : 'Error: ' + r.error); +}; +document.getElementById('stop-joy').onclick = async () => { + const r = await post('/api/windows/stop_joy'); + alert(r.ok ? 'Joy node stopped' : 'Error: ' + r.error); +}; diff --git a/web/index.html b/web/index.html new file mode 100644 index 0000000..2c276c0 --- /dev/null +++ b/web/index.html @@ -0,0 +1,113 @@ + + + + + + ROV Control + + + +
+ ROV Control + ● Disconnected +
+ +
+ +
+
Camera stream placeholder
+
+

IMU Orientation

+
Roll--°
+
Pitch--°
+
Yaw--°
+
+
+ + +
+

Thruster Forces

+
+
+ + +
+ +
+

Control Modes

+
+ Hold Rotation +
+
+ Hold Depth +
+
+ Ang Vel Control +
+
+ Depth Lock +
+
+ +
+

Rotation PID Tuning

+
+ + + 1.00 +
+
+ + + 0.00 +
+
+ + + 0.00 +
+
+ + + 2.40 +
+ + +

Depth PID Tuning

+
+ + + 0.00 +
+
+ + + 0.00 +
+
+ + + 0.00 +
+ +
+
+ +
+

Node Control

+
+ + +
+
+ + +
+
+ +
+
+ + + + diff --git a/web/style.css b/web/style.css new file mode 100644 index 0000000..f5ced18 --- /dev/null +++ b/web/style.css @@ -0,0 +1,161 @@ +* { box-sizing: border-box; margin: 0; padding: 0; } +body { font-family: 'Segoe UI', Arial, sans-serif; background: #1a1a2e; color: #e0e0e0; font-size: 13px; } + +header { + background: #0d0d1a; + color: #fff; + padding: 10px 18px; + display: flex; + justify-content: space-between; + align-items: center; + border-bottom: 1px solid #333; + font-size: 15px; + font-weight: 600; +} + +.badge { font-size: 12px; padding: 3px 10px; border-radius: 12px; font-weight: 500; } +.badge.connected { background: #1b5e20; color: #a5d6a7; } +.badge.disconnected { background: #3a1a1a; color: #ef9a9a; } + +main { + display: grid; + grid-template-columns: 260px 1fr 300px; + gap: 12px; + padding: 12px; + height: calc(100vh - 42px); + overflow: hidden; +} + +section { background: #16213e; border-radius: 8px; padding: 12px; overflow-y: auto; } + +h3 { font-size: 12px; font-weight: 600; text-transform: uppercase; letter-spacing: 0.5px; color: #90caf9; margin-bottom: 10px; } + +/* ── Left column ── */ +#camera-placeholder { + background: #0d0d1a; + color: #555; + height: 180px; + display: flex; + align-items: center; + justify-content: center; + border-radius: 6px; + margin-bottom: 12px; + border: 1px dashed #333; + font-size: 12px; +} + +#imu-panel { } +.imu-row { + display: flex; + justify-content: space-between; + align-items: center; + padding: 5px 0; + border-bottom: 1px solid #222; +} +.imu-label { color: #888; width: 45px; } +.imu-val { font-size: 15px; font-weight: 700; color: #e0e0e0; font-family: monospace; } +.imu-unit { color: #666; font-size: 11px; } + +/* ── Center column: thruster bars ── */ +#thruster-bars { display: flex; flex-direction: column; gap: 6px; } + +.tbar { + display: grid; + grid-template-columns: 36px 1fr 42px; + align-items: center; + gap: 6px; +} +.tbar-label { color: #90caf9; font-size: 11px; font-family: monospace; font-weight: 600; } +.tbar-track { + position: relative; + height: 16px; + background: #0d0d1a; + border-radius: 3px; + overflow: hidden; +} +.tbar-center-line { + position: absolute; + left: 50%; + top: 0; + width: 1px; + height: 100%; + background: #333; +} +.tbar-fill { + position: absolute; + top: 0; + height: 100%; + transition: width 0.08s ease; +} +.tbar-fill.pos { background: #43a047; left: 50%; } +.tbar-fill.neg { background: #e53935; right: 50%; } +.tbar-val { font-family: monospace; font-size: 11px; color: #aaa; text-align: right; } + +/* ── Right column ── */ +#mode-panel { margin-bottom: 14px; } +.mode-row { display: flex; align-items: center; gap: 8px; padding: 5px 0; border-bottom: 1px solid #1e2a45; color: #ccc; } + +.led { + display: inline-block; + width: 10px; + height: 10px; + border-radius: 50%; + background: #2a2a2a; + border: 1px solid #444; + flex-shrink: 0; +} +.led.active { + background: #43a047; + border-color: #66bb6a; + box-shadow: 0 0 6px #43a04799; +} + +/* PID panel */ +#pid-panel { margin-bottom: 14px; } +.slider-row { + display: grid; + grid-template-columns: 56px 1fr 36px; + align-items: center; + gap: 6px; + margin-bottom: 7px; +} +.slider-row label { color: #90caf9; font-size: 11px; } +.slider-row input[type=range] { + -webkit-appearance: none; + appearance: none; + width: 100%; + height: 4px; + background: #0d0d1a; + border-radius: 2px; + outline: none; +} +.slider-row input[type=range]::-webkit-slider-thumb { + -webkit-appearance: none; + width: 14px; + height: 14px; + border-radius: 50%; + background: #42a5f5; + cursor: pointer; +} +.slider-val { font-family: monospace; font-size: 11px; color: #e0e0e0; text-align: right; } + +.status-msg { font-size: 11px; margin-top: 6px; min-height: 16px; color: #81c784; } + +/* Buttons */ +button { + border: none; + border-radius: 5px; + padding: 7px 12px; + cursor: pointer; + font-size: 12px; + font-weight: 500; + transition: opacity 0.15s; +} +button:hover { opacity: 0.85; } +button:active { opacity: 0.7; } +.btn-primary { background: #1565c0; color: #fff; width: 100%; margin-top: 4px; } +.btn-green { background: #2e7d32; color: #fff; flex: 1; } +.btn-red { background: #b71c1c; color: #fff; flex: 1; } + +#node-controls { } +.btn-row { display: flex; gap: 8px; margin-bottom: 8px; }