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