aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: update MATLAB example

Browse Source
This commit is contained in:
Iampete1 2020-05-29 15:22:19 +01:00 committed by Andrew Tridgell
parent fbb4df97a3
commit f5e98a6d69
4 changed files with 111 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m
View File

@ -61,8 +61,8 @@ copter.battery = battery;
copter.mass = 2; % (kg)
inertia = (2/5) * copter.mass * (0.45*0.2)^2; % (sphere)
copter.inertia = diag(ones(3,1)*inertia); % rotational inertia matrix (kgm^2)
copter.cd = [0.5,0.5,0.5];
copter.cd_ref_area = [1,1,1] * pi * (0.45*0.5)^2;
copter.cd = [0.5;0.5;0.5];
copter.cd_ref_area = [1;1;1] * pi * (0.45*0.5)^2;
save('Hexsoon','copter')

BIN
libraries/SITL/examples/JSON/MATLAB/Copter/Hexsoon.mat
View File

Binary file not shown.

134
libraries/SITL/examples/JSON/MATLAB/Copter/SIM_multicopter.m
View File

@ -19,18 +19,14 @@ 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';
max_timestep = 1/50;
% define init and time setup functions
init_function = @(state)init(state);
physics_function = @(pwm_in,state)physics_step(pwm_in,state,delta_t);
init_function = @init;
physics_function = @physics_step;
% setup connection
SITL_connector(target_ip,state,init_function,physics_function,delta_t);
SITL_connector(state,init_function,physics_function,max_timestep);
% Simulator model must take and return a structure with the felids:
% gyro(roll, pitch, yaw) (radians/sec) body frame
@ -46,17 +42,17 @@ 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
state.gyro = [0;0;0]; % (rad/sec)
state.dcm = diag([1,1,1]); % direction cosine matrix
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.bf_velo = [0;0;0]; % (m/s) body frame
end
% Take a physics time step
function state = physics_step(pwm_in,state,delta_t)
function state = physics_step(pwm_in,state)
% Calculate the dropped battery voltage, assume current draw from last step
state.copter.battery.current = sum([state.copter.motors.current]);
@ -86,7 +82,7 @@ for i = 1:numel(state.copter.motors)
w = motor.rpm * ((2*pi)/60); % convert to rad/sec
w1 = w + ((torque-prop_drag) / motor.prop.inertia) * delta_t;
w1 = w + ((torque-prop_drag) / motor.prop.inertia) * state.delta_t;
rps = w1 * (1/(2*pi));
@ -111,50 +107,90 @@ for i = 1:numel(state.copter.motors)
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;];
drag = sign(state.bf_velo) .* state.copter.cd .* state.copter.cd_ref_area .* 0.5 .* state.environment.density .* state.bf_velo.^2;
% Calculate the forces about the CG (N,E,D) (body frame)
force = [0,0,-sum([state.copter.motors.thrust])] - state.drag;
force = [0;0;-sum([state.copter.motors.thrust])] - drag;
% body frame accelerations (NED)
% estimate rotational drag
rotational_drag = 0.2 * sign(state.gyro) .* state.gyro.^2; % estimated to give a reasonable max rotation rate
% 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])] - rotational_drag;
state = update_dynamics(state,force,moments);
end
% integrate the acceleration resulting from the forces and moments to get the
% new state
function state = update_dynamics(state,force,moments)
rot_accel = (moments' / state.copter.inertia)';
state.gyro = state.gyro + rot_accel * state.delta_t;
% Constrain to 2000 deg per second, this is what typical sensors max out at
state.gyro = max(state.gyro,deg2rad(-2000));
state.gyro = min(state.gyro,deg2rad(2000));
% update the dcm and attitude
[state.dcm, state.attitude] = rotate_dcm(state.dcm,state.gyro * state.delta_t);
% body frame accelerations
state.accel = force / state.copter.mass;
% earth frame accelerations (NED)
acel_ef = state.accel * dcm;
acel_ef(3) = acel_ef(3) + state.gravity_mss;
accel_ef = state.dcm * state.accel;
accel_ef(3) = accel_ef(3) + state.gravity_mss;
state.velocity = state.velocity + acel_ef * delta_t;
state.position = state.position + state.velocity * delta_t;
% if we're on the ground, then our vertical acceleration is limited
% to zero. This effectively adds the force of the ground on the aircraft
if state.position(3) >= 0 && accel_ef(3) > 0
accel_ef(3) = 0;
end
% work out acceleration as seen by the accelerometers. It sees the kinematic
% acceleration (ie. real movement), plus gravity
state.accel = state.dcm' * (accel_ef + [0; 0; -state.gravity_mss]);
state.velocity = state.velocity + accel_ef * state.delta_t;
state.position = state.position + state.velocity * state.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];
state.velocity = [0;0;0];
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
% calculate the body frame velocity for drag calculation
state.bf_velo = state.dcm' * state.velocity;
end
function [dcm, euler] = rotate_dcm(dcm, ang)
% rotate
delta = [dcm(1,2) * ang(3) - dcm(1,3) * ang(2), dcm(1,3) * ang(1) - dcm(1,1) * ang(3), dcm(1,1) * ang(2) - dcm(1,2) * ang(1);
dcm(2,2) * ang(3) - dcm(2,3) * ang(2), dcm(2,3) * ang(1) - dcm(2,1) * ang(3), dcm(2,1) * ang(2) - dcm(2,2) * ang(1);
dcm(3,2) * ang(3) - dcm(3,3) * ang(2), dcm(3,3) * ang(1) - dcm(3,1) * ang(3), dcm(3,1) * ang(2) - dcm(3,2) * ang(1)];
dcm = dcm + delta;
% normalise
a = dcm(1,:);
b = dcm(2,:);
error = a * b';
t0 = a - (b *(0.5 * error));
t1 = b - (a *(0.5 * error));
t2 = cross(t0,t1);
dcm(1,:) = t0 * (1/norm(t0));
dcm(2,:) = t1 * (1/norm(t1));
dcm(3,:) = t2 * (1/norm(t2));
% calculate euler angles
euler = [atan2(dcm(3,2),dcm(3,3)); -asin(dcm(3,1)); atan2(dcm(2,1),dcm(1,1))];
end

47
libraries/SITL/examples/JSON/MATLAB/SITL_connector.m
View File

@ -2,7 +2,7 @@
% Toolbox 2.0.6 by Peter Rydesäter
% https://uk.mathworks.com/matlabcentral/fileexchange/345-tcp-udp-ip-toolbox-2-0-6
function SITL_connector(target_ip,state,init_function,physics_function,delta_t)
function SITL_connector(state,init_function,physics_function,max_timestep)
try
pnet('closeall') % close any connections left open from past runs
catch
@ -31,17 +31,15 @@ pnet(u,'setreadtimeout',0);
frame_time = tic;
frame_count = 0;
physics_time_us = 0;
physics_time_s = 0;
last_SITL_frame = -1;
print_frame_count = 1000;
print_frame_count = 1000; % print the fps every x frames
connected = false;
bytes_read = 4 + 4 + 16*2;
time_correction = 1;
bytes_read = 4 + 4 + 16*2; % the number of bytes received in each packet
while true
mat_time = tic;
% Wait for data
while true
while true
in_bytes = pnet(u,'readpacket',bytes_read);
if in_bytes > 0
break;
@ -59,14 +57,23 @@ while true
continue;
end
% read in the current SITL frame and PWM
% read in data from AP
magic = pnet(u,'read',1,'UINT16','intel');
frame_rate = double(pnet(u,'read',1,'UINT16','intel'));
SITL_frame = pnet(u,'read',1,'UINT32','intel');
speed_up = double(pnet(u,'read',1,'SINGLE','intel'));
pwm_in = double(pnet(u,'read',16,'UINT16','intel'))';
% check the magic value is what expect
if magic ~= 18458
warning('incorrect magic value')
continue;
end
% Check if the fame is in expected order
if SITL_frame < last_SITL_frame
% Controller has reset, reset physics also
state = init_function(state);
connected = false;
fprintf('Controller reset\n')
elseif SITL_frame == last_SITL_frame
% duplicate frame, skip
@ -76,19 +83,21 @@ while true
fprintf('Missed %i input frames\n',SITL_frame - last_SITL_frame - 1)
end
last_SITL_frame = SITL_frame;
physics_time_us = physics_time_us + delta_t * 10^6;
state.delta_t = min(1/frame_rate,max_timestep);
physics_time_s = physics_time_s + state.delta_t;
if ~connected
connected = true;
fprintf('Connected\n')
[ip, port] = pnet(u,'gethost');
fprintf('Connected to %i.%i.%i.%i:%i\n',ip,port)
end
frame_count = frame_count + 1;
% Do a physics time step
% do a physics time step
state = physics_function(pwm_in,state);
% build structure representing the JSON string to be sent
JSON.timestamp = physics_time_us;
JSON.timestamp = physics_time_s;
JSON.imu.gyro = state.gyro;
JSON.imu.accel_body = state.accel;
JSON.position = state.position;
@ -97,22 +106,14 @@ while true
% Report to AP
pnet(u,'printf',sprintf('\n%s\n',jsonencode(JSON)));
pnet(u,'writepacket',target_ip,9003);
pnet(u,'writepacket');
% print a fps and runtime update
if rem(frame_count,print_frame_count) == 0
total_time = toc(frame_time);
frame_time = tic;
time_ratio = (print_frame_count*delta_t)/total_time;
time_ratio = (print_frame_count*state.delta_t)/total_time;
fprintf("%0.2f fps, %0.2f%% of realtime\n",print_frame_count/total_time,time_ratio*100)
time_ratio = speed_up / time_ratio;
if time_ratio < 1.1 && time_ratio > 0.9
time_correction = time_correction * 0.95 + time_ratio * 0.05;
end
end
while toc(mat_time) < (delta_t / speed_up) / time_correction
end
end