zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

SITL: examples: JSON: add MATLAB example

c415-sdk
Iampete1 5 years ago committed by Andrew Tridgell
parent
fa712082d7
commit
13965133b0
6 changed files with 449 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 132
      libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m
  2. BIN
      libraries/SITL/examples/JSON/MATLAB/Copter/Hexsoon.mat
  3. 160
      libraries/SITL/examples/JSON/MATLAB/Copter/SIM_multicopter.m
  4. 3
      libraries/SITL/examples/JSON/MATLAB/Copter/readme.md
  5. 118
      libraries/SITL/examples/JSON/MATLAB/SITL_connector.m
  6. 36
      libraries/SITL/examples/JSON/MATLAB/readme.md

132
libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m
Unescape View File

@ -0,0 +1,132 @@ @@ -0,0 +1,132 @@
clc
clear
close all
% ArduCopter SITL example vehicle
% This sets up the vehicle properties that are used in the simulation
% simple 450 size x quad based on Hexsoon
% setup the motors
% locations (xyz) in m
motor(1).location = [[sind(45),cosd(45)]*450*0.5,0] * 0.001; % front right
motor(2).location = [[sind(135),cosd(135)]*450*0.5,0] * 0.001; % rear right
motor(3).location = [[sind(225),cosd(225)]*450*0.5,0] * 0.001; % rear left
motor(4).location = [[sind(315),cosd(315)]*450*0.5,0] * 0.001; % front left
% PWM output to use
motor(1).channel = 1;
motor(2).channel = 4;
motor(3).channel = 2;
motor(4).channel = 3;
% rotation direction: 1 = cw, -1 = ccw
motor(1).direction = -1;
motor(2).direction = 1;
motor(3).direction = -1;
motor(4).direction = 1;
% motor properties
electrical.kv = 880; % (rpm/volt)
electrical.no_load_current = [0.7,10]; % (A) @ (V)
electrical.resistance = 0.115; % (ohms)
% ESC properties
esc.resistance = 0.01; % (ohms)
% Propeller properties
prop.diameter = 245 * 0.001; % (m)
prop.pitch = 114.3 * 0.001; % (m)
prop.num_blades = 2;
prop.PConst = 1.13;
prop.TConst = 1;
prop.mass = 12.5*0.001; % (kg) (only used for inertia)
prop.inertia = (1/12)*prop.mass*prop.diameter^2; % rotational inertia (kgm^2) (rod about center)
% assign properties to motors
for i = 1:4
motor(i).electrical = electrical;
motor(i).esc = esc;
motor(i).prop = prop;
end
% Setup battery
battery.voltage = 4*4.2; % (volts)
battery.resistance = 0.0034; % (ohms)
battery.capacity = 5.2; % (ah)
% Add all to vehicle
copter.motors = motor;
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;
save('Hexsoon','copter')
% Plot motor curves
% http://www.bavaria-direct.co.za/constants/
% http://www.stefanv.com/rcstuff/qf200204.html
% Some calculators estimate heat and increase resistance with temp
% But then we have to estimate the power dissipation
% Max power for plot only
max_power = 260;
battery.voltage = battery.voltage * 0.50;
Kt = 1/(electrical.kv * ((2*pi)/60) ); % Convert Kv to rads/second
% plot the current from 0 to max power
amps = 0:0.1:max_power/battery.voltage;
power_in = amps * battery.voltage;
% voltage drop due to copper and esc
copper_drop = amps * electrical.resistance;
esc_drop = amps * esc.resistance;
ideal_voltage = battery.voltage - copper_drop - esc_drop;
power_out = ideal_voltage .* (amps - electrical.no_load_current(1));
efficiency = power_out ./ power_in;
torque = Kt * amps;
rpm = ideal_voltage * electrical.kv;
% Plot motor characteristics
figure('name',sprintf('motor characteristics at %0.2f volts',battery.voltage))
subplot(2,2,1)
hold all
title('RPM')
plot(amps,rpm)
xlabel('Current (A)')
ylabel('RPM')
xlim([0,amps(end)])
subplot(2,2,2)
hold all
title('torque')
plot(amps,torque)
xlabel('Current (A)')
ylabel('torque (NM)')
xlim([0,amps(end)])
subplot(2,2,3)
hold all
title('power')
plot(amps,power_in)
plot(amps,power_out)
xlabel('Current (A)')
ylabel('power (W)')
ylim([0,inf])
xlim([0,amps(end)])
legend('Power in','Power out','location','northwest')
subplot(2,2,4)
hold all
title('efficiency')
plot(amps,efficiency)
xlabel('Current (A)')
ylabel('efficiency (%)')
ylim([0,inf])
xlim([0,amps(end)])

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

Binary file not shown.

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

@ -0,0 +1,160 @@ @@ -0,0 +1,160 @@
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

3
libraries/SITL/examples/JSON/MATLAB/Copter/readme.md
Unescape View File

@ -0,0 +1,3 @@ @@ -0,0 +1,3 @@
This is a example MATLAB multicopter backend for the JSON SITL interface. Its mostly physics based, Copter.m is used to setup the property's of the vehicle. This does some basic performance plots of motor thrust and power from the given motor and propeller parameters. The vehicle parameters are then saved in a .mat file, Hexsoon.mat in this case as the model is roughly based on the Hexsoon EDU450. This file is then loaded into SIM_multicopter.m and the connection to SITL started. SIM_multicopter is written in such a way that it should be simple to save a set of parameters for another vehicle type, such as a hexacopter and run them with the same model.
There are many simplifications in the model, the most egregious of which is a arbitrary thrust multiplier to get a sensible hover thrust from the correct vehicle parameters.

118
libraries/SITL/examples/JSON/MATLAB/SITL_connector.m
Unescape View File

@ -0,0 +1,118 @@ @@ -0,0 +1,118 @@
% this function handles all the UDP connection to SITL using the TCP/UDP/IP
% 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)
try
pnet('closeall') % close any connections left open from past runs
catch
warning('Could not execute pnet mex, trying to compile')
if ispc
% running on windows
mex -O -outdir ../tcp_udp_ip_2.0.6 ../tcp_udp_ip_2.0.6/pnet.c ws2_32.lib -DWIN32
else
% running on unix or mac
mex -O -outdir ../tcp_udp_ip_2.0.6 ../tcp_udp_ip_2.0.6/pnet.c
end
try
pnet('closeall')
catch
error('Failed to compile pnet mex file, see tcp_udp_ip_2.0.6/pnet.c for instructions on manual complication')
end
end
% init physics
state = init_function(state);
% Init the UDP port
u = pnet('udpsocket',9002);
pnet(u,'setwritetimeout',1);
pnet(u,'setreadtimeout',0);
frame_time = tic;
frame_count = 0;
physics_time_us = 0;
last_SITL_frame = -1;
print_frame_count = 1000;
connected = false;
bytes_read = 4 + 4 + 16*2;
time_correction = 1;
while true
mat_time = tic;
% Wait for data
while true
in_bytes = pnet(u,'readpacket',bytes_read);
if in_bytes > 0
break;
end
end
% if there is another frame waiting, read it straight away
if in_bytes > bytes_read
if in_bytes == u.InputBufferSize
% buffer got full, reset
% should only happen if we have been paused in Matlab for some time
fprintf('Buffer reset\n')
continue;
end
continue;
end
% read in the current SITL frame and PWM
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 if the fame is in expected order
if SITL_frame < last_SITL_frame
% Controller has reset, reset physics also
state = init_function(state);
fprintf('Controller reset\n')
elseif SITL_frame == last_SITL_frame
% duplicate frame, skip
fprintf('Duplicate input frame\n')
continue;
elseif SITL_frame ~= last_SITL_frame + 1 && connected
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;
if ~connected
connected = true;
fprintf('Connected\n')
end
frame_count = frame_count + 1;
% 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.imu.gyro = state.gyro;
JSON.imu.accel_body = state.accel;
JSON.position = state.position;
JSON.attitude = state.attitude;
JSON.velocity = state.velocity;
% Report to AP
pnet(u,'printf',sprintf('\n%s\n',jsonencode(JSON)));
pnet(u,'writepacket',target_ip,9003);
% 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;
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

36
libraries/SITL/examples/JSON/MATLAB/readme.md
Unescape View File

@ -0,0 +1,36 @@ @@ -0,0 +1,36 @@
This is a tool to allow MATLAB to interface with the SITL JSON backend.
The SITL_connector function can be used to simplify the connection process. SITL_connector.m uses the TCP/UDP/IP Toolbox 2.0.6 by Peter Rydesäter and is much (10x) faster than the MATLAB functions available with the instrument control toolbox (and free!). However this may require compilation of a mex file, prebuilt mex files are included in most cases they will work without the need to re-compile.
see Copter/SIM_multicopter.m for example usage.
The function is defined as:
```
SITL_connector(target_ip,state,init_function,physics_function,delta_t)
```
- target_ip: the IP address of the machine running SITL eg '127.0.0.1'
- state: this is the persistent physics state of the vehicle its is a structure and must contain the following felids:
```
state.gyro(roll, pitch, yaw) (radians/sec) body frame
state.attitude(roll, pitch yaw) (radians)
state.accel(north, east, down) (m/s^2) body frame
state.velocity(north, east,down) (m/s) earth frame
state.position(north, east, down) (m) earth frame
```
the structure can have also any other felids required for the physics model
- init_function: function handle that will be called to init the physics model, this will be called on the first run and after a SITL reboot. It should take and return the state.
function state init(state)
init_function = @(state)init(state);
- physics_function: function handle that will be called to update the physics model by a single time step. It should take in the state and array of 16 PWM inputs and return the state.
```
function state = physics_step(pwm_in,state)
physics_function = @(pwm_in,state)physics_step(pwm_in,state);
```
- delta_t: time step size the physics model wil use in seconds. Note that this is directly connected to the maximum speed you can run SITL at.
The JSON SITL interface is lock-step scheduled. This allows matlab breakpoints to work as normal.
Using the connection it should be possible to achieve > 1500 fps, at this speed MATLAB code efficiency plays a important factor in the max frame rate. For a 400hz physics time step this gives a maximum speedup of 4 to 5 times. For planes and rovers it should be possible to use a much larger physics time step resulting in a larger maximum speed up. Note that large speedups risk the GCS getting left behind.
Write Preview
Loading…
Cancel
Save