latest updates

.idea/workspace.xml

+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ChangeListManager">
+    <list default="true" id="5a4d9477-ac1c-4c56-8feb-a6533584fa5a" name="Changes" comment="Demos">
+      <change beforePath="$PROJECT_DIR$/controls/Sim-nonlinear-quad-example/Sim-nonlinear-quad-example/lqr_quad_gains.m" beforeDir="false" afterPath="$PROJECT_DIR$/controls/Sim-nonlinear-quad-example/Sim-nonlinear-quad-example/lqr_quad_gains.m" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/controls/Sim-nonlinear-quad-example/Sim-nonlinear-quad-example/measured_flight_path.mat" beforeDir="false" afterPath="$PROJECT_DIR$/controls/Sim-nonlinear-quad-example/Sim-nonlinear-quad-example/measured_flight_path.mat" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/controls/Sim-nonlinear-quad-example/Sim-nonlinear-quad-example/quad_params.m" beforeDir="false" afterPath="$PROJECT_DIR$/controls/Sim-nonlinear-quad-example/Sim-nonlinear-quad-example/quad_params.m" afterDir="false" />
+    </list>
+    <option name="SHOW_DIALOG" value="false" />
+    <option name="HIGHLIGHT_CONFLICTS" value="true" />
+    <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
+    <option name="LAST_RESOLUTION" value="IGNORE" />
+  </component>
+  <component name="FileTemplateManagerImpl">
+    <option name="RECENT_TEMPLATES">
+      <list>
+        <option value="Python Script" />
+      </list>
+    </option>
+  </component>
+  <component name="Git.Settings">
+    <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
+  </component>
+  <component name="MarkdownSettingsMigration">
+    <option name="stateVersion" value="1" />
+  </component>
+  <component name="ProjectId" id="2LtgSLVpndDdx4GLkO3vm9q6PkF" />
+  <component name="ProjectViewState">
+    <option name="hideEmptyMiddlePackages" value="true" />
+    <option name="showLibraryContents" value="true" />
+  </component>
+  <component name="PropertiesComponent">
+    <property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
+    <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
+    <property name="WebServerToolWindowFactoryState" value="false" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
+    <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
+  </component>
+  <component name="RunManager" selected="Python.basic_hover_demo_matlab">
+    <configuration name="basic_hover_demo" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
+      <module name="MicroCART" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/crazyflie_demos" />
+      <option name="IS_MODULE_SDK" value="true" />
+      <option name="ADD_CONTENT_ROOTS" value="true" />
+      <option name="ADD_SOURCE_ROOTS" value="true" />
+      <EXTENSION ID="PythonCoverageRunConfigurationExtension" runner="coverage.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/crazyflie_demos/basic_hover_demo.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
+    <configuration name="basic_hover_demo_matlab" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
+      <module name="MicroCART" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/crazyflie_demos" />
+      <option name="IS_MODULE_SDK" value="true" />
+      <option name="ADD_CONTENT_ROOTS" value="true" />
+      <option name="ADD_SOURCE_ROOTS" value="true" />
+      <EXTENSION ID="PythonCoverageRunConfigurationExtension" runner="coverage.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/crazyflie_demos/basic_hover_demo_matlab.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
+    <configuration name="swarm_circles" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
+      <module name="MicroCART" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/crazyflie_demos" />
+      <option name="IS_MODULE_SDK" value="true" />
+      <option name="ADD_CONTENT_ROOTS" value="true" />
+      <option name="ADD_SOURCE_ROOTS" value="true" />
+      <EXTENSION ID="PythonCoverageRunConfigurationExtension" runner="coverage.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/crazyflie_demos/swarm_circles.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
+    <recent_temporary>
+      <list>
+        <item itemvalue="Python.basic_hover_demo_matlab" />
+        <item itemvalue="Python.swarm_circles" />
+        <item itemvalue="Python.basic_hover_demo" />
+      </list>
+    </recent_temporary>
+  </component>
+  <component name="SpellCheckerSettings" RuntimeDictionaries="0" Folders="0" CustomDictionaries="0" DefaultDictionary="application-level" UseSingleDictionary="true" transferred="true" />
+  <component name="TaskManager">
+    <task active="true" id="Default" summary="Default task">
+      <changelist id="5a4d9477-ac1c-4c56-8feb-a6533584fa5a" name="Changes" comment="" />
+      <created>1676695792603</created>
+      <option name="number" value="Default" />
+      <option name="presentableId" value="Default" />
+      <updated>1676695792603</updated>
+      <workItem from="1676695794336" duration="10721000" />
+      <workItem from="1676734874720" duration="2646000" />
+      <workItem from="1676852864787" duration="19000" />
+    </task>
+    <task id="LOCAL-00001" summary="Demos">
+      <created>1676706099193</created>
+      <option name="number" value="00001" />
+      <option name="presentableId" value="LOCAL-00001" />
+      <option name="project" value="LOCAL" />
+      <updated>1676706099194</updated>
+    </task>
+    <task id="LOCAL-00002" summary="Demos">
+      <created>1676706912809</created>
+      <option name="number" value="00002" />
+      <option name="presentableId" value="LOCAL-00002" />
+      <option name="project" value="LOCAL" />
+      <updated>1676706912809</updated>
+    </task>
+    <option name="localTasksCounter" value="3" />
+    <servers />
+  </component>
+  <component name="TypeScriptGeneratedFilesManager">
+    <option name="version" value="3" />
+  </component>
+  <component name="Vcs.Log.Tabs.Properties">
+    <option name="TAB_STATES">
+      <map>
+        <entry key="MAIN">
+          <value>
+            <State />
+          </value>
+        </entry>
+      </map>
+    </option>
+  </component>
+  <component name="VcsManagerConfiguration">
+    <MESSAGE value="Demos" />
+    <option name="LAST_COMMIT_MESSAGE" value="Demos" />
+  </component>
+  <component name="com.intellij.coverage.CoverageDataManagerImpl">
+    <SUITE FILE_PATH="coverage/MicroCART$swarm_circles.coverage" NAME="swarm_circles Coverage Results" MODIFIED="1676705981323" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="true" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/crazyflie_demos" />
+    <SUITE FILE_PATH="coverage/MicroCART$basic_hover_demo_matlab.coverage" NAME="basic_hover_demo_matlab Coverage Results" MODIFIED="1676741034853" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="true" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/crazyflie_demos" />
+    <SUITE FILE_PATH="coverage/MicroCART$basic_hover_demo.coverage" NAME="basic_hover_demo Coverage Results" MODIFIED="1676696324064" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="true" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/crazyflie_demos" />
+  </component>
+</project>
\ No newline at end of file

controls/Sim-nonlinear-quad-example/Sim-nonlinear-quad-example/compute_quad_wrapper_pid.asv

+function [y, X, U, tspan, pid_states, pid_gains] = compute_quad_wrapper_pid(tstep, X, U, X_ref,F_hover, tspan, pid_states, pid_gains)
+
+% tspan=[0 tstep]; 
+X_init = X(end,:); % Initialize state from where last ode left off
+tspan=[tspan(2) tspan(2)+tstep];  % Advance to next time interval
+
+pid_states_dumb = zeros(12,1);
+pid_states_dumb = pid_states;
+alt_error_sum = pid_states(1);
+alt_error_prev = 
+y_error_sum
+y_error_prev
+phi_error_sum
+phi_error_prev
+x_error_sum
+x_error_prev
+theta_error_sum
+theta_error_prev
+phi_setpoint
+
+
+[alt_error_sum, alt_error_prev, y_error_sum, y_error_prev, phi_error_sum, phi_error_prev, x_error_sum, x_error_prev, theta_error_sum, theta_error_prev, phi_setpoint, theta_setpoint] = pid_states(1:12);
+[Kp_alt, Ki_alt, Kd_alt, Kp_y, Ki_y, Kd_y, Kp_phi, Ki_phi, Kd_phi, Kp_x, Ki_x, Kd_x, Kp_theta, Ki_theta, Kd_theta] = pid_gains(1:15);
+
+[~, X] = ode45(@(t,X) compute_quad(t,X,U), tspan, X_init); % Simulate
+
+% Xplot = [Xplot; [t, X]]; % log data for plotting later (this is the main thing that contributes to time
+
+% Compute feedback (i.e. Force inputs (U) in terms of state (X) )
+%Get current state;
+X_c = X(end,:);
+
+  % Altitude PID control
+% alt_c = -X_c(3); % Current Altitude is -Z, as Z-axis points down
+% [~, ~, ~] = pid_ctrl(1,2,3,4,5,6,7);
+% [alt_cmd, alt_error_sum, alt_error_prev] = pid_ctrl(X_ref(3),alt_c, ...
+%     Kp_alt, Ki_alt, Kd_alt, alt_error_sum, alt_error_prev);
+% U(1) = F_hover + alt_cmd;
+% 
+%   % Y position (controlled by phi angle)
+% y_c = X_c(2); % Current Y position
+% [y_cmd, y_error_sum, y_error_prev] = pid_ctrl(X_ref(2),y_c, ...
+%     Kp_y, Ki_y, Kd_y, y_error_sum, y_error_prev);
+% phi_setpoint = y_cmd; % Use Y correction command to drive phi setpoint
+% 
+%   % Roll (phi) angle
+% phi_c = X_c(4); % Current angle phi
+% [phi_cmd, phi_error_sum, phi_error_prev] = pid_ctrl(X_ref(4),phi_c, ...
+%     Kp_phi, Ki_phi, Kd_phi, phi_error_sum, phi_error_prev);
+% U(2) = phi_cmd; %phi rotation (roll), impacts Y-position @ Yaw = 0
+% 
+%   % X position (controlled by theta angle)
+% x_c = X_c(1); % Current X position
+% [x_cmd, x_error_sum, x_error_prev] = pid_ctrl(X_ref(1),x_c, ...
+%     Kp_x, Ki_x, Kd_x, x_error_sum, x_error_prev);
+% theta_setpoint = -x_cmd; % Use X correction command to drive -theta setpoint
+% 
+%   % Pitch (theta) angle
+% theta_c = X_c(5); % Current angle theta
+% [theta_cmd, theta_error_sum, theta_error_prev] = pid_ctrl(X_ref(5),theta_c, ...
+%     Kp_theta, Ki_theta, Kd_theta, theta_error_sum, theta_error_prev);
+% U(3) = theta_cmd; %theta rotation (pitch), impacts X-position @ Yaw = 0
+
+U = [0 0 0 0];
+
+pid_states = [alt_error_sum alt_error_prev y_error_sum y_error_prev phi_error_sum phi_error_prev x_error_sum x_error_prev theta_error_sum theta_error_prev phi_setpoint theta_setpoint];
+pid_gains = [Kp_alt Ki_alt Kd_alt Kp_y Ki_y Kd_y Kp_phi Ki_phi Kd_phi Kp_x Ki_x Kd_x Kp_theta Ki_theta Kd_theta];
+
+euler_angles = [X_c(6) X_c(5) X_c(4)];
+displacement = [X_c(1) X_c(2) X_c(3)];
+
+y = [euler_angles displacement]; 

controls/Sim-nonlinear-quad-example/Sim-nonlinear-quad-example/pid_ctrl.asv

+function [cmd, error_sum, error_prev] = pid_ctrl(setpoint,state, Kp, Ki, Kd, goal_error_sum, goal_error_prev)
+% Name: pid_ctrl
+% Authour: Phillip Jones (07/20/2022)
+% Description: Compute a PID control law
+% PID correction cmd = Kp*error + Ki*error_sum + Kd*error_diff
+%
+
+coder.extrinsic('get_param');
+x = double(0);
+x = get_param(<modelname/block>,'Value');
+
+% Compute PID error terms
+goal_error = setpoint - state;  % Current error
+goal_error_sum = goal_error_sum + goal_error; % Error sum
+goal_error_diff1 = goal_error - goal_error_prev; % Error difference
+
+% Compute PID correction command
+pid_cmd = Kp*goal_error + Ki*goal_error_sum1 + Kd*goal_error_diff1;
+
+% Return results
+cmd = pid_cmd;
+error_sum = goal_error_sum1;  % Use as input on next pid function call
+error_prev = goal_error;     % Use as input on next pid function call 
+end
\ No newline at end of file