mirror of https://github.com/ArduPilot/ardupilot
SITL: update MATLAB example
This commit is contained in:
Iampete1
Andrew Tridgell
parent
fbb4df97a3
commit
f5e98a6d69
|
|
@ -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')
|
||||
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue