Statistics
| Branch: | Tag: | Revision:

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
}