diff --git a/libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m b/libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m index 22a7e217f0..905edf9d7e 100644 --- a/libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m +++ b/libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m @@ -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') diff --git a/libraries/SITL/examples/JSON/MATLAB/Copter/Hexsoon.mat b/libraries/SITL/examples/JSON/MATLAB/Copter/Hexsoon.mat index 4848175d80..5ea3572c81 100644 Binary files a/libraries/SITL/examples/JSON/MATLAB/Copter/Hexsoon.mat and b/libraries/SITL/examples/JSON/MATLAB/Copter/Hexsoon.mat differ diff --git a/libraries/SITL/examples/JSON/MATLAB/Copter/SIM_multicopter.m b/libraries/SITL/examples/JSON/MATLAB/Copter/SIM_multicopter.m index eb80ffd749..259976cdd6 100644 --- a/libraries/SITL/examples/JSON/MATLAB/Copter/SIM_multicopter.m +++ b/libraries/SITL/examples/JSON/MATLAB/Copter/SIM_multicopter.m @@ -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; + +% 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)); -% body frame accelerations (NED) +% 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; + +% 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 -state.velocity = state.velocity + acel_ef * delta_t; -state.position = state.position + state.velocity * delta_t; +% 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; +% calculate the body frame velocity for drag calculation +state.bf_velo = state.dcm' * state.velocity; + +end -% 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 +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 + diff --git a/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m b/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m index d7107bda5b..bd0568d82f 100644 --- a/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m +++ b/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m @@ -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