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