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