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