src/main/sensors/voltage.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
  29 #include "common/filter.h"
  30 #include "common/maths.h"
  31 #include "common/utils.h"
  32
  33 #include "config/config.h"
  34 #include "config/config_reset.h"
  35
  36 #include "drivers/adc.h"
  37
  38 #include "flight/pid.h"
  39
  40 #include "pg/pg.h"
  41 #include "pg/pg_ids.h"
  42
  43 #include "sensors/adcinternal.h"
  44 #include "sensors/battery.h"
  45 #include "sensors/esc_sensor.h"
  46
  47 #include "voltage.h"
  48
  49 const char * const voltageMeterSourceNames[VOLTAGE_METER_COUNT] = {
  50     "NONE", "ADC", "ESC"
  51 };
  52
  53 const uint8_t voltageMeterIds[] = {
  54     VOLTAGE_METER_ID_BATTERY_1,
  55 #ifdef ADC_POWER_12V
  56     VOLTAGE_METER_ID_12V_1,
  57 #endif
  58 #ifdef ADC_POWER_9V
  59     VOLTAGE_METER_ID_9V_1,
  60 #endif
  61 #ifdef ADC_POWER_5V
  62     VOLTAGE_METER_ID_5V_1,
  63 #endif
  64 #ifdef USE_ESC_SENSOR
  65     VOLTAGE_METER_ID_ESC_COMBINED_1,
  66     VOLTAGE_METER_ID_ESC_MOTOR_1,
  67     VOLTAGE_METER_ID_ESC_MOTOR_2,
  68     VOLTAGE_METER_ID_ESC_MOTOR_3,
  69     VOLTAGE_METER_ID_ESC_MOTOR_4,
  70     VOLTAGE_METER_ID_ESC_MOTOR_5,
  71     VOLTAGE_METER_ID_ESC_MOTOR_6,
  72     VOLTAGE_METER_ID_ESC_MOTOR_7,
  73     VOLTAGE_METER_ID_ESC_MOTOR_8,
  74     VOLTAGE_METER_ID_ESC_MOTOR_9,
  75     VOLTAGE_METER_ID_ESC_MOTOR_10,
  76     VOLTAGE_METER_ID_ESC_MOTOR_11,
  77     VOLTAGE_METER_ID_ESC_MOTOR_12,
  78 #endif
  79 };
  80
  81 const uint8_t supportedVoltageMeterCount = ARRAYLEN(voltageMeterIds);
  82
  83
  84 //
  85 // ADC/ESC shared
  86 //
  87
  88 void voltageMeterReset(voltageMeter_t *meter)
  89 {
  90     meter->displayFiltered = 0;
  91     meter->unfiltered = 0;
  92 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
  93     meter->sagFiltered = 0;
  94 #endif
  95 }
  96 //
  97 // ADC
  98 //
  99
 100 #ifndef VBAT_SCALE_DEFAULT
 101 #define VBAT_SCALE_DEFAULT 110
 102 #endif
 103
 104 #ifndef VBAT_RESDIVVAL_DEFAULT
 105 #define VBAT_RESDIVVAL_DEFAULT 10
 106 #endif
 107
 108 #ifndef VBAT_RESDIVMULTIPLIER_DEFAULT
 109 #define VBAT_RESDIVMULTIPLIER_DEFAULT 1
 110 #endif
 111
 112 typedef struct voltageMeterADCState_s {
 113     uint16_t voltageDisplayFiltered;         // battery voltage in 0.01V steps (filtered)
 114     uint16_t voltageUnfiltered;       // battery voltage in 0.01V steps (unfiltered)
 115     pt1Filter_t displayFilter;
 116 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 117     uint16_t voltageSagFiltered;      // battery voltage in 0.01V steps (filtered for vbat sag compensation)
 118     pt1Filter_t sagFilter;            // filter for vbat sag compensation
 119 #endif
 120 } voltageMeterADCState_t;
 121
 122 voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
 123
 124 static bool sagCompensationConfigured;
 125
 126 voltageMeterADCState_t *getVoltageMeterADC(uint8_t index)
 127 {
 128     return &voltageMeterADCStates[index];
 129 }
 130
 131 PG_REGISTER_ARRAY_WITH_RESET_FN(voltageSensorADCConfig_t, MAX_VOLTAGE_SENSOR_ADC, voltageSensorADCConfig, PG_VOLTAGE_SENSOR_ADC_CONFIG, 0);
 132
 133 void pgResetFn_voltageSensorADCConfig(voltageSensorADCConfig_t *instance)
 134 {
 135     for (int i = 0; i < MAX_VOLTAGE_SENSOR_ADC; i++) {
 136         RESET_CONFIG(voltageSensorADCConfig_t, &instance[i],
 137             .vbatscale = VBAT_SCALE_DEFAULT,
 138             .vbatresdivval = VBAT_RESDIVVAL_DEFAULT,
 139             .vbatresdivmultiplier = VBAT_RESDIVMULTIPLIER_DEFAULT,
 140         );
 141     }
 142 }
 143
 144
 145 static const uint8_t voltageMeterAdcChannelMap[] = {
 146     ADC_BATTERY,
 147 #ifdef ADC_POWER_12V
 148     ADC_POWER_12V,
 149 #endif
 150 #ifdef ADC_POWER_9V
 151     ADC_POWER_9V,
 152 #endif
 153 #ifdef ADC_POWER_5V
 154     ADC_POWER_5V,
 155 #endif
 156 };
 157
 158 STATIC_UNIT_TESTED uint16_t voltageAdcToVoltage(const uint16_t src, const voltageSensorADCConfig_t *config)
 159 {
 160     // calculate battery voltage based on ADC reading
 161     // result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 10:1 voltage divider (10k:1k) * 100 for 0.01V
 162     return ((((uint32_t)src * config->vbatscale * getVrefMv() / 10 + (0xFFF * 5)) / (0xFFF * config->vbatresdivval)) / config->vbatresdivmultiplier);
 163 }
 164
 165 void voltageMeterADCRefresh(void)
 166 {
 167     for (uint8_t i = 0; i < MAX_VOLTAGE_SENSOR_ADC && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) {
 168         voltageMeterADCState_t *state = &voltageMeterADCStates[i];
 169 #ifdef USE_ADC
 170         // store the battery voltage with some other recent battery voltage readings
 171
 172         const voltageSensorADCConfig_t *config = voltageSensorADCConfig(i);
 173
 174         uint8_t channel = voltageMeterAdcChannelMap[i];
 175         uint16_t rawSample = adcGetChannel(channel);
 176         uint16_t filteredDisplaySample = pt1FilterApply(&state->displayFilter, rawSample);
 177
 178         // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand.
 179         state->voltageDisplayFiltered = voltageAdcToVoltage(filteredDisplaySample, config);
 180         state->voltageUnfiltered = voltageAdcToVoltage(rawSample, config);
 181
 182 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 183         if (isSagCompensationConfigured()) {
 184             uint16_t filteredSagSample = pt1FilterApply(&state->sagFilter, rawSample);
 185             state->voltageSagFiltered = voltageAdcToVoltage(filteredSagSample, config);
 186         }
 187 #endif
 188 #else
 189         UNUSED(voltageAdcToVoltage);
 190
 191         state->voltageDisplayFiltered = 0;
 192         state->voltageUnfiltered = 0;
 193 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 194         state->voltageSagFiltered = 0;
 195 #endif
 196 #endif
 197     }
 198 }
 199
 200 void voltageMeterADCRead(voltageSensorADC_e adcChannel, voltageMeter_t *voltageMeter)
 201 {
 202     voltageMeterADCState_t *state = &voltageMeterADCStates[adcChannel];
 203
 204     voltageMeter->displayFiltered = state->voltageDisplayFiltered;
 205     voltageMeter->unfiltered = state->voltageUnfiltered;
 206 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 207     voltageMeter->sagFiltered = state->voltageSagFiltered;
 208 #endif
 209 }
 210
 211 bool isSagCompensationConfigured(void)
 212 {
 213     return sagCompensationConfigured;
 214 }
 215
 216 void voltageMeterADCInit(void)
 217 {
 218     for (uint8_t i = 0; i < MAX_VOLTAGE_SENSOR_ADC && i < ARRAYLEN(voltageMeterAdcChannelMap); i++) {
 219         // store the battery voltage with some other recent battery voltage readings
 220
 221         voltageMeterADCState_t *state = &voltageMeterADCStates[i];
 222         memset(state, 0, sizeof(voltageMeterADCState_t));
 223
 224         pt1FilterInit(&state->displayFilter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatDisplayLpfPeriod), HZ_TO_INTERVAL(isSagCompensationConfigured() ? FAST_VOLTAGE_TASK_FREQ_HZ : SLOW_VOLTAGE_TASK_FREQ_HZ)));
 225 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 226         if (isSagCompensationConfigured()) {
 227             pt1FilterInit(&state->sagFilter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatSagLpfPeriod), HZ_TO_INTERVAL(FAST_VOLTAGE_TASK_FREQ_HZ)));
 228         }
 229 #endif
 230     }
 231 }
 232
 233 void voltageMeterGenericInit(void)
 234 {
 235     sagCompensationConfigured = false;
 236 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 237     for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
 238         if (pidProfiles(i)->vbat_sag_compensation > 0) {
 239             sagCompensationConfigured = true;
 240         }
 241     }
 242 #endif
 243 }
 244
 245 //
 246 // ESC
 247 //
 248
 249 #ifdef USE_ESC_SENSOR
 250 typedef struct voltageMeterESCState_s {
 251     uint16_t voltageDisplayFiltered;         // battery voltage in 0.01V steps (filtered)
 252     uint16_t voltageUnfiltered;       // battery voltage in 0.01V steps (unfiltered)
 253     pt1Filter_t displayFilter;
 254 } voltageMeterESCState_t;
 255
 256 static voltageMeterESCState_t voltageMeterESCState;
 257 #endif
 258
 259
 260
 261 void voltageMeterESCInit(void)
 262 {
 263 #ifdef USE_ESC_SENSOR
 264     memset(&voltageMeterESCState, 0, sizeof(voltageMeterESCState_t));
 265     pt1FilterInit(&voltageMeterESCState.displayFilter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatDisplayLpfPeriod), HZ_TO_INTERVAL(isSagCompensationConfigured() ? FAST_VOLTAGE_TASK_FREQ_HZ : SLOW_VOLTAGE_TASK_FREQ_HZ)));
 266 #endif
 267 }
 268
 269 void voltageMeterESCRefresh(void)
 270 {
 271 #ifdef USE_ESC_SENSOR
 272     escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
 273     if (escData) {
 274         voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage : 0;
 275         voltageMeterESCState.voltageDisplayFiltered = pt1FilterApply(&voltageMeterESCState.displayFilter, voltageMeterESCState.voltageUnfiltered);
 276     }
 277 #endif
 278 }
 279
 280 void voltageMeterESCReadMotor(uint8_t motorNumber, voltageMeter_t *voltageMeter)
 281 {
 282 #ifndef USE_ESC_SENSOR
 283     UNUSED(motorNumber);
 284     voltageMeterReset(voltageMeter);
 285 #else
 286     escSensorData_t *escData = getEscSensorData(motorNumber);
 287     if (escData) {
 288         voltageMeter->unfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage : 0;
 289         voltageMeter->displayFiltered = voltageMeter->unfiltered; // no filtering for ESC motors currently.
 290     } else {
 291         voltageMeterReset(voltageMeter);
 292     }
 293
 294 #endif
 295 }
 296
 297 void voltageMeterESCReadCombined(voltageMeter_t *voltageMeter)
 298 {
 299 #ifndef USE_ESC_SENSOR
 300     voltageMeterReset(voltageMeter);
 301 #else
 302     voltageMeter->displayFiltered = voltageMeterESCState.voltageDisplayFiltered;
 303     voltageMeter->unfiltered = voltageMeterESCState.voltageUnfiltered;
 304 #endif
 305 }
 306
 307 //
 308 // API for using voltage meters using IDs
 309 //
 310 // This API is used by MSP, for configuration/status.
 311 //
 312
 313
 314 // the order of these much match the indexes in voltageSensorADC_e
 315 const uint8_t voltageMeterADCtoIDMap[MAX_VOLTAGE_SENSOR_ADC] = {
 316     VOLTAGE_METER_ID_BATTERY_1,
 317 #ifdef ADC_POWER_12V
 318     VOLTAGE_METER_ID_12V_1,
 319 #endif
 320 #ifdef ADC_POWER_9V
 321     VOLTAGE_METER_ID_9V_1,
 322 #endif
 323 #ifdef ADC_POWER_5V
 324     VOLTAGE_METER_ID_5V_1,
 325 #endif
 326 };
 327
 328 void voltageMeterRead(voltageMeterId_e id, voltageMeter_t *meter)
 329 {
 330     if (id == VOLTAGE_METER_ID_BATTERY_1) {
 331         voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, meter);
 332     } else
 333 #ifdef ADC_POWER_12V
 334     if (id == VOLTAGE_METER_ID_12V_1) {
 335         voltageMeterADCRead(VOLTAGE_SENSOR_ADC_12V, meter);
 336     } else
 337 #endif
 338 #ifdef ADC_POWER_9V
 339     if (id == VOLTAGE_METER_ID_9V_1) {
 340         voltageMeterADCRead(VOLTAGE_SENSOR_ADC_9V, meter);
 341     } else
 342 #endif
 343 #ifdef ADC_POWER_5V
 344     if (id == VOLTAGE_METER_ID_5V_1) {
 345         voltageMeterADCRead(VOLTAGE_SENSOR_ADC_5V, meter);
 346     } else
 347 #endif
 348 #ifdef USE_ESC_SENSOR
 349     if (id == VOLTAGE_METER_ID_ESC_COMBINED_1) {
 350         voltageMeterESCReadCombined(meter);
 351     } else
 352     if (id >= VOLTAGE_METER_ID_ESC_MOTOR_1 && id <= VOLTAGE_METER_ID_ESC_MOTOR_20 ) {
 353         int motor = id - VOLTAGE_METER_ID_ESC_MOTOR_1;
 354         voltageMeterESCReadMotor(motor, meter);
 355     } else
 356 #endif
 357     {
 358         voltageMeterReset(meter);
 359     }
 360 }