From c69259183bfbe8f2bbb8de161a64a31f77c9b1d0 Mon Sep 17 00:00:00 2001 From: jackbogatz Date: Tue, 3 Dec 2024 17:48:38 -0500 Subject: [PATCH] Added phi adjustments and theta adjustments in controller --- include/MissionConstants.hpp | 1 + src/Controller.cpp | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/include/MissionConstants.hpp b/include/MissionConstants.hpp index 7fb6b0c..556484a 100644 --- a/include/MissionConstants.hpp +++ b/include/MissionConstants.hpp @@ -44,6 +44,7 @@ namespace MissionConstants { const int kTvcYPin = 18; const std::string kKMatrixFile = "k_matrix.csv"; const int kNumberControllerGains = 10; + const double weights_control_velocity = 0; // Ignition constants, TODO: USER EDIT PRE-FlIGHT const int kIgnitionPin = 6; diff --git a/src/Controller.cpp b/src/Controller.cpp index 6bf27fd..b96e787 100644 --- a/src/Controller.cpp +++ b/src/Controller.cpp @@ -61,6 +61,12 @@ void Controller::stabilizeAtOffset(Navigation& navigation, double current_time, // Create a matrix to store the returned stateEstimate from getNavigation() and store the yaw value into a varible Eigen::Matrix stateEstimate = navigation.GetNavigation(); + + // phi_adjusted = phi - v_y*weights_control_velocity + stateEstimate[6] = stateEstimate[6] - stateEstimate[4]*MissionConstants::weights_control_velocity; + + // theta_adjusted = theta - v_x*weights_control_velocity + stateEstimate[7] = stateEstimate[7] - stateEstimate[3]*MissionConstants::weights_control_velocity; /* WE DON'T NEED THIS CODE FOR RIGHT NOW. X AND Y VELOCITY WILL BE 0 double yaw = stateEstimate(8); @@ -80,6 +86,7 @@ void Controller::stabilizeAtOffset(Navigation& navigation, double current_time, x_control.segment(4,2) = stateEstimate.segment(6,2); x_control.segment(6,2) = stateEstimate.segment(9,2); + // Extract roll and pitch from stateEstimate, and put current integral step into euler_queue std::vector currentIntegralStep = {stateEstimate(6)*loopTime, stateEstimate(7)*loopTime}; euler_queue.push_back(currentIntegralStep);