% MIT License % % Copyright (c) 2022 Jongrae.K % % Permission is hereby granted, free of charge, to any person obtaining a copy % of this software and associated documentation files (the "Software"), to deal % in the Software without restriction, including without limitation the rights % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell % copies of the Software, and to permit persons to whom the Software is % furnished to do so, subject to the following conditions: % % The above copyright notice and this permission notice shall be included in all % copies or substantial portions of the Software. % % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE % SOFTWARE. clear; %% simulation parameters init_time = 0; % [s] final_time = 10.0; % [s] time_interval = [init_time final_time]; J_inertia = [0.005 -0.001 0.004; -0.001 0.006 -0.002; 0.004 -0.002 0.004]; % vehicle moment of inertia [kg m^2] J_inv = inv(J_inertia); C_T = 8.8e-7; % motor thruster coefficient [N/(rad/s)^2] C_D = 11.3e-8; % motor drag coefficient [Nm/(rad/s)^2] L_arm = 0.127; % length from the centre of quadcopter to the motor [m] quadcopter_uav = {J_inertia, J_inv, C_T, C_D, L_arm}; q0 = [0 0 0 1]'; % initial quaternion w0 = [0 0 0]'; % initial angular velocity %----------------------Added Part--------------------------------------- % first-order motor model tau_motor = 0.01; %[s] w_motor = [0 0 0 0]'; % [initial 4-motor angular velocity] %----------------------------------------------------------------------- %----------------------Modified Part------------------------------------ state_0 = [q0; w0; w_motor]; % states including q0 and omega0 % use global variables only for saving values global global_motor_time_FM_all; global_motor_time_FM_all = []; % minimum time interval for saving values to the global dt_save = 0.05; %[s] %% simulation ode_options = odeset('RelTol',1e-6,'AbsTol',1e-9, 'MaxStep', 0.01); [tout,state_out] = ode45(@(time,state) dqdt_dwdt(time,state,quadcopter_uav,dt_save,tau_motor), ... time_interval, state_0, ode_options); qout = state_out(:,1:4); wout = state_out(:,5:7); w_motor_out = state_out(:,8:11); time_Motor = global_motor_time_FM_all(:,1); Force_Motor = global_motor_time_FM_all(:,2); Torque_Motor = global_motor_time_FM_all(:,3:5); % clear all global variables clearvars -global %% plot figure; subplot(211) plot(tout,qout(:,1),'b-',tout,qout(:,2),'r--',tout,qout(:,3),'g-.',tout,qout(:,4),'m:') set(gca,'FontSize',14); ylabel('quaternion'); xlabel('time [s]'); legend('q_1','q_2','q_3','q_4'); subplot(212) plot(tout,wout(:,1),'r-',tout,wout(:,2),'b--',tout,wout(:,3),'m-.') set(gca,'FontSize',14); ylabel('\omega [rad/s]'); xlabel('time [s]'); legend('\omega_1','\omega_2','\omega_3'); figure; subplot(211); plot(time_Motor,Force_Motor) set(gca,'FontSize',14); ylabel('Force [N]'); xlabel('time [s]'); axis([init_time final_time -15 0]); subplot(212); plot(time_Motor,Torque_Motor(:,1),'r-',time_Motor,Torque_Motor(:,2),'b--', ... time_Motor,Torque_Motor(:,3),'m-.') set(gca,'FontSize',14); ylabel('Torque [Nm]'); xlabel('time [s]'); legend('M_1','M_2','M_3'); % motor angular velocity figure; plot(tout, w_motor_out*60/(2*pi)) ylabel('Motor Angular Velocity [rpm]') xlabel('time [s]') legend('\omega_m^{forward}','\omega_m^{backward}','\omega_m^{left}','\omega_m^{right}') %% functions % -----------------Updated Part: tau_motor added in the input parameters-- function dstate_dt = dqdt_dwdt(time,state,quadcopter_uav,dt_save,tau_motor) %------------------------------------------------------------------------- global global_motor_time_FM_all; q_current = state(1:4); q_current = q_current(:)/norm(q_current); w_current = state(5:7); w_current = w_current(:); %------------------Updated Part: motor state--- w_motor_current = state(8:11); w_motor_current = w_motor_current(:); %--------------------------------------------- J_inertia = quadcopter_uav{1}; inv_J = quadcopter_uav{2}; C_T = quadcopter_uav{3}; C_D = quadcopter_uav{4}; L_arm = quadcopter_uav{5}; %-------------------------------- % Begin: this part is controller %-------------------------------- M_Desired = [ 0.00001+0.0005*sin(2*time); -0.00002+0.0001*cos(0.75*time); -0.0001]; % [Nm] mg = 10; %[N] F_M_desired = [-mg; M_Desired]; w_motor_fblr_squared_desired = propeller_motor_FM2w_conversion(F_M_desired, ... C_T, C_D, L_arm); w_motor_fblr_desired = sqrt(w_motor_fblr_squared_desired); %-------------------------------- % End: this part is controller %-------------------------------- %------------------------added part---------------------------------- dw_motor_dt = (-w_motor_current + w_motor_fblr_desired)/tau_motor; % Motor Force & Torque FM_Motor = propeller_motor_actuator(C_T, C_D, L_arm, w_motor_current); % updated part M_torque = FM_Motor(2:4); if time < 1e-200 global_motor_time_FM_all = [time FM_Motor(:)']; elseif time > global_motor_time_FM_all(end,1)+dt_save global_motor_time_FM_all = [global_motor_time_FM_all; time FM_Motor(:)']; end % Kinematics & Dynamics dqdt = dqdt_attitude_kinematics(q_current,w_current); dwdt = dwdt_attitude_dynamics(w_current, J_inertia, inv_J, M_torque); dstate_dt = [dqdt(:); dwdt(:); dw_motor_dt]; % updated part end function dqdt = dqdt_attitude_kinematics(q_true,w_true) q_true = q_true(:); w_true = w_true(:); wx = [ 0 -w_true(3) w_true(2); w_true(3) 0 -w_true(1); -w_true(2) w_true(1) 0]; Omega = [ -wx w_true; -w_true' 0]; dqdt = 0.5*Omega*q_true; end function dwdt = dwdt_attitude_dynamics(w_true, J_inertia, inv_J_inertia, M_torque) w_true = w_true(:); Jw = J_inertia*w_true; Jw_dot = -cross(w_true,Jw) + M_torque(:); dwdt = inv_J_inertia*Jw_dot; end function FM_Motor = propeller_motor_actuator(C_T,C_D,L_arm,w_motor) % this part is replaced by current motor angular velocity % assume perfect motor angular velocity control % w_motor = w_command(:); F_fblr = C_T*(w_motor.^2); tau_fblr = C_D*(w_motor.^2); F_motor = -sum(F_fblr); M_motor = [ L_arm*(F_fblr(3)-F_fblr(4)); L_arm*(F_fblr(1)-F_fblr(2)); sum(tau_fblr(1:2))-sum(tau_fblr(3:4))]; FM_Motor = [F_motor; M_motor]; end function w_motor_fblr_squared_desired = propeller_motor_FM2w_conversion(F_M_desired, C_T, C_D, L_arm) F_M_desired = F_M_desired(:); inv_C_T = 1/C_T; inv_C_D = 1/C_D; inv_2_L_C_T = 2/(L_arm*C_T); Conv_Mat = 0.25*[ -inv_C_T 0 inv_2_L_C_T inv_C_D; -inv_C_T 0 -inv_2_L_C_T inv_C_D; -inv_C_T inv_2_L_C_T 0 -inv_C_D; -inv_C_T -inv_2_L_C_T 0 -inv_C_D]; w_motor_fblr_squared_desired = Conv_Mat*F_M_desired; w_motor_fblr_squared_desired(w_motor_fblr_squared_desired<0) = 0; end