-
Notifications
You must be signed in to change notification settings - Fork 3
/
FrSkySportSensorFcs.cpp
executable file
·75 lines (69 loc) · 1.97 KB
/
FrSkySportSensorFcs.cpp
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
/*
FrSky FCS-40A/FCS-150A current sensor class for Teensy 3.x/4.0/LC, ESP8266, ATmega2560 (Mega) and ATmega328P based boards (e.g. Pro Mini, Nano, Uno)
(c) Pawelsky 202000503
Not for commercial use
*/
#include "FrSkySportSensorFcs.h"
FrSkySportSensorFcs::FrSkySportSensorFcs(SensorId id) : FrSkySportSensor(id) { }
void FrSkySportSensorFcs::setData(float current, float voltage)
{
currentData = (uint32_t)(current * 10);
voltageData = (uint32_t)(voltage * 100);
}
uint16_t FrSkySportSensorFcs::send(FrSkySportSingleWireSerial& serial, uint8_t id, uint32_t now)
{
uint16_t dataId = SENSOR_NO_DATA_ID;
if(sensorId == id)
{
switch(sensorDataIdx)
{
case 0:
dataId = FCS_CURR_DATA_ID;
if(now > currentTime)
{
currentTime = now + FCS_CURR_DATA_PERIOD;
serial.sendData(dataId, currentData);
}
else
{
serial.sendEmpty(dataId);
dataId = SENSOR_EMPTY_DATA_ID;
}
break;
case 1:
dataId = FCS_VOLT_DATA_ID;
if(now > voltageTime)
{
voltageTime = now + FCS_VOLT_DATA_PERIOD;
serial.sendData(dataId, voltageData);
}
else
{
serial.sendEmpty(dataId);
dataId = SENSOR_EMPTY_DATA_ID;
}
break;
}
sensorDataIdx++;
if(sensorDataIdx >= FCS_DATA_COUNT) sensorDataIdx = 0;
}
return dataId;
}
uint16_t FrSkySportSensorFcs::decodeData(uint8_t id, uint16_t appId, uint32_t data)
{
if((sensorId == id) || (sensorId == FrSkySportSensor::ID_IGNORE))
{
switch(appId)
{
case FCS_CURR_DATA_ID:
current = data / 10.0;
return appId;
case FCS_VOLT_DATA_ID:
voltage = data / 100.0;
return appId;
}
}
return SENSOR_NO_DATA_ID;
}
float FrSkySportSensorFcs::getCurrent() { return current; }
float FrSkySportSensorFcs::getVoltage() { return voltage; }