src/main/sensors/current.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 "string.h"
  24
  25 #include "platform.h"
  26
  27 #include "build/build_config.h"
  28 #include "build/debug.h"
  29
  30 #include "common/maths.h"
  31 #include "common/utils.h"
  32 #include "common/filter.h"
  33
  34 #include "drivers/adc.h"
  35
  36 #include "pg/pg.h"
  37 #include "pg/pg_ids.h"
  38
  39 #include "sensors/adcinternal.h"
  40 #include "sensors/battery.h"
  41 #include "sensors/esc_sensor.h"
  42
  43 #include "current.h"
  44
  45 const char * const currentMeterSourceNames[CURRENT_METER_COUNT] = {
  46     "NONE", "ADC", "VIRTUAL", "ESC", "MSP"
  47 };
  48
  49 const uint8_t currentMeterIds[] = {
  50     CURRENT_METER_ID_BATTERY_1,
  51 #ifdef USE_VIRTUAL_CURRENT_METER
  52     CURRENT_METER_ID_VIRTUAL_1,
  53 #endif
  54 #ifdef USE_ESC_SENSOR
  55     CURRENT_METER_ID_ESC_COMBINED_1,
  56     CURRENT_METER_ID_ESC_MOTOR_1,
  57     CURRENT_METER_ID_ESC_MOTOR_2,
  58     CURRENT_METER_ID_ESC_MOTOR_3,
  59     CURRENT_METER_ID_ESC_MOTOR_4,
  60     CURRENT_METER_ID_ESC_MOTOR_5,
  61     CURRENT_METER_ID_ESC_MOTOR_6,
  62     CURRENT_METER_ID_ESC_MOTOR_7,
  63     CURRENT_METER_ID_ESC_MOTOR_8,
  64     CURRENT_METER_ID_ESC_MOTOR_9,
  65     CURRENT_METER_ID_ESC_MOTOR_10,
  66     CURRENT_METER_ID_ESC_MOTOR_11,
  67     CURRENT_METER_ID_ESC_MOTOR_12,
  68 #endif
  69 #ifdef USE_MSP_CURRENT_METER
  70     CURRENT_METER_ID_MSP_1,
  71 #endif
  72 };
  73
  74 const uint8_t supportedCurrentMeterCount = ARRAYLEN(currentMeterIds);
  75
  76 //
  77 // ADC/Virtual/ESC/MSP shared
  78 //
  79
  80 void currentMeterReset(currentMeter_t *meter)
  81 {
  82     meter->amperage = 0;
  83     meter->amperageLatest = 0;
  84     meter->mAhDrawn = 0;
  85 }
  86
  87 //
  88 // ADC/Virtual shared
  89 //
  90
  91 static pt1Filter_t adciBatFilter;
  92
  93 #ifndef CURRENT_METER_SCALE_DEFAULT
  94 #define CURRENT_METER_SCALE_DEFAULT 400 // for Allegro ACS758LCB-100U (40mV/A)
  95 #endif
  96
  97 #ifndef CURRENT_METER_OFFSET_DEFAULT
  98 #define CURRENT_METER_OFFSET_DEFAULT 0
  99 #endif
 100
 101 PG_REGISTER_WITH_RESET_TEMPLATE(currentSensorADCConfig_t, currentSensorADCConfig, PG_CURRENT_SENSOR_ADC_CONFIG, 0);
 102
 103 PG_RESET_TEMPLATE(currentSensorADCConfig_t, currentSensorADCConfig,
 104     .scale = CURRENT_METER_SCALE_DEFAULT,
 105     .offset = CURRENT_METER_OFFSET_DEFAULT,
 106 );
 107
 108 #ifdef USE_VIRTUAL_CURRENT_METER
 109 PG_REGISTER(currentSensorVirtualConfig_t, currentSensorVirtualConfig, PG_CURRENT_SENSOR_VIRTUAL_CONFIG, 0);
 110 #endif
 111
 112 static int32_t currentMeterADCToCentiamps(const uint16_t src)
 113 {
 114
 115     const currentSensorADCConfig_t *config = currentSensorADCConfig();
 116
 117     int32_t millivolts = ((uint32_t)src * getVrefMv()) / 4096;
 118     // y=x/m+b m is scale in (mV/10A) and b is offset in (mA)
 119     int32_t centiAmps = config->scale ? (millivolts * 10000 / (int32_t)config->scale + (int32_t)config->offset) / 10 : 0;
 120
 121     DEBUG_SET(DEBUG_CURRENT_SENSOR, 0, millivolts);
 122     DEBUG_SET(DEBUG_CURRENT_SENSOR, 1, centiAmps);
 123
 124     return centiAmps; // Returns Centiamps to maintain compatability with the rest of the code
 125 }
 126
 127 #if defined(USE_ADC) || defined(USE_VIRTUAL_CURRENT_METER)
 128 static void updateCurrentmAhDrawnState(currentMeterMAhDrawnState_t *state, int32_t amperageLatest, int32_t lastUpdateAt)
 129 {
 130     state->mAhDrawnF = state->mAhDrawnF + (amperageLatest * lastUpdateAt / (100.0f * 1000 * 3600));
 131     state->mAhDrawn = state->mAhDrawnF;
 132 }
 133 #endif
 134
 135 //
 136 // ADC
 137 //
 138
 139 currentMeterADCState_t currentMeterADCState;
 140
 141 void currentMeterADCInit(void)
 142 {
 143     memset(&currentMeterADCState, 0, sizeof(currentMeterADCState_t));
 144     pt1FilterInit(&adciBatFilter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->ibatLpfPeriod), HZ_TO_INTERVAL(50)));
 145 }
 146
 147 void currentMeterADCRefresh(int32_t lastUpdateAt)
 148 {
 149 #ifdef USE_ADC
 150     const uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
 151     currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample);
 152     currentMeterADCState.amperage = currentMeterADCToCentiamps(pt1FilterApply(&adciBatFilter, iBatSample));
 153
 154     updateCurrentmAhDrawnState(&currentMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt);
 155 #else
 156     UNUSED(lastUpdateAt);
 157     UNUSED(currentMeterADCToCentiamps);
 158
 159     currentMeterADCState.amperageLatest = 0;
 160     currentMeterADCState.amperage = 0;
 161 #endif
 162 }
 163
 164 void currentMeterADCRead(currentMeter_t *meter)
 165 {
 166     meter->amperageLatest = currentMeterADCState.amperageLatest;
 167     meter->amperage = currentMeterADCState.amperage;
 168     meter->mAhDrawn = currentMeterADCState.mahDrawnState.mAhDrawn;
 169
 170     DEBUG_SET(DEBUG_CURRENT_SENSOR, 2, meter->amperageLatest);
 171     DEBUG_SET(DEBUG_CURRENT_SENSOR, 3, meter->mAhDrawn);
 172 }
 173
 174 //
 175 // VIRTUAL
 176 //
 177
 178 #ifdef USE_VIRTUAL_CURRENT_METER
 179 currentSensorVirtualState_t currentMeterVirtualState;
 180
 181 void currentMeterVirtualInit(void)
 182 {
 183     memset(&currentMeterVirtualState, 0, sizeof(currentSensorVirtualState_t));
 184 }
 185
 186 void currentMeterVirtualRefresh(int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset)
 187 {
 188     currentMeterVirtualState.amperage = (int32_t)currentSensorVirtualConfig()->offset;
 189     if (armed) {
 190         if (throttleLowAndMotorStop) {
 191             throttleOffset = 0;
 192         }
 193
 194         int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); // FIXME magic number 50. Possibly use thrustLinearization if configured.
 195         currentMeterVirtualState.amperage += throttleFactor * (int32_t)currentSensorVirtualConfig()->scale / 1000;
 196     }
 197     updateCurrentmAhDrawnState(&currentMeterVirtualState.mahDrawnState, currentMeterVirtualState.amperage, lastUpdateAt);
 198 }
 199
 200 void currentMeterVirtualRead(currentMeter_t *meter)
 201 {
 202     meter->amperageLatest = currentMeterVirtualState.amperage;
 203     meter->amperage = currentMeterVirtualState.amperage;
 204     meter->mAhDrawn = currentMeterVirtualState.mahDrawnState.mAhDrawn;
 205 }
 206 #endif
 207
 208 //
 209 // ESC
 210 //
 211
 212 #ifdef USE_ESC_SENSOR
 213 currentMeterESCState_t currentMeterESCState;
 214
 215 void currentMeterESCInit(void)
 216 {
 217     memset(&currentMeterESCState, 0, sizeof(currentMeterESCState_t));
 218 }
 219
 220 void currentMeterESCRefresh(int32_t lastUpdateAt)
 221 {
 222     UNUSED(lastUpdateAt);
 223
 224     escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
 225     if (escData && escData->dataAge <= ESC_BATTERY_AGE_MAX) {
 226         currentMeterESCState.amperage = escData->current + escSensorConfig()->offset / 10;
 227         currentMeterESCState.mAhDrawn = escData->consumption + escSensorConfig()->offset * millis() / (1000.0f * 3600);
 228     } else {
 229         currentMeterESCState.amperage = 0;
 230         currentMeterESCState.mAhDrawn = 0;
 231     }
 232 }
 233
 234 void currentMeterESCReadCombined(currentMeter_t *meter)
 235 {
 236     meter->amperageLatest = currentMeterESCState.amperage;
 237     meter->amperage = currentMeterESCState.amperage;
 238     meter->mAhDrawn = currentMeterESCState.mAhDrawn;
 239 }
 240
 241 void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter)
 242 {
 243     escSensorData_t *escData = getEscSensorData(motorNumber);
 244     if (escData && escData->dataAge <= ESC_BATTERY_AGE_MAX) {
 245         meter->amperage = escData->current;
 246         meter->amperageLatest = escData->current;
 247         meter->mAhDrawn = escData->consumption;
 248     } else {
 249         currentMeterReset(meter);
 250     }
 251 }
 252 #endif
 253
 254
 255 #ifdef USE_MSP_CURRENT_METER
 256 #include "common/streambuf.h"
 257
 258 #include "msp/msp_protocol.h"
 259 #include "msp/msp_serial.h"
 260
 261 currentMeterMSPState_t currentMeterMSPState;
 262
 263 void currentMeterMSPSet(uint16_t amperage, uint16_t mAhDrawn)
 264 {
 265     // We expect the FC's MSP_ANALOG response handler to call this function
 266     currentMeterMSPState.amperage = amperage;
 267     currentMeterMSPState.mAhDrawn = mAhDrawn;
 268 }
 269
 270 void currentMeterMSPInit(void)
 271 {
 272     memset(&currentMeterMSPState, 0, sizeof(currentMeterMSPState_t));
 273 }
 274
 275 void currentMeterMSPRefresh(timeUs_t currentTimeUs)
 276 {
 277     // periodically request MSP_ANALOG
 278     static timeUs_t streamRequestAt = 0;
 279     if (cmp32(currentTimeUs, streamRequestAt) > 0) {
 280         streamRequestAt = currentTimeUs + ((1000 * 1000) / 10); // 10hz
 281
 282         mspSerialPush(SERIAL_PORT_ALL, MSP_ANALOG, NULL, 0, MSP_DIRECTION_REQUEST);
 283     }
 284 }
 285
 286 void currentMeterMSPRead(currentMeter_t *meter)
 287 {
 288     meter->amperageLatest = currentMeterMSPState.amperage;
 289     meter->amperage = currentMeterMSPState.amperage;
 290     meter->mAhDrawn = currentMeterMSPState.mAhDrawn;
 291 }
 292 #endif
 293
 294 //
 295 // API for current meters using IDs
 296 //
 297 // This API is used by MSP, for configuration/status.
 298 //
 299
 300 void currentMeterRead(currentMeterId_e id, currentMeter_t *meter)
 301 {
 302     if (id == CURRENT_METER_ID_BATTERY_1) {
 303         currentMeterADCRead(meter);
 304     }
 305 #ifdef USE_VIRTUAL_CURRENT_METER
 306     else if (id == CURRENT_METER_ID_VIRTUAL_1) {
 307         currentMeterVirtualRead(meter);
 308     }
 309 #endif
 310 #ifdef USE_MSP_CURRENT_METER
 311     else if (id == CURRENT_METER_ID_MSP_1) {
 312         currentMeterMSPRead(meter);
 313     }
 314 #endif
 315 #ifdef USE_ESC_SENSOR
 316     else if (id == CURRENT_METER_ID_ESC_COMBINED_1) {
 317         currentMeterESCReadCombined(meter);
 318     }
 319     else if (id >= CURRENT_METER_ID_ESC_MOTOR_1 && id <= CURRENT_METER_ID_ESC_MOTOR_20 ) {
 320         int motor = id - CURRENT_METER_ID_ESC_MOTOR_1;
 321         currentMeterESCReadMotor(motor, meter);
 322     }
 323 #endif
 324     else {
 325         currentMeterReset(meter);
 326     }
 327 }