src/main/sensors/esc_sensor.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   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/.
   7  *
   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:
  10  *
  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.
  15  *
  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.
  20  *
  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/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <ctype.h>
  28 #include <math.h>
  29
  30 #include "platform.h"
  31
  32 #include "build/build_config.h"
  33 #include "build/debug.h"
  34
  35 #include "common/maths.h"
  36 #include "common/crc.h"
  37
  38 #include "config/feature.h"
  39 #include "config/config_reset.h"
  40 #include "config/parameter_group.h"
  41 #include "config/parameter_group_ids.h"
  42
  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"
  50
  51
  52 #if defined(USE_ESC_SENSOR)
  53
  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
  58
  59 typedef enum {
  60     ESC_SENSOR_WAIT_STARTUP = 0,
  61     ESC_SENSOR_READY = 1,
  62     ESC_SENSOR_WAITING = 2
  63 } escSensorState_t;
  64
  65 typedef enum {
  66     ESC_SENSOR_FRAME_PENDING,
  67     ESC_SENSOR_FRAME_COMPLETE,
  68     ESC_SENSOR_FRAME_FAILED
  69 } escSensorFrameStatus_t;
  70
  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;
  80
  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,
  85 );
  86
  87 static int getTelemetryMotorCount(void)
  88 {
  89     if (escSensorConfig()->listenOnly) {
  90         return 1;
  91     }
  92     else {
  93         return getMotorCount();
  94     }
  95 }
  96
  97 static void escSensorSelectNextMotor(void)
  98 {
  99     if (escSensorConfig()->listenOnly) {
 100         escSensorMotor = 0;
 101     }
 102     else {
 103         escSensorMotor = (escSensorMotor + 1) % getTelemetryMotorCount();
 104     }
 105 }
 106
 107 static void escSensorIncreaseDataAge(void)
 108 {
 109     if (escSensorData[escSensorMotor].dataAge < ESC_DATA_INVALID) {
 110         escSensorData[escSensorMotor].dataAge++;
 111         escSensorDataNeedsUpdate = true;
 112     }
 113 }
 114
 115 static bool escSensorDecodeFrame(void)
 116 {
 117     // Receive bytes
 118     while (serialRxBytesWaiting(escSensorPort) > 0) {
 119         uint8_t c = serialRead(escSensorPort);
 120
 121         if (bufferPosition < TELEMETRY_FRAME_SIZE) {
 122             telemetryBuffer[bufferPosition++] = c;
 123         }
 124     }
 125
 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;
 136
 137             return ESC_SENSOR_FRAME_COMPLETE;
 138         }
 139         else {
 140             // CRC error
 141             return ESC_SENSOR_FRAME_FAILED;
 142         }
 143     }
 144
 145     return ESC_SENSOR_FRAME_PENDING;
 146 }
 147
 148 uint32_t computeRpm(int16_t erpm) {
 149     return lrintf((float)erpm * ERPM_PER_LSB / (motorConfig()->motorPoleCount / 2));
 150 }
 151
 152 escSensorData_t NOINLINE * getEscTelemetry(uint8_t esc)
 153 {
 154     return &escSensorData[esc];
 155 }
 156
 157 escSensorData_t * escSensorGetData(void)
 158 {
 159     if (!escSensorPort) {
 160         return NULL;
 161     }
 162
 163     if (escSensorDataNeedsUpdate) {
 164         escSensorDataCombined.dataAge = 0;
 165         escSensorDataCombined.temperature = 0;
 166         escSensorDataCombined.voltage = 0;
 167         escSensorDataCombined.current = 0;
 168         escSensorDataCombined.rpm = 0;
 169
 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;
 180             }
 181         }
 182
 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;
 188         }
 189         else {
 190             escSensorDataCombined.dataAge = ESC_DATA_INVALID;
 191         }
 192
 193         escSensorDataNeedsUpdate = false;
 194     }
 195
 196     // Return NULL if sensors are old
 197     if (escSensorDataCombined.dataAge >= ESC_DATA_INVALID) {
 198         return NULL;
 199     }
 200     else {
 201         return &escSensorDataCombined;
 202     }
 203 }
 204
 205 bool escSensorInitialize(void)
 206 {
 207     escSensorDataNeedsUpdate = true;
 208     escSensorPort = NULL;
 209
 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;
 213     }
 214
 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;
 220     }
 221
 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;
 225     }
 226
 227     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 228         escSensorData[i].dataAge = ESC_DATA_INVALID;
 229     }
 230
 231     ENABLE_STATE(ESC_SENSOR_ENABLED);
 232
 233     return true;
 234 }
 235
 236 void escSensorUpdate(timeUs_t currentTimeUs)
 237 {
 238     if (!escSensorPort) {
 239         return;
 240     }
 241
 242     const timeMs_t currentTimeMs = currentTimeUs / 1000;
 243
 244     switch (escSensorState) {
 245         case ESC_SENSOR_WAIT_STARTUP:
 246             if (currentTimeMs > ESC_BOOTTIME_MS) {
 247                 escSensorMotor = 0;
 248                 escSensorState = ESC_SENSOR_READY;
 249             }
 250             break;
 251
 252         case ESC_SENSOR_READY:
 253             if (!escSensorConfig()->listenOnly) {
 254                 pwmRequestMotorTelemetry(escSensorMotor);
 255             }
 256             bufferPosition = 0;
 257             escTriggerTimeMs = currentTimeMs;
 258             escSensorState = ESC_SENSOR_WAITING;
 259             break;
 260
 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;
 267             }
 268             else {
 269                 // Receive serial data and decode frame
 270                 escSensorFrameStatus_t status = escSensorDecodeFrame();
 271
 272                 switch (status) {
 273                     case ESC_SENSOR_FRAME_COMPLETE:
 274                         escSensorSelectNextMotor();
 275                         escSensorState = ESC_SENSOR_READY;
 276                         break;
 277
 278                     case ESC_SENSOR_FRAME_FAILED:
 279                         escSensorIncreaseDataAge();
 280                         escSensorSelectNextMotor();
 281                         escSensorState = ESC_SENSOR_READY;
 282                         break;
 283
 284                     case ESC_SENSOR_FRAME_PENDING:
 285                     default:
 286                         break;
 287                 }
 288             }
 289             break;
 290
 291     }
 292
 293 }
 294
 295 #endif