You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
160 lines
5.7 KiB
160 lines
5.7 KiB
clc |
|
clearvars |
|
close all |
|
addpath(genpath('../../MATLAB')) |
|
|
|
% Physics of a multi copter |
|
|
|
% load in the parameters for a frame, generated by Copter.m |
|
try |
|
state = load('Hexsoon','copter'); |
|
catch |
|
run('Copter.m') |
|
fprintf('Could not find Hexsoon.mat file, running copter.m\n') |
|
return |
|
end |
|
|
|
% Setup environmental conditions |
|
state.environment.density = 1.225; % (kg/m^3) |
|
state.gravity_mss = 9.80665; % (m/s^2) |
|
|
|
% Setup the time step size for the Physics model |
|
delta_t = 1/400; |
|
|
|
% This is the ip that SITL is running at |
|
target_ip = '127.0.0.1'; |
|
target_ip = '192.168.194.97'; |
|
|
|
% define init and time setup functions |
|
init_function = @(state)init(state); |
|
physics_function = @(pwm_in,state)physics_step(pwm_in,state,delta_t); |
|
|
|
% setup connection |
|
SITL_connector(target_ip,state,init_function,physics_function,delta_t); |
|
|
|
% Simulator model must take and return a structure with the felids: |
|
% gyro(roll, pitch, yaw) (radians/sec) body frame |
|
% attitude(roll, pitch yaw) (radians) |
|
% accel(north, east, down) (m/s^2) body frame |
|
% velocity(north, east,down) (m/s) earth frame |
|
% position(north, east, down) (m) earth frame |
|
% the structure can have any other felids required for the physics model |
|
|
|
% init values |
|
function state = init(state) |
|
for i = 1:numel(state.copter.motors) |
|
state.copter.motors(i).rpm = 0; |
|
state.copter.motors(i).current = 0; |
|
end |
|
state.gyro = [0,0,0]; % (rad/sec) |
|
state.attitude = [0,0,0]; % (radians) (roll, pitch, yaw) |
|
state.accel = [0,0,0]; % (m/s^2) body frame |
|
state.velocity = [0,0,0]; % (m/s) earth frame |
|
state.position = [0,0,0]; % (m) earth frame |
|
state.drag = [0,0,0]; % (N) body frame drag |
|
state.rotational_drag = [0,0,0]; % (N/m) body frame rotational drag |
|
end |
|
|
|
% Take a physics time step |
|
function state = physics_step(pwm_in,state,delta_t) |
|
|
|
% Calculate the dropped battery voltage, assume current draw from last step |
|
state.copter.battery.current = sum([state.copter.motors.current]); |
|
state.copter.battery.dropped_voltage = state.copter.battery.voltage - state.copter.battery.resistance * state.copter.battery.current; |
|
|
|
% Calculate the torque and thrust, assume RPM is last step value |
|
for i = 1:numel(state.copter.motors) |
|
motor = state.copter.motors(i); |
|
|
|
% Calculate the throttle |
|
throttle = (pwm_in(motor.channel) - 1100) / 800; |
|
throttle = max(throttle,0); |
|
throttle = min(throttle,1); |
|
|
|
% effective voltage |
|
voltage = throttle * state.copter.battery.dropped_voltage; |
|
|
|
% Take the RPM from the last step to calculate the new |
|
% torque and current |
|
Kt = 1/(motor.electrical.kv * ( (2*pi)/60) ); |
|
|
|
% rpm equation rearranged for current |
|
current = ((motor.electrical.kv * voltage) - motor.rpm) / ((motor.electrical.resistance + motor.esc.resistance) * motor.electrical.kv); |
|
torque = current * Kt; |
|
|
|
prop_drag = motor.prop.PConst * state.environment.density * (motor.rpm/60)^2 * motor.prop.diameter^5; |
|
|
|
w = motor.rpm * ((2*pi)/60); % convert to rad/sec |
|
|
|
w1 = w + ((torque-prop_drag) / motor.prop.inertia) * delta_t; |
|
|
|
rps = w1 * (1/(2*pi)); |
|
|
|
% can never have negative rps |
|
rps = max(rps,0); |
|
|
|
% Calculate the thrust (with fudge factor!) |
|
thrust = 2.2 * motor.prop.TConst * state.environment.density * rps^2 * motor.prop.diameter^4; |
|
|
|
% calculate resulting moments |
|
moment_roll = thrust * motor.location(1); |
|
moment_pitch = thrust * motor.location(2); |
|
moment_yaw = -torque * motor.direction; |
|
|
|
% Update main structure |
|
state.copter.motors(i).torque = torque; |
|
state.copter.motors(i).current = current; |
|
state.copter.motors(i).rpm = rps * 60; |
|
state.copter.motors(i).thrust = thrust; |
|
state.copter.motors(i).moment_roll = moment_roll; |
|
state.copter.motors(i).moment_pitch = moment_pitch; |
|
state.copter.motors(i).moment_yaw = moment_yaw; |
|
end |
|
|
|
% Update attitude, moments to rotational acceleration to rotational velocity to attitude |
|
moments = [-sum([state.copter.motors.moment_roll]),sum([state.copter.motors.moment_pitch]),sum([state.copter.motors.moment_yaw])] - state.rotational_drag; |
|
ang_acel = moments / state.copter.inertia; |
|
state.gyro = state.gyro + ang_acel * delta_t; |
|
state.attitude = state.attitude + state.gyro * delta_t; |
|
|
|
% Calculate a dcm, see https://github.com/ArduPilot/ardupilot/blob/f5320e88161a17e78ffc969bf7ce0bec48adbe7a/libraries/AP_Math/matrix3.cpp#L26 |
|
cp = cos(state.attitude(2)); |
|
sp = sin(state.attitude(2)); |
|
sr = sin(state.attitude(1)); |
|
cr = cos(state.attitude(1)); |
|
sy = sin(state.attitude(3)); |
|
cy = cos(state.attitude(3)); |
|
|
|
dcm = [cp * cy, cp * sy, -sp;... |
|
(sr * sp * cy) - (cr * sy), (sr * sp * sy) + (cr * cy), sr * cp;... |
|
(cr * sp * cy) + (sr * sy), (cr * sp * sy) - (sr * cy), cr * cp;]; |
|
|
|
% Calculate the forces about the CG (N,E,D) (body frame) |
|
force = [0,0,-sum([state.copter.motors.thrust])] - state.drag; |
|
|
|
% body frame accelerations (NED) |
|
state.accel = force / state.copter.mass; |
|
|
|
% earth frame accelerations (NED) |
|
acel_ef = state.accel * dcm; |
|
acel_ef(3) = acel_ef(3) + state.gravity_mss; |
|
|
|
state.velocity = state.velocity + acel_ef * delta_t; |
|
state.position = state.position + state.velocity * delta_t; |
|
|
|
% make sure we can't go underground (NED so underground is positive) |
|
if state.position(3) >= 0 |
|
state.position(3) = 0; |
|
state.velocity = [0,0,0]; |
|
state.accel = [0,0,-state.gravity_mss]; |
|
state.gyro = [0,0,0]; |
|
end |
|
|
|
% caculate the body frame velocity and resulting drag |
|
bf_velo = (state.velocity / dcm); |
|
state.drag = sign(bf_velo) .* state.copter.cd .* state.copter.cd_ref_area .* 0.5 .* state.environment.density .* bf_velo.^2; |
|
|
|
% estimate rotational drag (mostly for yaw) |
|
state.rotational_drag = 0.3 * sign(state.gyro) .* state.gyro.^2; % estimated to give a reasonable max rotation rate |
|
|
|
end
|
|
|