From 3bb31393c10b41568cdda8b524e130ea1de55798 Mon Sep 17 00:00:00 2001
From: Emilio Corigliano <emilio.corigliano@skyward.eu>
Date: Fri, 12 Aug 2022 20:42:31 +0200
Subject: [PATCH] [pitot] fixed problems in simulating pitot
---
sensors/acquisition_Sys.m | 7 ++++---
sensors/initSensors.m | 4 ++--
simulator/configEuroc.m | 13 +++++++------
simulator/configRoccaraso.m | 2 +-
simulator/manageCalibration.m | 23 +++++++++++++++--------
simulator/manageSignalFrequencies.m | 6 +++---
simulator/start_simulation.m | 2 +-
7 files changed, 33 insertions(+), 24 deletions(-)
diff --git a/sensors/acquisition_Sys.m b/sensors/acquisition_Sys.m
index 3b5dfe87..aaa89921 100644
--- a/sensors/acquisition_Sys.m
+++ b/sensors/acquisition_Sys.m
@@ -99,11 +99,12 @@ OUTPUT:
if isfield(sensorData, 'pitot')
for ii=1:length(sensorData.pitot.time)
- sensorData.pitot.measures(ii,:) = s.SSCDRRN015PDAD5.sens(sensorData.pitot.measures(ii)/100,...
+ asd = s.SSCDRRN015PDAD5.sens(sensorData.pitot.measures(ii)/100,...
sensorData.pitot.temperature(ii) - 273.15);
- sensorData.pitot.measures(ii,:) = sensorData.pitot.measures(ii)*100;
+ sensorData.pitot.measures(ii,1) = asd;
+ sensorData.pitot.measures(ii,1) = sensorData.pitot.measures(ii)*100;
end
- tot.pitot_tot(tot.npitot_old:tot.npitot_old + size(sensorData.pitot.measures,1) - 1,1) = sensorData.pitot.measures(1:end);
+ tot.pitot_tot(tot.npitot_old:tot.npitot_old + size(sensorData.pitot.measures,1) - 1,1) = sensorData.pitot.measures;
tot.time_pitot(tot.npitot_old:tot.npitot_old + size(sensorData.pitot.measures,1) - 1) = sensorData.pitot.time ;
tot.npitot_old = tot.npitot_old + size(sensorData.pitot.measures,2);
end
diff --git a/sensors/initSensors.m b/sensors/initSensors.m
index 927b8734..3bf2a85b 100644
--- a/sensors/initSensors.m
+++ b/sensors/initSensors.m
@@ -98,10 +98,10 @@ s.SSCDRRN015PDAD5.minMeasurementRange = -1034; % in mbar (-1
s.SSCDRRN015PDAD5.offset = -1.9327; % in mbar
s.SSCDRRN015PDAD5.resolution = 1; % in mbar
s.SSCDRRN015PDAD5.noiseVariance = 75; % guess in mbar
-s.SSCDRRN015PDAD5.error2dOffset = ep_data; % [p in mbar, T in celsius, ep in mbar]
+% s.SSCDRRN015PDAD5.error2dOffset = ep_data; % [p in mbar, T in celsius, ep in mbar]
% check 2d offset for pitot
-
+sensorTot.npitot_old = 1;
sensorTot.np_old = 1;
sensorTot.na_old = 1;
sensorTot.ngps_old = 1;
diff --git a/simulator/configEuroc.m b/simulator/configEuroc.m
index 5289afdd..d4d5f3cd 100644
--- a/simulator/configEuroc.m
+++ b/simulator/configEuroc.m
@@ -122,12 +122,13 @@ settings.Controls = s.State.hprot';
clear('s');
%% CONTROL AND SENSOR FREQUENCIES
-settings.frequencies.controlFrequency = 10; % [hz] control action frequency
-settings.frequencies.accelerometerFrequency = 100; % [hz] control action frequency
-settings.frequencies.gyroFrequency = 100; % [hz] control action frequency
-settings.frequencies.magnetometerFrequency = 100; % [hz] control action frequency
-settings.frequencies.gpsFrequency = 10; % [hz] control action frequency
-settings.frequencies.barometerFrequency = 20; % [hz] control action frequency
+settings.frequencies.controlFrequency = 10; % [hz] control action frequency
+settings.frequencies.accelerometerFrequency = 100; % [hz] sensor frequency
+settings.frequencies.gyroFrequency = 100; % [hz] sensor frequency
+settings.frequencies.magnetometerFrequency = 100; % [hz] sensor frequency
+settings.frequencies.gpsFrequency = 10; % [hz] sensor frequency
+settings.frequencies.barometerFrequency = 20; % [hz] sensor frequency
+settings.frequencies.pitotFrequency = 20; % [hz] sensor frequency
%% KALMAN TUNING PARAMETERS
settings.kalman.dt_k = 0.01; % [s] kalman time step
diff --git a/simulator/configRoccaraso.m b/simulator/configRoccaraso.m
index 1d63a434..69a1bf94 100644
--- a/simulator/configRoccaraso.m
+++ b/simulator/configRoccaraso.m
@@ -130,7 +130,7 @@ settings.frequencies.gyroFrequency = 100; % [hz
settings.frequencies.magnetometerFrequency = 100; % [hz] sensor frequency
settings.frequencies.gpsFrequency = 10; % [hz] sensor frequency
settings.frequencies.barometerFrequency = 20; % [hz] sensor frequency
-settings.frequencies.pitot = 20; % [hz] sensor frequency
+settings.frequencies.pitotFrequency = 20; % [hz] sensor frequency
%% KALMAN TUNING PARAMETERS
diff --git a/simulator/manageCalibration.m b/simulator/manageCalibration.m
index f1fcafe8..7b7bc645 100644
--- a/simulator/manageCalibration.m
+++ b/simulator/manageCalibration.m
@@ -4,6 +4,8 @@ Na = freq.accelerometerFrequency/freq.controlFrequency;
Nm = freq.magnetometerFrequency/freq.controlFrequency;
Ngps = freq.gpsFrequency/freq.controlFrequency;
Nbar = freq.barometerFrequency/freq.controlFrequency;
+Npit = freq.pitotFrequency/freq.controlFrequency;
+
ome = settings.OMEGA;
phi = settings.PHI;
@@ -14,33 +16,38 @@ for i = 1:Na
SensorData.accelerometer.measures(i,:) = (A*[0; 0; -9.81])';
SensorData.gyro.measures(i,:) = [0, 0, 0];
end
- SensorData.accelerometer.time = zeros(1,Na);
- SensorData.gyro.time = zeros(1,Na);
+SensorData.accelerometer.time = zeros(1,Na);
+SensorData.gyro.time = zeros(1,Na);
for i = 1:Nm
SensorData.magnetometer.measures(i,:) = (A * magField)';
end
- SensorData.magnetometer.time = zeros(1,Nm);
+SensorData.magnetometer.time = zeros(1,Nm);
for i = 1:Ngps
SensorData.gps.velocityMeasures(i,:) = [0,0,0];
SensorData.gps.positionMeasures(i,:) = [settings.lat0,settings.lon0,settings.z0];
end
- SensorData.gps.time = zeros(1,Ngps);
+SensorData.gps.time = zeros(1,Ngps);
- [T,~,P,~] = atmosisa(settings.z0);
+[T,~,P,~] = atmosisa(settings.z0);
for i = 1:Nbar
SensorData.barometer.measures(i,1) = P;
SensorData.barometer.temperature(i,1) = T;
end
- SensorData.barometer.time = zeros(1,Nbar);
-
+SensorData.barometer.time = zeros(1,Nbar);
+
+SensorData.pitot.measures = zeros(Npit,1);
+for i = 1:Npit
+ SensorData.pitot.temperature(i,1) = T;
end
+SensorData.pitot.time = zeros(1,Npit);
+end
+
function A = cdmrot(q)
A = [q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2, 2*(q(1)*q(2) + q(3)*q(4)), 2*(q(1)*q(3) - q(2)*q(4));
2*(q(1)*q(2) - q(3)*q(4)), -q(1)^2 + q(2)^2 - q(3)^2 + q(4)^2, 2*(q(2)*q(3) + q(1)*q(4)) ;
2*(q(1)*q(3) + q(2)*q(4)), 2*(q(2)*q(3) - q(1)*q(4)), -q(1)^2 - q(2)^2 + q(3)^2 + q(4)^2];
-
end
\ No newline at end of file
diff --git a/simulator/manageSignalFrequencies.m b/simulator/manageSignalFrequencies.m
index c22f25fa..e8e3b5fe 100644
--- a/simulator/manageSignalFrequencies.m
+++ b/simulator/manageSignalFrequencies.m
@@ -287,7 +287,7 @@ if isfield(freq, 'pitotFrequency')
z(i) = - m*iTimeBarometer + q;
else
vx(i) = Y(iTimePitot == T, 4);
- z(i) = -Y(iTimeBarometer == T, 3);
+ z(i) = -Y(iTimePitot == T, 3);
end
end
@@ -308,8 +308,8 @@ if isfield(freq, 'pitotFrequency')
wind_body = ned2body*wind_ned;
- v = vx + wind_body(1); % Speed x_body + wind in x_body direction
+ v = (vx + wind_body(1))'; % Speed x_body + wind in x_body direction
sensorData.pitot.temperature = Temp;
- sensorData.pitot.measures = 0.5*rho*v*v*sign(v); % differential pressure in Pascals
+ sensorData.pitot.measures = (0.5*rho'.*v.*v.*sign(v))'; % differential pressure in Pascals
end
diff --git a/simulator/start_simulation.m b/simulator/start_simulation.m
index d5a06abf..79a36bd8 100644
--- a/simulator/start_simulation.m
+++ b/simulator/start_simulation.m
@@ -50,7 +50,7 @@ end
%start simulation
-[Yf, Tf, cpuTimes, flagMatr, otherData] = std_run(settings);
+ [Yf, Tf, cpuTimes, flagMatr, otherData] = std_run(settings);
if not(settings.electronics)
--
GitLab