flybrix-firmware/power.cpp at master · Flybrix/flybrix-firmware · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
/*
    *  Flybrix Flight Controller -- Copyright 2018 Flying Selfie Inc. d/b/a Flybrix
    *
    *  http://www.flybrix.com
*/

#include "power.h"
#include "board.h"

PowerMonitor::PowerMonitor() {
    // REFERENCE: https://github.com/pedvide/ADC
    pinMode(board::V0_DETECT, INPUT);
    pinMode(board::I0_DETECT, INPUT);
    pinMode(board::I1_DETECT, INPUT);

    adc.setReference(ADC_REFERENCE::REF_1V2, ADC_0);
    adc.setAveraging(1, ADC_0);                                       // set number of averages
    adc.setResolution(16, ADC_0);                                     // set bits of resolution
    adc.setConversionSpeed(ADC_CONVERSION_SPEED::HIGH_SPEED, ADC_0);  // change the conversion speed
    adc.setSamplingSpeed(ADC_SAMPLING_SPEED::HIGH_SPEED, ADC_0);      // change the sampling speed

    adc.setReference(BOARD_ADC_REF, ADC_1);
    adc.setAveraging(1, ADC_1);                                       // set number of averages
    adc.setResolution(16, ADC_1);                                     // set bits of resolution
    adc.setConversionSpeed(ADC_CONVERSION_SPEED::HIGH_SPEED, ADC_1);  // change the conversion speed
    adc.setSamplingSpeed(ADC_SAMPLING_SPEED::HIGH_SPEED, ADC_1);      // change the sampling speed
}

void PowerMonitor::updateLevels(void) {
    V0_ = getV0Raw();
    I0_ = getI0Raw();
    I1_ = getI1Raw();
}

float PowerMonitor::totalPower(void) const {
    return I0() * V0();
}

float PowerMonitor::electronicsPower(void) const {
    return I1() * 3.7;
}

uint16_t PowerMonitor::getV0Raw(void) {
    return (uint16_t)adc.analogRead(board::V0_DETECT, ADC_1);
}

uint16_t PowerMonitor::getI0Raw(void) {
    return (uint16_t)adc.analogRead(board::I0_DETECT, ADC_0);
}

uint16_t PowerMonitor::getI1Raw(void) {
    return (uint16_t)adc.analogRead(board::I1_DETECT, ADC_0);
}

namespace {
constexpr float calculateVoltageScale(float v_ref, float r_in, float r_out, float int_max) {
    return v_ref * r_in / r_out / int_max;
}

constexpr float calculateCurrentScale(float v_ref, float ic_scaling, float r, float int_max) {
    return v_ref / ic_scaling / r / int_max;
}

constexpr float V0_SCALE = calculateVoltageScale(3.3, 150 + 150, 150, 65536);
constexpr float I0_SCALE = calculateCurrentScale(3.3, 50, 0.005, 65536);
constexpr float I1_SCALE = calculateCurrentScale(3.3, 50, 0.033, 65536);
}

float PowerMonitor::V0(void) const {
    return V0_SCALE * V0_;
}

float PowerMonitor::I0(void) const {
    return I0_SCALE * I0_;
}

float PowerMonitor::I1(void) const {
    return I1_SCALE * I1_;
}