amiro-os / devices / PowerManagement / PowerManagement.cpp @ f336542d
History | View | Annotate | Download (6.7 KB)
1 |
#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 |
BaseThread::sleep(US2ST(10)); // Use to sleep for 10 CAN cycle (@1Mbit), otherwise the cognition-board might not receive all messagee |
137 |
} |
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 |
} |