irsim/sensor.cpp at master · macols77/irsim · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
#include "sensor.h"

/******************************************************************************/
/******************************************************************************/

char  CSensor::m_pchSensorReadingsString[10240];

/******************************************************************************/
/******************************************************************************/

CSensor::CSensor(const char* pch_name, unsigned int un_no_outputs) : CSimObject(pch_name), m_unNumberOfInputs(un_no_outputs)
{
    if (m_unNumberOfInputs > 0)
        m_pfInputs =(double*)  malloc(sizeof(double) * m_unNumberOfInputs);
    else
        m_pfInputs = NULL;
}

/******************************************************************************/
/******************************************************************************/

CSensor::~CSensor()
{
    if (m_pfInputs)
        free(m_pfInputs);
}

/******************************************************************************/
/******************************************************************************/

unsigned int CSensor::GetNumberOfInputs()
{
    return m_unNumberOfInputs;
}


/******************************************************************************/
/******************************************************************************/

void CSensor::SetInput(unsigned int un_index, double f_value)
{
    if (un_index < m_unNumberOfInputs)
        m_pfInputs[un_index] = f_value;
    else{
            printf("Input out of %d >= %d", un_index, m_unNumberOfInputs);
            fflush(stdout);
        }
}

/******************************************************************************/
/******************************************************************************/

double* CSensor::GetInputs()
{
    return m_pfInputs;
}

/******************************************************************************/
/******************************************************************************/

double CSensor::GetInput(unsigned int un_index)
{
    return m_pfInputs[un_index];
}

/******************************************************************************/
/******************************************************************************/

char* CSensor::SensorReadingsToString(unsigned int un_number_of_readings,
                                      double* pf_readings)
{
    unsigned int unStrLen = 0;

    for (int i = 0; i < un_number_of_readings; i++)
    {
        sprintf(&m_pchSensorReadingsString[unStrLen], "[%2d: %2.6f] ", i, pf_readings[i]);
        unStrLen += strlen(&m_pchSensorReadingsString[unStrLen]);
    }

    return &m_pchSensorReadingsString[0];
}


/******************************************************************************/
/******************************************************************************/

double* CSensor::GetComputedSensorReadings()
{
	return m_pfInputs;
    //return GetInputs();
}

/******************************************************************************/
/******************************************************************************/


//float CSensor::NormalizeAngle( float angle)
//{
	//if (angle < 0.0 )
		//return fmod(angle, 2 * M_PI) + 2 * M_PI;
	//else
		//return fmod(angle, 2*M_PI);
//}