Inclination measurement

The calculation of an angle with higher resolution and accuracy should take place on a PC. The sensors used are capable of an accuracy of less than 0.1°.

Since the angle values are derived from the acceleration values, which are subject to certain noise, they have to be filtered via suitable algorithms.

In simple cases this could be a sliding average value, for example.

Inclination measurement 1:Fig.7: Angle measurement, process data as acceleration values, calculation on a PC
Inclination measurement 2:Fig.8: Signal noise in detail

Color

Meaning

Red

Angle measured with 1024-step encoder / 4-way analysis for reference

Green

Angle trigonometrically calculated on a PC, without noise suppression

blue

Fast algorithm

yellow

Arithmetic mean (1000 sliding values)

Sample program 1: Calculating roll and pitch

This sample program demonstrates how to calculate the roll and pitch angles based on the measured acceleration values.

PROGRAM MAIN

VAR
    (* Acceleration values *)
    fX_11 AT %I* : INT;
    fY_11 AT %I* : INT;
    fZ_11 AT %I* : INT;
    pi: LREAL;

    fRoll : LREAL; (* Angle about X-axis *)
    fPitch : LREAL; (* Angle about Y-axis *)
END_VAR
pi:=3.14159265;

IF fX_11 <> 0 AND fZ_11 <> 0 THEN
    fRoll := ATAN(fY_11/SQRT(EXPT(fX_11,2)+EXPT(fZ_11,2)));
    IF fRoll < 0 THEN
        fRoll := fRoll + 2*pi;
    END_IF
    fRoll := fRoll*180/pi;
END_IF

IF fY_11 <> 0 AND fZ_11 <> 0 THEN
    fPitch := ATAN(fX_11/SQRT(EXPT(fY_11,2)+EXPT(fZ_11,2)));
    IF fPitch < 0 THEN
        fPitch := fPitch + 2*pi;
    END_IF
    fPitch := fPitch*180/pi;
END_IF

Sample program 2: Calculating the angle between two measurement states

This sample program demonstrates how to calculate the angle between the current acceleration vector and a reference acceleration vector.

PROGRAM MAIN

VAR
    (* Acceleration values *)
    fX_11 AT %I* : INT;
    fY_11 AT %I* : INT;
    fZ_11 AT %I* : INT;

    fAbsLast: LREAL;
    bStoreInitial: BOOL;
    fAbsInit: LREAL;
    pi: LREAL;
    fXInit: LREAL;
    fYInit: LREAL;
    fZInit: LREAL;

    fAlpha: LREAL;
END_VAR
pi:=3.14159265;

IF bStoreInitial THEN
    fXInit := fX_11;
    fYInit := fY_11;
    fZInit := fZ_11;
    bStoreInitial := FALSE;
END_IF

fAbsInit := SQRT(EXPT(fXInit,2)+EXPT(fYInit,2)+EXPT(fZInit,2));
fAbsLast := SQRT(EXPT(fX_11,2)+EXPT(fY_11,2)+EXPT(fZ_11,2));

IF fAbsInit <> 0 AND fAbsLast <> 0 THEN
    fAlpha := ACOS((fXInit*fX_11 + fYInit*fY_11 + fZInit*fZ_11)/(fAbsInit*fAbsLast))*180/pi;
END_IF

Sample

Inclination measurement 3:

Equation for calculating the angle α:

Inclination measurement 4:

Implementation in TwinCAT:

alpha := ATAN(a_y1 / (SQRT(a_x1 * a_x1 + a_z1 * a_z1))) * 360/(2*3.14);