amiro-os / devices / PowerManagement / PowerManagement.cpp @ f336542d
History | View | Annotate | Download (6.7 KB)
1 | 58fe0e0b | Thomas Schöpping | #include "ch.hpp" |
---|---|---|---|
2 | #include "hal.h" |
||
3 | #include "PowerManagement.h" |
||
4 | |||
5 | #include <amiro/proximity/vcnl4020.hpp> |
||
6 | #include <global.hpp> |
||
7 | |||
8 | #include <algorithm> |
||
9 | #include <chprintf.h> |
||
10 | |||
11 | using namespace chibios_rt; |
||
12 | using namespace amiro; |
||
13 | |||
14 | extern Global global;
|
||
15 | |||
16 | PowerManagement::PowerManagement(CANDriver *can) |
||
17 | : ControllerAreaNetworkTx(can, CAN::POWER_MANAGEMENT_ID), |
||
18 | ControllerAreaNetworkRx(can, CAN::POWER_MANAGEMENT_ID), |
||
19 | bc_counter(0)
|
||
20 | { |
||
21 | this->powerStatus.charging_flags.value = 0; |
||
22 | } |
||
23 | |||
24 | msg_t PowerManagement::receiveMessage(CANRxFrame *frame) { |
||
25 | int deviceId = this->decodeDeviceId(frame); |
||
26 | switch (deviceId) {
|
||
27 | |||
28 | case CAN::SHELL_REPLY_ID(CAN::POWER_MANAGEMENT_ID):
|
||
29 | if (frame->DLC > 0) { |
||
30 | sdWrite(&SD1, frame->data8, frame->DLC); |
||
31 | return RDY_OK;
|
||
32 | } |
||
33 | break;
|
||
34 | |||
35 | case CAN::SHELL_QUERY_ID(CAN::POWER_MANAGEMENT_ID):
|
||
36 | if (frame->DLC != 0) { |
||
37 | global.sercanmux1.convCan2Serial(frame->data8, frame->DLC); |
||
38 | return RDY_OK;
|
||
39 | } else {
|
||
40 | global.sercanmux1.rcvSwitchCmd(this->decodeBoardId(frame));
|
||
41 | return RDY_OK;
|
||
42 | } |
||
43 | break;
|
||
44 | |||
45 | case CAN::CALIBRATE_PROXIMITY_RING:
|
||
46 | // Dont care about the payload but start the calibration
|
||
47 | // TODO Care about the payload. Differ between:
|
||
48 | // 1: Do fresh calibration (Save values to memory and to temporary values)
|
||
49 | // 2: Remove temporary Calibration and get uncalibrated values
|
||
50 | // 3: Load calibration from memory
|
||
51 | this->calibrate();
|
||
52 | break;
|
||
53 | |||
54 | case CAN::ROBOT_ID:
|
||
55 | if (frame->DLC == 1) { |
||
56 | this->robotId = frame->data8[0]; |
||
57 | return RDY_OK;
|
||
58 | } |
||
59 | break;
|
||
60 | |||
61 | default:
|
||
62 | break;
|
||
63 | } |
||
64 | |||
65 | return -1; |
||
66 | } |
||
67 | |||
68 | msg_t PowerManagement::updateSensorVal() { |
||
69 | |||
70 | // update charger status
|
||
71 | this->powerStatus.charging_flags.content.powermanagement_plugged_in = global.ltc4412.isPluggedIn();
|
||
72 | |||
73 | // update fuel gauges values
|
||
74 | const BQ27500::Driver::UpdateData* power[2] { |
||
75 | &global.bq27500[constants::PowerManagement::BAT_A].getStatus(), |
||
76 | &global.bq27500[constants::PowerManagement::BAT_B].getStatus() |
||
77 | }; |
||
78 | this->powerStatus.charging_flags.content.powermanagement_charging = (this->powerStatus.charging_flags.content.powermanagement_plugged_in && |
||
79 | this->powerStatus.charging_flags.content.vsys_higher_than_9V &&
|
||
80 | power[0]->minutes_to_empty == uint16_t(~0) && |
||
81 | power[1]->minutes_to_empty == uint16_t(~0))? |
||
82 | true : false; |
||
83 | this->powerStatus.charging_flags.content.diwheeldrive_charging = (this->powerStatus.charging_flags.content.diwheeldrive_enable_power_path && |
||
84 | this->powerStatus.charging_flags.content.vsys_higher_than_9V &&
|
||
85 | power[0]->minutes_to_empty == uint16_t(~0) && |
||
86 | power[1]->minutes_to_empty == uint16_t(~0))? |
||
87 | true : false; |
||
88 | this->powerStatus.state_of_charge = (power[0]->state_of_charge + power[1]->state_of_charge) / 2; |
||
89 | if (this->powerStatus.charging_flags.content.powermanagement_charging || this->powerStatus.charging_flags.content.diwheeldrive_charging) { |
||
90 | /*
|
||
91 | * Assumption:
|
||
92 | * When charging there is enough power available to charge both batteries at full rate simultaneously.
|
||
93 | * Thus, the second battery will not charge faster when the first battery is fully charged.
|
||
94 | */
|
||
95 | this->powerStatus.minutes_remaining = std::max(power[0]->minutes_to_full, power[1]->minutes_to_full); |
||
96 | } else {
|
||
97 | /*
|
||
98 | * Computation of the remaining discharging time:
|
||
99 | * Take the time until the first of the two batteries is empty and add the remaining time of the second battery but half.
|
||
100 | * time = min(a,b) + (max(a,b) - min(a,b))/2
|
||
101 | * <=> 2*time = 2*min(a,b) + max(a,b) - min(a,b)
|
||
102 | * <=> 2*time = min(a,b) + max(a,b)
|
||
103 | * <=> 2*time = a + b
|
||
104 | * <=> time = (a + b)/2
|
||
105 | */
|
||
106 | this->powerStatus.minutes_remaining = (power[0]->minutes_to_empty + power[1]->minutes_to_empty) / 2; |
||
107 | } |
||
108 | this->powerStatus.power_consumption = (power[0]->average_power_mW + power[1]->average_power_mW) / 2; |
||
109 | |||
110 | // update infrared sensor value
|
||
111 | // Note: The CANRx Value will never be updated in this thread
|
||
112 | for (int idx = 0; idx < 8; idx++) |
||
113 | this->proximityRingValue[idx] = global.vcnl4020[idx].getProximityScaledWoOffset();
|
||
114 | |||
115 | return 0; |
||
116 | } |
||
117 | |||
118 | void PowerManagement::periodicBroadcast() {
|
||
119 | CANTxFrame frame; |
||
120 | if (this->bc_counter % 10 == 0) { |
||
121 | frame.SID = 0;
|
||
122 | this->encodeDeviceId(&frame, CAN::POWER_STATUS_ID);
|
||
123 | frame.data8[0] = this->powerStatus.charging_flags.value; |
||
124 | frame.data8[1] = this->powerStatus.state_of_charge; |
||
125 | frame.data16[1] = this->powerStatus.minutes_remaining; |
||
126 | frame.data16[2] = this->powerStatus.power_consumption; |
||
127 | frame.DLC = 6;
|
||
128 | this->transmitMessage(&frame);
|
||
129 | } |
||
130 | for (int i = 0; i < 8; i++) { |
||
131 | frame.SID = 0;
|
||
132 | this->encodeDeviceId(&frame, CAN::PROXIMITY_RING_ID(i));
|
||
133 | frame.data16[0] = this->proximityRingValue[i]; |
||
134 | frame.DLC = 2;
|
||
135 | this->transmitMessage(&frame);
|
||
136 | b4885314 | Thomas Schöpping | BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee |
137 | 58fe0e0b | Thomas Schöpping | } |
138 | ++this->bc_counter;
|
||
139 | } |
||
140 | |||
141 | void PowerManagement::calibrate() {
|
||
142 | // Stop sending and receiving of values to indicate the calibration phase
|
||
143 | // eventTimerEvtSource->unregister(&this->eventTimerEvtListener);
|
||
144 | // rxFullCanEvtSource->unregister(&this->rxFullCanEvtListener);
|
||
145 | |||
146 | this->calibrateProximityRingValues();
|
||
147 | |||
148 | // Start sending and receving of values
|
||
149 | // eventTimerEvtSource->registerOne(&this->eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
|
||
150 | // rxFullCanEvtSource->registerOne(&this->rxFullCanEvtListener, CAN::RECEIVED_ID);
|
||
151 | } |
||
152 | |||
153 | void PowerManagement::calibrateProximityRingValues() {
|
||
154 | |||
155 | uint16_t buffer; |
||
156 | for (uint8_t idx = 0; idx < 8; ++idx) { |
||
157 | global.vcnl4020[idx].calibrate(); |
||
158 | buffer = global.vcnl4020[idx].getProximityOffset(); |
||
159 | global.memory.setVcnl4020Offset(buffer,idx); |
||
160 | } |
||
161 | } |
||
162 | |||
163 | ThreadReference PowerManagement::start(tprio_t PRIO) { |
||
164 | this->ControllerAreaNetworkRx::start(PRIO + 1); |
||
165 | this->ControllerAreaNetworkTx::start(PRIO);
|
||
166 | return NULL; |
||
167 | } |
||
168 | |||
169 | types::power_status& |
||
170 | PowerManagement::getPowerStatus() |
||
171 | { |
||
172 | return this->powerStatus; |
||
173 | } |
||
174 | |||
175 | msg_t PowerManagement::terminate(void) {
|
||
176 | msg_t ret = RDY_OK; |
||
177 | |||
178 | this->ControllerAreaNetworkTx::requestTerminate();
|
||
179 | ret |= this->ControllerAreaNetworkTx::wait();
|
||
180 | this->ControllerAreaNetworkRx::requestTerminate();
|
||
181 | ret |= this->ControllerAreaNetworkRx::wait();
|
||
182 | |||
183 | return ret;
|
||
184 | } |