src/main/sensors/esc_sensor.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24
  25 #include <platform.h>
  26
  27 #if defined(USE_ESC_SENSOR)
  28
  29 #include "build/debug.h"
  30
  31 #include "config/feature.h"
  32 #include "pg/pg.h"
  33 #include "pg/pg_ids.h"
  34
  35 #include "common/maths.h"
  36 #include "common/utils.h"
  37
  38 #include "drivers/pwm_output.h"
  39 #include "drivers/serial.h"
  40 #include "drivers/serial_uart.h"
  41
  42 #include "esc_sensor.h"
  43
  44 #include "fc/config.h"
  45
  46 #include "flight/mixer.h"
  47
  48 #include "io/serial.h"
  49
  50 /*
  51 KISS ESC TELEMETRY PROTOCOL
  52 ---------------------------
  53
  54 One transmission will have 10 times 8-bit bytes sent with 115200 baud and 3.6V.
  55
  56 Byte 0: Temperature
  57 Byte 1: Voltage high byte
  58 Byte 2: Voltage low byte
  59 Byte 3: Current high byte
  60 Byte 4: Current low byte
  61 Byte 5: Consumption high byte
  62 Byte 6: Consumption low byte
  63 Byte 7: Rpm high byte
  64 Byte 8: Rpm low byte
  65 Byte 9: 8-bit CRC
  66
  67 */
  68
  69 PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 0);
  70
  71 PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig,
  72         .halfDuplex = 0
  73 );
  74
  75 /*
  76 DEBUG INFORMATION
  77 -----------------
  78
  79 set debug_mode = DEBUG_ESC_SENSOR in cli
  80
  81 */
  82
  83 enum {
  84     DEBUG_ESC_MOTOR_INDEX = 0,
  85     DEBUG_ESC_NUM_TIMEOUTS = 1,
  86     DEBUG_ESC_NUM_CRC_ERRORS = 2,
  87     DEBUG_ESC_DATA_AGE = 3,
  88 };
  89
  90 typedef enum {
  91     ESC_SENSOR_FRAME_PENDING = 0,
  92     ESC_SENSOR_FRAME_COMPLETE = 1,
  93     ESC_SENSOR_FRAME_FAILED = 2
  94 } escTlmFrameState_t;
  95
  96 typedef enum {
  97     ESC_SENSOR_TRIGGER_STARTUP = 0,
  98     ESC_SENSOR_TRIGGER_READY = 1,
  99     ESC_SENSOR_TRIGGER_PENDING = 2
 100 } escSensorTriggerState_t;
 101
 102 #define ESC_SENSOR_BAUDRATE 115200
 103 #define ESC_BOOTTIME 5000               // 5 seconds
 104 #define ESC_REQUEST_TIMEOUT 100         // 100 ms (data transfer takes only 900us)
 105
 106 #define TELEMETRY_FRAME_SIZE 10
 107 static uint8_t telemetryBuffer[TELEMETRY_FRAME_SIZE] = { 0, };
 108
 109 static volatile uint8_t *buffer;
 110 static volatile uint8_t bufferSize = 0;
 111 static volatile uint8_t bufferPosition = 0;
 112
 113 static serialPort_t *escSensorPort = NULL;
 114
 115 static escSensorData_t escSensorData[MAX_SUPPORTED_MOTORS];
 116
 117 static escSensorTriggerState_t escSensorTriggerState = ESC_SENSOR_TRIGGER_STARTUP;
 118 static uint32_t escTriggerTimestamp;
 119 static uint8_t escSensorMotor = 0;      // motor index
 120
 121 static escSensorData_t combinedEscSensorData;
 122 static bool combinedDataNeedsUpdate = true;
 123
 124 static uint16_t totalTimeoutCount = 0;
 125 static uint16_t totalCrcErrorCount = 0;
 126
 127 void startEscDataRead(uint8_t *frameBuffer, uint8_t frameLength)
 128 {
 129     buffer = frameBuffer;
 130     bufferPosition = 0;
 131     bufferSize = frameLength;
 132 }
 133
 134 uint8_t getNumberEscBytesRead(void)
 135 {
 136     return bufferPosition;
 137 }
 138
 139 static bool isFrameComplete(void)
 140 {
 141     return bufferPosition == bufferSize;
 142 }
 143
 144 bool isEscSensorActive(void)
 145 {
 146     return escSensorPort != NULL;
 147 }
 148
 149 escSensorData_t *getEscSensorData(uint8_t motorNumber)
 150 {
 151     if (!feature(FEATURE_ESC_SENSOR)) {
 152         return NULL;
 153     }
 154
 155     if (motorNumber < getMotorCount()) {
 156         return &escSensorData[motorNumber];
 157     } else if (motorNumber == ESC_SENSOR_COMBINED) {
 158         if (combinedDataNeedsUpdate) {
 159             combinedEscSensorData.dataAge = 0;
 160             combinedEscSensorData.temperature = 0;
 161             combinedEscSensorData.voltage = 0;
 162             combinedEscSensorData.current = 0;
 163             combinedEscSensorData.consumption = 0;
 164             combinedEscSensorData.rpm = 0;
 165
 166             for (int i = 0; i < getMotorCount(); i = i + 1) {
 167                 combinedEscSensorData.dataAge = MAX(combinedEscSensorData.dataAge, escSensorData[i].dataAge);
 168                 combinedEscSensorData.temperature = MAX(combinedEscSensorData.temperature, escSensorData[i].temperature);
 169                 combinedEscSensorData.voltage += escSensorData[i].voltage;
 170                 combinedEscSensorData.current += escSensorData[i].current;
 171                 combinedEscSensorData.consumption += escSensorData[i].consumption;
 172                 combinedEscSensorData.rpm += escSensorData[i].rpm;
 173             }
 174
 175             combinedEscSensorData.voltage = combinedEscSensorData.voltage / getMotorCount();
 176             combinedEscSensorData.rpm = combinedEscSensorData.rpm / getMotorCount();
 177
 178             combinedDataNeedsUpdate = false;
 179
 180             DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_DATA_AGE, combinedEscSensorData.dataAge);
 181         }
 182
 183         return &combinedEscSensorData;
 184     } else {
 185         return NULL;
 186     }
 187 }
 188
 189 // Receive ISR callback
 190 static void escSensorDataReceive(uint16_t c, void *data)
 191 {
 192     UNUSED(data);
 193
 194     // KISS ESC sends some data during startup, ignore this for now (maybe future use)
 195     // startup data could be firmware version and serialnumber
 196
 197     if (isFrameComplete()) {
 198         return;
 199     }
 200
 201     buffer[bufferPosition++] = (uint8_t)c;
 202 }
 203
 204 bool escSensorInit(void)
 205 {
 206     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_ESC_SENSOR);
 207     if (!portConfig) {
 208         return false;
 209     }
 210
 211     portOptions_e options = SERIAL_NOT_INVERTED  | (escSensorConfig()->halfDuplex ? SERIAL_BIDIR : 0);
 212
 213     // Initialize serial port
 214     escSensorPort = openSerialPort(portConfig->identifier, FUNCTION_ESC_SENSOR, escSensorDataReceive, NULL, ESC_SENSOR_BAUDRATE, MODE_RX, options);
 215
 216     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i = i + 1) {
 217         escSensorData[i].dataAge = ESC_DATA_INVALID;
 218     }
 219
 220     return escSensorPort != NULL;
 221 }
 222
 223 static uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
 224 {
 225     uint8_t crc_u = crc;
 226     crc_u ^= crc_seed;
 227
 228     for (int i=0; i<8; i++) {
 229         crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
 230     }
 231
 232     return (crc_u);
 233 }
 234
 235 uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
 236 {
 237     uint8_t crc = 0;
 238     for (int i = 0; i < BufLen; i++) {
 239         crc = updateCrc8(Buf[i], crc);
 240     }
 241
 242     return crc;
 243 }
 244
 245 static uint8_t decodeEscFrame(void)
 246 {
 247     if (!isFrameComplete()) {
 248         return ESC_SENSOR_FRAME_PENDING;
 249     }
 250
 251     // Get CRC8 checksum
 252     uint16_t chksum = calculateCrc8(telemetryBuffer, TELEMETRY_FRAME_SIZE - 1);
 253     uint16_t tlmsum = telemetryBuffer[TELEMETRY_FRAME_SIZE - 1];     // last byte contains CRC value
 254     uint8_t frameStatus;
 255     if (chksum == tlmsum) {
 256         escSensorData[escSensorMotor].dataAge = 0;
 257         escSensorData[escSensorMotor].temperature = telemetryBuffer[0];
 258         escSensorData[escSensorMotor].voltage = telemetryBuffer[1] << 8 | telemetryBuffer[2];
 259         escSensorData[escSensorMotor].current = telemetryBuffer[3] << 8 | telemetryBuffer[4];
 260         escSensorData[escSensorMotor].consumption = telemetryBuffer[5] << 8 | telemetryBuffer[6];
 261         escSensorData[escSensorMotor].rpm = telemetryBuffer[7] << 8 | telemetryBuffer[8];
 262
 263         combinedDataNeedsUpdate = true;
 264
 265         frameStatus = ESC_SENSOR_FRAME_COMPLETE;
 266
 267         DEBUG_SET(DEBUG_ESC_SENSOR_RPM, escSensorMotor, (escSensorData[escSensorMotor].rpm * 10) / (motorConfig()->motorPolesCount / 2)); // output actual rpm/10 to fit in 16bit signed.
 268         DEBUG_SET(DEBUG_ESC_SENSOR_TMP, escSensorMotor, escSensorData[escSensorMotor].temperature);
 269     } else {
 270         frameStatus = ESC_SENSOR_FRAME_FAILED;
 271     }
 272
 273     return frameStatus;
 274 }
 275
 276 static void increaseDataAge(void)
 277 {
 278     if (escSensorData[escSensorMotor].dataAge < ESC_DATA_INVALID) {
 279         escSensorData[escSensorMotor].dataAge++;
 280
 281         combinedDataNeedsUpdate = true;
 282     }
 283 }
 284
 285 static void selectNextMotor(void)
 286 {
 287     escSensorMotor++;
 288     if (escSensorMotor == getMotorCount()) {
 289         escSensorMotor = 0;
 290     }
 291 }
 292
 293 void escSensorProcess(timeUs_t currentTimeUs)
 294 {
 295     const timeMs_t currentTimeMs = currentTimeUs / 1000;
 296
 297     if (!escSensorPort || !pwmAreMotorsEnabled()) {
 298         return;
 299     }
 300
 301     switch (escSensorTriggerState) {
 302         case ESC_SENSOR_TRIGGER_STARTUP:
 303             // Wait period of time before requesting telemetry (let the system boot first)
 304             if (currentTimeMs >= ESC_BOOTTIME) {
 305                 escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
 306             }
 307
 308             break;
 309         case ESC_SENSOR_TRIGGER_READY:
 310             escTriggerTimestamp = currentTimeMs;
 311
 312             startEscDataRead(telemetryBuffer, TELEMETRY_FRAME_SIZE);
 313             motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
 314             motor->requestTelemetry = true;
 315             escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
 316
 317             DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1);
 318
 319             break;
 320         case ESC_SENSOR_TRIGGER_PENDING:
 321             if (currentTimeMs < escTriggerTimestamp + ESC_REQUEST_TIMEOUT) {
 322                 uint8_t state = decodeEscFrame();
 323                 switch (state) {
 324                     case ESC_SENSOR_FRAME_COMPLETE:
 325                         selectNextMotor();
 326                         escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
 327
 328                         break;
 329                     case ESC_SENSOR_FRAME_FAILED:
 330                         increaseDataAge();
 331
 332                         selectNextMotor();
 333                         escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
 334
 335                         DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_CRC_ERRORS, ++totalCrcErrorCount);
 336                         break;
 337                     case ESC_SENSOR_FRAME_PENDING:
 338                         break;
 339                 }
 340             } else {
 341                 // Move on to next ESC, we'll come back to this one
 342                 increaseDataAge();
 343
 344                 selectNextMotor();
 345                 escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
 346
 347                 DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_TIMEOUTS, ++totalTimeoutCount);
 348             }
 349
 350             break;
 351     }
 352 }
 353 #endif