Statistics
| Branch: | Tag: | Revision:

amiro-os / components / ControllerAreaNetworkTx.cpp @ 58fe0e0b

History | View | Annotate | Download (6.73 KB)

1 58fe0e0b Thomas Schöpping
#include <ch.hpp>
2
#include <hal.h>
3
#include <string.h>  // memcpy
4
5
#include <amiro/Constants.h>
6
#include <amiro/ControllerAreaNetworkTx.h>
7
8
using namespace chibios_rt;
9
using namespace amiro;
10
11
using namespace types;
12
using namespace amiro::constants;
13
14
ControllerAreaNetworkTx::ControllerAreaNetworkTx(CANDriver *can, const uint8_t boardId)
15
    : BaseStaticThread<128>(),
16
      boardId(boardId),
17
      canDriver(can)
18
{
19
#ifdef STM32F4XX
20
  this->canConfig.mcr = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP;
21
  this->canConfig.btr = CAN_BTR_SJW(1) | CAN_BTR_TS2(3) | CAN_BTR_TS1(15)
22
                        | CAN_BTR_BRP(1);
23
#else
24
  this->canConfig.mcr = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP;
25
  this->canConfig.btr = CAN_BTR_SJW(1) | CAN_BTR_TS2(2) | CAN_BTR_TS1(13)
26
                        | CAN_BTR_BRP(1);
27
#endif
28
}
29
30
ControllerAreaNetworkTx::
31
~ControllerAreaNetworkTx() {
32
33
}
34
35
//----------------------------------------------------------------
36
37
void ControllerAreaNetworkTx::setLightBrightness(int brightness) {
38
  CANTxFrame frame;
39
  frame.SID = 0;
40
  this->encodeDeviceId(&frame, CAN::BRIGHTNESS_ID);
41
  frame.data8[0] = brightness;
42
  frame.DLC = 1;
43
  this->transmitMessage(&frame);
44
}
45
46
void ControllerAreaNetworkTx::setLightColor(int index, Color color) {
47
  CANTxFrame frame;
48
  frame.SID = 0;
49
  this->encodeDeviceId(&frame, CAN::COLOR_ID(index));
50
  frame.data8[0] = color.getRed();
51
  frame.data8[1] = color.getGreen();
52
  frame.data8[2] = color.getBlue();
53
  frame.DLC = 3;
54
  this->transmitMessage(&frame);
55
}
56
57
void ControllerAreaNetworkTx::setOdometry(types::position robotPosition) {
58
  CANTxFrame frame;
59
  frame.SID = 0;
60
  this->encodeDeviceId(&frame, CAN::SET_ODOMETRY_ID);
61
  // Cut of the first byte, which precission is not needed
62
  int32_t x_mm = (robotPosition.x >> 8);
63
  int32_t y_mm = (robotPosition.y >> 8);
64
  int16_t f_z_mrad = int16_t(robotPosition.f_z >> 8 );
65
  // Copy the data structure
66
  memcpy((uint8_t *)&(frame.data8[0]), (uint8_t *)&x_mm, 3);
67
  memcpy((uint8_t *)&(frame.data8[3]), (uint8_t *)&y_mm, 3);
68
  memcpy((uint8_t *)&(frame.data8[6]), (uint8_t *)&f_z_mrad, 2);
69
  frame.DLC = 8;
70
  this->transmitMessage(&frame);
71
}
72
73
//----------------------------------------------------------------
74
75
void ControllerAreaNetworkTx::setTargetSpeed(int32_t leftURpm, int32_t rightURpm) {
76
  CANTxFrame frame;
77
  frame.SID = 0;
78
  this->encodeDeviceId(&frame, CAN::TARGET_RPM_ID);
79
  frame.data32[0] = leftURpm;
80
  frame.data32[1] = rightURpm;
81
  frame.DLC = 8;
82
  this->transmitMessage(&frame);
83
}
84
85
void ControllerAreaNetworkTx::setTargetSpeed(kinematic &targetSpeed) {
86
  CANTxFrame frame;
87
  frame.SID = 0;
88
  this->encodeDeviceId(&frame, CAN::TARGET_SPEED_ID);
89
  frame.data32[0] = targetSpeed.x;
90
  frame.data32[1] = targetSpeed.w_z;
91
  frame.DLC = 8;
92
  this->transmitMessage(&frame);
93
}
94
95
void ControllerAreaNetworkTx::setTargetPosition(types::position &targetPosition, uint32_t targetTime) {
96
  CANTxFrame frame;
97
  frame.SID = 0;
98
  this->encodeDeviceId(&frame, CAN::TARGET_POSITION_ID);
99
  frame.data32[0] = targetPosition.x;
100
  frame.data32[1] = targetPosition.f_z;
101
  frame.data32[1] = targetTime;
102
  frame.DLC = 12;
103
  this->transmitMessage(&frame);
104
}
105
106
void ControllerAreaNetworkTx::setKinematicConstants(float Ed, float Eb) {
107
  CANTxFrame frame;
108
  frame.SID = 0;
109
  this->encodeDeviceId(&frame, CAN::SET_KINEMATIC_CONST_ID);
110
  frame.data32[0] = Ed;
111
  frame.data32[1] = Eb;
112
  frame.DLC = 8;
113
  this->transmitMessage(&frame);
114
}
115
116
void ControllerAreaNetworkTx::broadcastShutdown() {
117
    CANTxFrame frame;
118
    frame.SID = 0x00u;
119
    this->encodeDeviceId(&frame, CAN::BROADCAST_SHUTDOWN);
120
    frame.data16[0] = CAN::SHUTDOWN_MAGIC;
121
    frame.DLC = 2;
122
    this->transmitMessage(&frame);
123
124
}
125
126
//----------------------------------------------------------------
127
128
void ControllerAreaNetworkTx::txQueryShell(uint8_t toBoardId, char *textdata, uint16_t size) {
129
    uint16_t cnt = size;
130
    CANTxFrame frame;
131
    frame.SID = 0;
132
    frame.DLC = 8;
133
    this->encodeDeviceId(&frame, CAN::SHELL_QUERY_ID(toBoardId));
134
135
    while (cnt > 8) {
136
        memcpy(frame.data8,&textdata[size-cnt],8) ;
137
        this->transmitMessage(&frame);
138
        cnt -= 8;
139
    }
140
    memcpy(frame.data8,&textdata[size-cnt],cnt) ;
141
    frame.DLC = cnt;
142
    this->transmitMessage(&frame);
143
}
144
145
146
void ControllerAreaNetworkTx::txReplyShell(uint8_t toBoardId, char *textdata, uint16_t size) {
147
    uint16_t cnt = size;
148
    CANTxFrame frame;
149
    frame.SID = 0;
150
    frame.DLC = 8;
151
    this->encodeDeviceId(&frame, CAN::SHELL_REPLY_ID(toBoardId));
152
153
    while (cnt > 8) {
154
        memcpy(frame.data8,&textdata[size-cnt],8) ;
155
        this->transmitMessage(&frame);
156
        cnt -= 8;
157
    }
158
    memcpy(frame.data8,&textdata[size-cnt],cnt) ;
159
    frame.DLC = cnt;
160
    this->transmitMessage(&frame);
161
}
162
163
//----------------------------------------------------------------
164
165
msg_t ControllerAreaNetworkTx::main(void) {
166
  evtInit(&this->evtimer, CAN::UPDATE_PERIOD_MSEC);
167
168
  this->eventTimerEvtSource = reinterpret_cast<EvtSource *>(&this->evtimer.et_es);
169
170
  this->eventTimerEvtSource->registerOne(&this->eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
171
172
  evtStart(&this->evtimer);
173
174
  this->setName("ControllerAreaNetworkTx");
175
176
  while (!this->shouldTerminate()) {
177
    eventmask_t eventMask = this->waitOneEvent(ALL_EVENTS);
178
    switch (eventMask) {
179
      case EVENT_MASK(CAN::PERIODIC_TIMER_ID):
180
        updateSensorVal();
181
        periodicBroadcast();
182
        break;
183
    }
184
  }
185
186
  evtStop(&this->evtimer);
187
  this->eventTimerEvtSource->unregister(&this->eventTimerEvtListener);
188
189
  return RDY_OK;
190
}
191
192
msg_t ControllerAreaNetworkTx::updateSensorVal() {
193
  return RDY_OK;
194
}
195
196
//----------------------------------------------------------------
197
198
void ControllerAreaNetworkTx::encodeBoardId(CANTxFrame *frame, int board) {
199
  frame->SID |= (board & CAN::BOARD_ID_MASK) << CAN::BOARD_ID_SHIFT;
200
}
201
202
void ControllerAreaNetworkTx::encodeDeviceId(CANTxFrame *frame, int device) {
203
  frame->SID |= (device & CAN::DEVICE_ID_MASK) << CAN::DEVICE_ID_SHIFT;
204
}
205
206
void ControllerAreaNetworkTx::encodeIndexId(CANTxFrame *frame, int index) {
207
  frame->SID |= (index & CAN::INDEX_ID_MASK) << CAN::INDEX_ID_SHIFT;
208
}
209
210
//----------------------------------------------------------------
211
212
void ControllerAreaNetworkTx::transmitMessage(CANTxFrame *frame) {
213
  this->encodeBoardId(frame, boardId);
214
  frame->IDE = CAN_IDE_STD;
215
  frame->RTR = CAN_RTR_DATA;
216
  /**
217
   * We cannot use TIME_INFINITE here b/c
218
   * lower boards might crash/power down etc.
219
   * and block CAN bus (though they should not...).
220
   * Then, we get stuck here and cannot terminate CAN thread
221
   * Therefore:
222
   *
223
   * 1 us       * (  1 + 11 +   1 +   1 +   1 +   4 +   64 +  15 +     1 +   1 +     1 +   7) * 5        = 545 us
224
   * 1/ (1 MHz) * (SOF + ID + RTR + IDE + RES + DLC + DATA + CRC + DELIM + ACK + DELIM + EOF) * #RETRIES
225
   */
226
227
  BaseThread::sleep(MS2ST(2));
228
  canTransmit(this->canDriver, CAN_TX_MAILBOXES, frame, US2ST(545));
229
}