CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/simulink/arducopter/functions/plausibilizeTime.m
Views: 1799
1
function [timePlaus, signalPlaus] = plausibilizeTime(time,signal)

2
% Checks if time vector has a major tracking error and delete respective

3
% element from time and signal vector

4
%

5
% Fabian Bredemeier - IAV GmbH

6
% License: GPL v3

7


8
% Get indices for time values that are too high

9
validIdx = ~(time > 1e6);

10
if time(1) > time(2) % First time stamp is invalid if greater than second

11
    validIdx(1) = 0;

12
end

13


14
% Calculate average sample time

15
Ts_avg = sum(diff(time(validIdx)))/length(time);

16


17
% Only plausibilize time vector

18
if nargin == 1

19
    % Set first element

20
    if time(1) > 1e6 && time(2) < 1e6 || time(1) > time(2)

21
        timePlaus = time(2)-Ts_avg;

22
    else

23
        timePlaus = time(1);

24
    end 

25
    idxDiff = 0; % Tracking of removed elements

26
    skipNext = 0; % Flag to skip next element

27
    for i = 2:length(time)-1

28
        if skipNext

29
            skipNext = 0;

30
            continue;

31
        end

32
        if(time(i+1)<time(i))

33
            % Check if current or next element is incorrect

34
            if abs((time(i)-time(i-1))-Ts_avg) > abs((time(i+1)-time(i-1))-Ts_avg) && time(i+1)-time(i-1) > 0

35
                % Current element is wrong since deviation to average

36
                % sample time is larger - skip current and do nothing

37
                

38
            else

39
                % Next element is wrong, so add current to corrected vector

40
                timePlaus(i-idxDiff,1) = time(i);

41
                

42
                % Make sure next one is skipped

43
                skipNext = 1;

44
            end

45
            idxDiff = idxDiff + 1;

46
        else

47
            timePlaus(i-idxDiff,1) = time(i);

48
        end

49
    end

50
    % Add last element

51
    len = length(time);

52
    timePlaus(len-idxDiff,1) = time(end);

53
    return;

54


55
% Also plausibilize signal vector

56
elseif nargin == 2

57
    % Set first element

58
    if time(1) > 1e6 && time(2) < 1e6 || time(1) > time(2)

59
        timePlaus = time(2)-Ts_avg;

60
        signalPlaus = signal(2);

61
    else

62
        timePlaus = time(1);

63
        signalPlaus = signal(1);

64
    end

65
    idxDiff = 0; % Tracking of removed elements

66
    skipNext = 0; % Flag to skip next element

67
    for i = 2:length(time)-1

68
        if skipNext

69
            skipNext = 0;

70
            continue;

71
        end

72
        if(time(i+1)<time(i))

73
            % Check if current or next element is incorrect

74
            if abs((time(i)-time(i-1))-Ts_avg) > abs((time(i+1)-time(i-1))-Ts_avg) && time(i+1)-time(i-1) > 0

75
                % Current element is wrong since deviation to average

76
                % sample time is larger - skip current and do nothing

77
                

78
            else

79
                % Next element is wrong, so add current to corrected vector

80
                timePlaus(i-idxDiff,1) = time(i);

81
                signalPlaus(i-idxDiff,1) = signal(i);

82
                

83
                % Make sure next one is skipped

84
                skipNext = 1;

85
            end

86
            idxDiff = idxDiff + 1;

87
        else

88
            timePlaus(i-idxDiff,1) = time(i);

89
            signalPlaus(i-idxDiff,1) = signal(i);

90
        end

91
    end

92
    % Add last element

93
    len = length(time);

94
    timePlaus(len-idxDiff,1) = time(end);

95
    signalPlaus(len-idxDiff,1) = signal(end);

96
end

97
end

98


99


100