Blackbox device type 'file' (SITL) considered working when file handler is available
[inav.git] / src / main / sensors / esc_sensor.c
blobbc77c281c7961f8f7fbdde44f0d93bb40bb8f6e2
1 /*
2 * This file is part of INAV Project.
4 * This Source Code Form is subject to the terms of the Mozilla Public
5 * License, v. 2.0. If a copy of the MPL was not distributed with this file,
6 * You can obtain one at http://mozilla.org/MPL/2.0/.
8 * Alternatively, the contents of this file may be used under the terms
9 * of the GNU General Public License Version 3, as described below:
11 * This file is free software: you may copy, redistribute and/or modify
12 * it under the terms of the GNU General Public License as published by the
13 * Free Software Foundation, either version 3 of the License, or (at your
14 * option) any later version.
16 * This file is distributed in the hope that it will be useful, but
17 * WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
19 * Public License for more details.
21 * You should have received a copy of the GNU General Public License
22 * along with this program. If not, see http://www.gnu.org/licenses/.
25 #include <stdbool.h>
26 #include <stdint.h>
27 #include <ctype.h>
28 #include <math.h>
30 #include "platform.h"
32 #include "build/build_config.h"
33 #include "build/debug.h"
35 #include "common/maths.h"
36 #include "common/crc.h"
38 #include "config/feature.h"
39 #include "config/config_reset.h"
40 #include "config/parameter_group.h"
41 #include "config/parameter_group_ids.h"
43 #include "flight/mixer.h"
44 #include "drivers/pwm_output.h"
45 #include "sensors/esc_sensor.h"
46 #include "io/serial.h"
47 #include "fc/config.h"
48 #include "fc/runtime_config.h"
49 #include "fc/settings.h"
52 #if defined(USE_ESC_SENSOR)
54 #define ESC_BOOTTIME_MS 5000
55 #define ESC_REQUEST_TIMEOUT_MS 50
56 #define ESC_SENSOR_BAUDRATE 115200
57 #define TELEMETRY_FRAME_SIZE 10
59 typedef enum {
60 ESC_SENSOR_WAIT_STARTUP = 0,
61 ESC_SENSOR_READY = 1,
62 ESC_SENSOR_WAITING = 2
63 } escSensorState_t;
65 typedef enum {
66 ESC_SENSOR_FRAME_PENDING,
67 ESC_SENSOR_FRAME_COMPLETE,
68 ESC_SENSOR_FRAME_FAILED
69 } escSensorFrameStatus_t;
71 static serialPort_t * escSensorPort = NULL;
72 static escSensorState_t escSensorState = ESC_SENSOR_WAIT_STARTUP;
73 static timeMs_t escTriggerTimeMs;
74 static int escSensorMotor;
75 static uint8_t telemetryBuffer[TELEMETRY_FRAME_SIZE];
76 static int bufferPosition = 0;
77 static escSensorData_t escSensorData[MAX_SUPPORTED_MOTORS];
78 static escSensorData_t escSensorDataCombined;
79 static bool escSensorDataNeedsUpdate;
81 PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 1);
82 PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig,
83 .currentOffset = 0, // UNUSED
84 .listenOnly = SETTING_ESC_SENSOR_LISTEN_ONLY_DEFAULT,
87 static int getTelemetryMotorCount(void)
89 if (escSensorConfig()->listenOnly) {
90 return 1;
92 else {
93 return getMotorCount();
97 static void escSensorSelectNextMotor(void)
99 if (escSensorConfig()->listenOnly) {
100 escSensorMotor = 0;
102 else {
103 escSensorMotor = (escSensorMotor + 1) % getTelemetryMotorCount();
107 static void escSensorIncreaseDataAge(void)
109 if (escSensorData[escSensorMotor].dataAge < ESC_DATA_INVALID) {
110 escSensorData[escSensorMotor].dataAge++;
111 escSensorDataNeedsUpdate = true;
115 static bool escSensorDecodeFrame(void)
117 // Receive bytes
118 while (serialRxBytesWaiting(escSensorPort) > 0) {
119 uint8_t c = serialRead(escSensorPort);
121 if (bufferPosition < TELEMETRY_FRAME_SIZE) {
122 telemetryBuffer[bufferPosition++] = c;
126 // Decode frame
127 if (bufferPosition >= TELEMETRY_FRAME_SIZE) {
128 uint8_t checksum = crc8_update(0, telemetryBuffer, TELEMETRY_FRAME_SIZE - 1);
129 if (checksum == telemetryBuffer[TELEMETRY_FRAME_SIZE - 1]) {
130 escSensorData[escSensorMotor].dataAge = 0;
131 escSensorData[escSensorMotor].temperature = telemetryBuffer[0];
132 escSensorData[escSensorMotor].voltage = ((uint16_t)telemetryBuffer[1]) << 8 | telemetryBuffer[2];
133 escSensorData[escSensorMotor].current = ((uint16_t)telemetryBuffer[3]) << 8 | telemetryBuffer[4];
134 escSensorData[escSensorMotor].rpm = computeRpm(((uint16_t)telemetryBuffer[7]) << 8 | telemetryBuffer[8]);
135 escSensorDataNeedsUpdate = true;
137 return ESC_SENSOR_FRAME_COMPLETE;
139 else {
140 // CRC error
141 return ESC_SENSOR_FRAME_FAILED;
145 return ESC_SENSOR_FRAME_PENDING;
148 uint32_t computeRpm(int16_t erpm) {
149 return lrintf((float)erpm * ERPM_PER_LSB / (motorConfig()->motorPoleCount / 2));
152 escSensorData_t NOINLINE * getEscTelemetry(uint8_t esc)
154 return &escSensorData[esc];
157 escSensorData_t * escSensorGetData(void)
159 if (!escSensorPort) {
160 return NULL;
163 if (escSensorDataNeedsUpdate) {
164 escSensorDataCombined.dataAge = 0;
165 escSensorDataCombined.temperature = 0;
166 escSensorDataCombined.voltage = 0;
167 escSensorDataCombined.current = 0;
168 escSensorDataCombined.rpm = 0;
170 // Combine data only from active sensors, ignore stale sensors
171 int usedEscSensorCount = 0;
172 for (int i = 0; i < getTelemetryMotorCount(); i++) {
173 if (escSensorData[i].dataAge < ESC_DATA_INVALID) {
174 usedEscSensorCount++;
175 escSensorDataCombined.dataAge = MAX(escSensorDataCombined.dataAge, escSensorData[i].dataAge);
176 escSensorDataCombined.temperature = MAX(escSensorDataCombined.temperature, escSensorData[i].temperature);
177 escSensorDataCombined.voltage += escSensorData[i].voltage;
178 escSensorDataCombined.current += escSensorData[i].current;
179 escSensorDataCombined.rpm += escSensorData[i].rpm;
183 // Make sure we calculate our sensor values only from non-stale values
184 if (usedEscSensorCount) {
185 escSensorDataCombined.current = (uint32_t)escSensorDataCombined.current * getTelemetryMotorCount() / usedEscSensorCount + escSensorConfig()->currentOffset;
186 escSensorDataCombined.voltage = (uint32_t)escSensorDataCombined.voltage / usedEscSensorCount;
187 escSensorDataCombined.rpm = (float)escSensorDataCombined.rpm / usedEscSensorCount;
189 else {
190 escSensorDataCombined.dataAge = ESC_DATA_INVALID;
193 escSensorDataNeedsUpdate = false;
196 // Return NULL if sensors are old
197 if (escSensorDataCombined.dataAge >= ESC_DATA_INVALID) {
198 return NULL;
200 else {
201 return &escSensorDataCombined;
205 bool escSensorInitialize(void)
207 escSensorDataNeedsUpdate = true;
208 escSensorPort = NULL;
210 // Fail immediately if motor output are disabled or motor outputs are not configured
211 if (!feature(FEATURE_PWM_OUTPUT_ENABLE) || getMotorCount() == 0) {
212 return false;
215 // FUNCTION_ESCSERIAL is shared between SERIALSHOT and ESC_SENSOR telemetry
216 // They are mutually exclusive
217 serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_ESCSERIAL);
218 if (!portConfig) {
219 return false;
222 escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESCSERIAL, NULL, NULL, ESC_SENSOR_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED | SERIAL_UNIDIR);
223 if (!escSensorPort) {
224 return false;
227 for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
228 escSensorData[i].dataAge = ESC_DATA_INVALID;
231 ENABLE_STATE(ESC_SENSOR_ENABLED);
233 return true;
236 void escSensorUpdate(timeUs_t currentTimeUs)
238 if (!escSensorPort) {
239 return;
242 const timeMs_t currentTimeMs = currentTimeUs / 1000;
244 switch (escSensorState) {
245 case ESC_SENSOR_WAIT_STARTUP:
246 if (currentTimeMs > ESC_BOOTTIME_MS) {
247 escSensorMotor = 0;
248 escSensorState = ESC_SENSOR_READY;
250 break;
252 case ESC_SENSOR_READY:
253 if (!escSensorConfig()->listenOnly) {
254 pwmRequestMotorTelemetry(escSensorMotor);
256 bufferPosition = 0;
257 escTriggerTimeMs = currentTimeMs;
258 escSensorState = ESC_SENSOR_WAITING;
259 break;
261 case ESC_SENSOR_WAITING:
262 if ((currentTimeMs - escTriggerTimeMs) >= ESC_REQUEST_TIMEOUT_MS) {
263 // Timed out. Select next motor and move on
264 escSensorIncreaseDataAge();
265 escSensorSelectNextMotor();
266 escSensorState = ESC_SENSOR_READY;
268 else {
269 // Receive serial data and decode frame
270 escSensorFrameStatus_t status = escSensorDecodeFrame();
272 switch (status) {
273 case ESC_SENSOR_FRAME_COMPLETE:
274 escSensorSelectNextMotor();
275 escSensorState = ESC_SENSOR_READY;
276 break;
278 case ESC_SENSOR_FRAME_FAILED:
279 escSensorIncreaseDataAge();
280 escSensorSelectNextMotor();
281 escSensorState = ESC_SENSOR_READY;
282 break;
284 case ESC_SENSOR_FRAME_PENDING:
285 default:
286 break;
289 break;
295 #endif