2 * Copyright (C) 2012-2013 Czech Technical University in Prague
4 * Created on: 16.5.2013
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU General Public License for more details.
19 * You should have received a copy of the GNU General Public License
20 * along with this program. If not, see <http://www.gnu.org/licenses/>.
25 * Example commands for motor controlling over FlexRay. The first board has connected a control panel
26 * with two buttons and variable resistor, the second board has a DC motor connected to the H-bridge.
27 * Both boards are connected by the FlexRay bus and user can drive the motor by the control panel
28 * (speed, direction and enabling).
31 #include "cmd_motor_example.h"
40 #define ADC_MAX_VALUE 0x0983
41 #define RX_ERROR_MAX 100
42 #define ENABLE_DEBOUNCE_TIME 2
43 #define DIRECTION_DEBOUNCE_TIME 2
46 * This structure contains global FlexRay configuration.
47 * All nodes in the network have to use the same values for
48 * all parameters of this structure.
50 static Fr_TMS570LS_ClusterConfigType Fr_cluster_config = {
51 .gColdStartAttempts = 0x2,
53 .gMacroPerCycle = 0x15E0, // (cycle period, 5.6us)
54 .gMaxWithoutClockCorrectionFatal = 0xF,
55 .gMaxWithoutClockCorrectionPassive = 0xF,
56 .gNetworkManagementVectorLength = 12,
57 .gNumberOfMinislots = 0x15A,
58 .gNumberOfStaticSlots = 0x8,
59 .gOffsetCorrectionStart = 0xAE4,
60 .gPayloadLengthStatic = 0x2,
62 .gdActionPointOffset = 0x4,
63 .gdCASRxLowMax = 0x43,
64 .gdDynamicSlotIdlePhase = 0x1,
66 .gdMinislotActionPointOffset = 0x2,
68 .gdSampleClockPeriod = 0, // 10mbit/sec
70 .gdTSSTransmitter = 0xA,
71 .gdWakeupSymbolRxIdle = 18,
72 .gdWakeupSymbolRxLow = 18,
73 .gdWakeupSymbolRxWindow = 76,
74 .gdWakeupSymbolTxIdle = 180,
75 .gdWakeupSymbolTxLow = 60
79 * This structure contains local configuration of the FlexRay node for
80 * the control panel reading and sending the data.
82 static Fr_TMS570LS_NodeConfigType Fr_node_control_config = {
83 .pAllowHaltDueToClock = 0,
84 .pAllowPassiveToActive = 0xF,
85 .pChannels = FR_CHANNEL_AB,
86 .pClusterDriftDamping = 0x1,
87 .pDelayCompensationA = 0x3,
88 .pDelayCompensationB = 0x3,
89 .pExternOffsetCorrection = 0,
90 .pExternRateCorrection = 0,
91 .pKeySlotUsedForStartup = TRUE,
92 .pKeySlotUsedForSync = TRUE,
94 .pMacroInitialOffsetA = 0x6,
95 .pMacroInitialOffsetB = 0x6,
96 .pMicroInitialOffsetA = 0x18,
97 .pMicroInitialOffsetB = 0x18,
98 .pMicroPerCycle = 0x36B00,
99 .pRateCorrectionOut = 0xCD,
100 .pOffsetCorrectionOut = 0x151,
101 .pSamplesPerMicrotick = 0, // 10 mbit/sec
102 .pSingleSlotEnabled = TRUE,
103 .pWakeupChannel = FR_CHANNEL_A,
105 .pdAcceptedStartupRange = 0x81,
106 .pdListenTimeout = 0x36DA2,
108 .pDecodingCorrection = 0x33
112 * This structure contains local configuration of the FlexRay node for
113 * the motor driving. This node receives control data from the FlexRay
114 * and configures the H-bridge.
116 static Fr_TMS570LS_NodeConfigType Fr_node_motor_config = {
117 .pAllowHaltDueToClock = 0,
118 .pAllowPassiveToActive = 0xF,
119 .pChannels = FR_CHANNEL_AB,
120 .pClusterDriftDamping = 0x1,
121 .pDelayCompensationA = 0x3,
122 .pDelayCompensationB = 0x3,
123 .pExternOffsetCorrection = 0,
124 .pExternRateCorrection = 0,
125 .pKeySlotUsedForStartup = TRUE,
126 .pKeySlotUsedForSync = TRUE,
128 .pMacroInitialOffsetA = 0x6,
129 .pMacroInitialOffsetB = 0x6,
130 .pMicroInitialOffsetA = 0x18,
131 .pMicroInitialOffsetB = 0x18,
132 .pMicroPerCycle = 0x36B00,
133 .pRateCorrectionOut = 0xCD,
134 .pOffsetCorrectionOut = 0x151,
135 .pSamplesPerMicrotick = 0, // 10 mbit/sec
136 .pSingleSlotEnabled = TRUE,
137 .pWakeupChannel = FR_CHANNEL_A,
139 .pdAcceptedStartupRange = 0x81,
140 .pdListenTimeout = 0x36DA2,
142 .pDecodingCorrection = 0x33
146 * Message RAM configuration for the node reading the control panel.
148 static Fr_TMS570LS_MsgRAMConfig Fr_node_control_msgRAM_config = {
149 .dynSegmentBufferCount = 0,
150 .fifoBufferCount = 0,
151 .secureBuffers = FR_SB_ALL_REC_DISABLED,
152 .statSegmentBufferCount = 2,
153 .syncFramePayloadMultiplexEnabled = 0
157 * Message RAM configuration for the node receiving the control data and driving the H=bridge.
159 static Fr_TMS570LS_MsgRAMConfig Fr_node_motor_msgRAM_config = {
160 .dynSegmentBufferCount = 0,
161 .fifoBufferCount = 0,
162 .secureBuffers = FR_SB_ALL_REC_DISABLED,
163 .statSegmentBufferCount = 2,
164 .syncFramePayloadMultiplexEnabled = 0
168 * Static buffers configuration for the node reading the control panel.
169 * The node is configured as coldstarter and sync node. This is why the first buffer
170 * is configured as TX.
171 * The second buffer is configured as TX in single-shot mode. The buffer is used to
172 * transfer the control data.
174 static Fr_TMS570LS_BufferConfigType Fr_node_control_static_buffers_config[] = {
176 .channel = FR_CHANNEL_AB,
177 .cycleCounterFiltering = 0,
180 .msgBufferInterrupt = TRUE,
181 .payloadPreambleIndicatorTr = FALSE,
182 .rejectNullFrames = FALSE,
183 .rejectStaticSegment = FALSE,
184 .singleTransmit = FALSE,
188 .channel = FR_CHANNEL_AB,
189 .cycleCounterFiltering = 0,
192 .msgBufferInterrupt = TRUE,
193 .payloadPreambleIndicatorTr = FALSE,
194 .rejectNullFrames = FALSE,
195 .rejectStaticSegment = FALSE,
196 .singleTransmit = FALSE,
202 * Static buffers configuration for the node driving the motor.
203 * The node is configured as coldstarter and sync node. This is why the first buffer
204 * is configured as TX.
205 * The second buffer is configured as RX and is receiving the control data.
207 static Fr_TMS570LS_BufferConfigType Fr_node_motor_static_buffers_config[] = {
209 .channel = FR_CHANNEL_AB,
210 .cycleCounterFiltering = 0,
213 .msgBufferInterrupt = TRUE,
214 .payloadPreambleIndicatorTr = FALSE,
215 .rejectNullFrames = FALSE,
216 .rejectStaticSegment = FALSE,
217 .singleTransmit = FALSE,
221 .channel = FR_CHANNEL_AB,
222 .cycleCounterFiltering = 0,
225 .msgBufferInterrupt = TRUE,
226 .payloadPreambleIndicatorTr = FALSE,
227 .rejectNullFrames = FALSE,
228 .rejectStaticSegment = FALSE,
229 .singleTransmit = FALSE,
235 * Unifying configuration structure for the node reading the control panel.
237 static Fr_ConfigType Fr_config_node_control = {
238 .clusterConfiguration = &Fr_cluster_config,
239 .dynamicBufferConfigs = NULL,
240 .fifoBufferConfigs = NULL,
241 .msgRAMConfig = &Fr_node_control_msgRAM_config,
242 .nodeConfiguration = &Fr_node_control_config,
243 .staticBufferConfigs = Fr_node_control_static_buffers_config
247 * Unifying configuration structure for the node driving the motor.
249 static Fr_ConfigType Fr_config_node_motor = {
250 .clusterConfiguration = &Fr_cluster_config,
251 .dynamicBufferConfigs = NULL,
252 .fifoBufferConfigs = NULL,
253 .msgRAMConfig = &Fr_node_motor_msgRAM_config,
254 .nodeConfiguration = &Fr_node_motor_config,
255 .staticBufferConfigs = Fr_node_motor_static_buffers_config
259 * Loads data into TX buffers for FlexRay node sending the control data.
261 * After data are copied into TX buffer, the TX request is enabled, which
262 * means that message will be transmitted as soon as the frame occures in the
263 * communication cycle.
265 * @param enable enable flag to be send
266 * @param direction direction flag to be send
267 * @param duty duty cycle value to be send
269 * @return SUCCESS when data were succesfully sent
270 * FAILURE when some error occured.
272 int8_t transmit_control_data(int enable, int direction, int duty)
276 // Write payload for buffer associated with frame 1
280 return rpp_fr_transmit_lpdu(0, 3, data, 3);
284 * Checks if some message was received on message buffer 2 of the FlexRay motor controller node.
286 * If new message was received, the function copies the data from the buffer.
288 * @param enable pointer to variable, where received enable flag will be stored
289 * @param direction pointer to variable, where received direction flag will be stored
290 * @param duty pointer to variable, where received duty cycle value will be stored
291 * @return 0 if message was received and is correct
292 * 1 if message no message was received
293 * 2 if receiving failed
295 int receive_control_data(int *enable, int *direction, int *duty)
298 Fr_RxLPduStatusType status;
301 if (rpp_fr_receive_lpdu(0, 3, data, &status, &size) == FAILURE)
303 if (status == FR_NOT_RECEIVED)
307 *direction = data[1];
314 * @brief Read data from control panel and send them to motor controller through FlexRay
316 * This node reads periodically each 10ms the ADC1 channel and computes
317 * the duty cycle. Then reads DIN0, which is connected to the red
318 * button on the control panel (connected as switch to battery) and then
319 * it reads the DIN1, which is connected to the black button (connected as
321 * Finally all data are sent on the Frame 3.
324 * @param[in] cmd_io Pointer to IO stack
325 * @param[in] des Pointer to command descriptor
326 * @param[in] param Parameters of command
327 * @return 0 when OK or error code
329 int cmd_do_control(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
331 port_desc_t *adc_desc;
332 uint32_t adc_values[PORT_ADC_CHANNEL_NUM*2];
338 int enable_cnt = 0; // Debounce counter for enable button
339 int dir_cnt = 0; // Debounce counter for direction button
342 adc_desc = hal_port_get_dsc(PORT_NAME_ADC, -1);
344 if (rpp_din_setup(0, FALSE, TRUE, FALSE) == FAILURE) { // For the red button, which is switch to battery
345 rpp_sci_printf("Din 0 setup failed.\n");
346 return -CMDERR_BADCFG;
348 if (rpp_din_setup(1, FALSE, TRUE, FALSE) == FAILURE) { // For the black button, which is switch to ground
349 rpp_sci_printf("Din 1 setup failed.\n");
350 return -CMDERR_BADCFG;
352 if (rpp_din_update() == FAILURE) {
353 rpp_sci_printf("Din update failed.\n");
354 return -CMDERR_BADCFG;
357 if (rpp_fr_init_driver(&Fr_config_node_control, &error) == FAILURE) {
358 rpp_sci_printf("Fray driver initialization failed: %#x.\n", error);
359 return -CMDERR_BADCFG;
361 if (rpp_fr_init_controller(0, &error) == FAILURE) {
362 rpp_sci_printf("Fray control node initialization failed: %#x.\n", error);
363 return -CMDERR_BADCFG;
365 rpp_sci_printf("Fray control node initialized.\r\n");
366 rpp_sci_printf("Waiting for network connection...\r\n");
368 if (rpp_fr_start_communication(0, &error) == FAILURE) {
369 rpp_sci_printf("Integration to the network failed: %#x.\n", error);
370 return -CMDERR_BADCFG;
372 if (rpp_fr_all_slots(0) == FAILURE) {
373 rpp_sci_printf("All slots mode selection failed.\n");
374 return -CMDERR_BADCFG;
377 rpp_sci_printf("Connected.\r\n");
380 vTaskDelay(10/portTICK_RATE_MS);
381 ret = adc_desc->port_getfnc_ptr(adc_desc->config, PORT_ADC_CHANNEL_NUM, adc_values);
383 rpp_sci_printf("ADC read failed!\n");
386 duty = (100*adc_values[0])/ADC_MAX_VALUE;
387 if (rpp_din_update() == FAILURE) {
388 rpp_sci_printf("Din update failed.\n");
391 button = !rpp_din_get(0);
393 if (enable_cnt++ == ENABLE_DEBOUNCE_TIME)
400 button = rpp_din_get(1);
402 if (dir_cnt++ == DIRECTION_DEBOUNCE_TIME)
403 direction = !direction;
410 if (transmit_control_data(enable, direction, duty) == FAILURE) {
411 rpp_sci_printf("Data transmission failed!\n");
414 rpp_sci_printf("Enable: %d, Direction: %s, Duty: %d%%\r", enable, (direction == 1) ? "L" : "R", duty);
417 if (rpp_fr_halt_communication(0) == FAILURE)
418 rpp_sci_printf("FlexRay HALT command failed, please reset the board to stop transmission.\n");
420 rpp_sci_printf("FlexRay halted, reset the board to make FlexRay usable again.\r\n");
425 * @brief Receive data from FlexRay and control motor on HBR
427 * This FlexRay node receives the control data from the FlexRay network
428 * and configures the H-bridge to drive the DC motor according the
429 * parameters from the received message.
431 * @param[in] cmd_io Pointer to IO stack
432 * @param[in] des Pointer to command descriptor
433 * @param[in] param Parameters of command
434 * @return 0 when OK or error code
436 int cmd_do_motor(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
438 double hbr_period = 50; // us
439 double control = 0.0;
443 uint32_t rx_error = 0;
447 if (rpp_hbr_enable(hbr_period) == FAILURE ||
448 rpp_hbr_control(0) == FAILURE
450 rpp_sci_printf("H-bridge initialization failed.\n");
451 return -CMDERR_BADCFG;
454 if (rpp_fr_init_driver(&Fr_config_node_motor, &error) == FAILURE) {
455 rpp_sci_printf("Fray driver initialization failed: %#x.\n", error);
456 return -CMDERR_BADCFG;
458 if (rpp_fr_init_controller(0, &error) == FAILURE) {
459 rpp_sci_printf("Fray motor node initialization failed: %#x.\n", error);
460 return -CMDERR_BADCFG;
462 rpp_sci_printf("Fray motor node initialized.\r\n");
463 rpp_sci_printf("Waiting for network connection...\r\n");
465 if (rpp_fr_start_communication(0, &error) == FAILURE) {
466 rpp_sci_printf("Integration to the network failed: %#x.\n", error);
467 return -CMDERR_BADCFG;
469 if (rpp_fr_all_slots(0) == FAILURE) {
470 rpp_sci_printf("All slots mode selection failed.\n");
471 return -CMDERR_BADCFG;
474 rpp_sci_printf("Connected.\r\n");
477 ret = receive_control_data(&enable, &direction, &hbr_duty);
478 if (ret == 2) { // receiving failed
480 if (rx_error > RX_ERROR_MAX) {
481 rpp_sci_printf("Maximum RX errors reached!\n");
485 else if (ret == 0) { // message received
486 control = hbr_duty/((double)100) * ((direction == 1) ? -1 : 1) * enable;
487 rpp_sci_printf("Enable: %d, Direction: %s, Duty: %d%%\r", enable, (direction == 1) ? "L" : "R", hbr_duty);
488 if (rpp_hbr_control(control) == FAILURE) {
489 rpp_sci_printf("H-bridge control failed!\n");
494 if (rpp_hbr_disable() == FAILURE)
495 rpp_sci_printf("H-bridge disabling failed!\n");
497 rpp_sci_printf("H-bridge disabled.\n");
498 if (rpp_fr_halt_communication(0) == FAILURE)
499 rpp_sci_printf("FlexRay HALT command failed, please reset the board to stop transmission.\n");
501 rpp_sci_printf("FlexRay halted, reset the board to make FlexRay usable again.\r\n");
507 /** Command descriptor for control */
508 cmd_des_t const cmd_des_control = {
510 "demomotctrl","Run motor control demo - reads input and sends it",
511 "### Command syntax ###\n"
515 "### Description ###\n"
517 "This command creates a FlexRay node and starts to read buttons\n"
518 "(connected to DIN0 and DIN1) and a potentiometer (ADC1) from a control\n"
519 "panel. The read data are sent via FlexRay to the second node, created\n"
520 "by running demomotdrive command.\n"
522 "The purpose of this pair of commands is to demonstrate functionality\n"
523 "of the FlexRay, ADC, DIN and HBR peripherals.\n",
524 CMD_HANDLER(cmd_do_control), (void *)&cmd_list_motor_example
527 /** Command descriptor for motor */
528 cmd_des_t const cmd_des_motor = {
530 "demomotdrive","Run motor control demo - drives the DC motor",
531 "### Command syntax ###\n"
535 "### Description ###\n"
537 "This command creates a FlexRay node and starts to receive the data\n"
538 "from another node created by command demomotctrl. The received data\n"
539 "are applied to HBR to control the DC motor.\n"
541 "The purpose of this pair of commands is to demonstrate functionality\n"
542 "of the FlexRay, ADC, DIN and HBR peripherals.\n",
543 CMD_HANDLER(cmd_do_motor), (void *)&cmd_list_motor_example
546 /** List of commands for example, defined as external */
547 cmd_des_t const *cmd_list_motor_example[] = {