rpp-test-sw/commands/cmd_motor_example.c

   1 /*
   2  * Copyright (C) 2012-2013, 2015 Czech Technical University in Prague
   3  *
   4  * Created on: 16.5.2013
   5  *
   6  * Authors:
   7  *     - Michal Horn
   8  *
   9  * Permission is hereby granted, free of charge, to any person
  10  * obtaining a copy of this software and associated documentation
  11  * files (the "Software"), to deal in the Software without
  12  * restriction, including without limitation the rights to use,
  13  * copy, modify, merge, publish, distribute, sublicense, and/or sell
  14  * copies of the Software, and to permit persons to whom the
  15  * Software is furnished to do so, subject to the following
  16  * conditions:
  17  *
  18  * The above copyright notice and this permission notice shall be
  19  * included in all copies or substantial portions of the Software.
  20  *
  21  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  22  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
  23  * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  24  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
  25  * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
  26  * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  27  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
  28  * OTHER DEALINGS IN THE SOFTWARE.
  29  *
  30  * File : cmd_pin.c
  31  *
  32  * Abstract:
  33  *              Example commands for motor controlling over FlexRay. The first board has connected a control panel
  34  *      with two buttons and variable resistor, the second board has a DC motor connected to the H-bridge.
  35  *      Both boards are connected by the FlexRay bus and user can drive the motor by the control panel
  36  *      (speed, direction and enabling).
  37  */
  38
  39 #include "cmd_motor_example.h"
  40
  41 #ifndef DOCGEN
  42
  43 #include "rpp/rpp.h"
  44 #include "cmdproc.h"
  45 #include "drv/port.h"
  46
  47 #define ADC_MAX_VALUE 0x0983
  48 #define RX_ERROR_MAX 100
  49 #define ENABLE_DEBOUNCE_TIME    2
  50 #define DIRECTION_DEBOUNCE_TIME 2
  51
  52 /**
  53  * This structure contains global FlexRay configuration.
  54  * All nodes in the network have to use the same values for
  55  * all parameters of this structure.
  56  */
  57 static Fr_TMS570LS_ClusterConfigType Fr_cluster_config = {
  58         .gColdStartAttempts = 0x2,
  59         .gListenNoise = 0xF,
  60         .gMacroPerCycle = 0x15E0,   // (cycle period, 5.6us)
  61         .gMaxWithoutClockCorrectionFatal = 0xF,
  62         .gMaxWithoutClockCorrectionPassive = 0xF,
  63         .gNetworkManagementVectorLength = 12,
  64         .gNumberOfMinislots = 0x15A,
  65         .gNumberOfStaticSlots = 0x8,
  66         .gOffsetCorrectionStart = 0xAE4,
  67         .gPayloadLengthStatic = 0x2,
  68         .gSyncNodeMax = 0xF,
  69         .gdActionPointOffset = 0x4,
  70         .gdCASRxLowMax = 0x43,
  71         .gdDynamicSlotIdlePhase = 0x1,
  72         .gdMinislot = 0x4,
  73         .gdMinislotActionPointOffset = 0x2,
  74         .gdNIT = 0xAE3,
  75         .gdSampleClockPeriod = 0,       // 10mbit/sec
  76         .gdStaticSlot = 0x56,
  77         .gdTSSTransmitter = 0xA,
  78         .gdWakeupSymbolRxIdle = 18,
  79         .gdWakeupSymbolRxLow = 18,
  80         .gdWakeupSymbolRxWindow = 76,
  81         .gdWakeupSymbolTxIdle = 180,
  82         .gdWakeupSymbolTxLow = 60
  83 };
  84
  85 /**
  86  * This structure contains local configuration of the FlexRay node for
  87  * the control panel reading and sending the data.
  88  */
  89 static Fr_TMS570LS_NodeConfigType Fr_node_control_config = {
  90         .pAllowHaltDueToClock = 0,
  91         .pAllowPassiveToActive = 0xF,
  92         .pChannels = FR_CHANNEL_AB,
  93         .pClusterDriftDamping = 0x1,
  94         .pDelayCompensationA = 0x3,
  95         .pDelayCompensationB = 0x3,
  96         .pExternOffsetCorrection = 0,
  97         .pExternRateCorrection = 0,
  98         .pKeySlotUsedForStartup = TRUE,
  99         .pKeySlotUsedForSync = TRUE,
 100         .pLatestTx = 0x10D,
 101         .pMacroInitialOffsetA = 0x6,
 102         .pMacroInitialOffsetB = 0x6,
 103         .pMicroInitialOffsetA = 0x18,
 104         .pMicroInitialOffsetB = 0x18,
 105         .pMicroPerCycle = 0x36B00,
 106         .pRateCorrectionOut = 0xCD,
 107         .pOffsetCorrectionOut = 0x151,
 108         .pSamplesPerMicrotick = 0,      // 10 mbit/sec
 109         .pSingleSlotEnabled = TRUE,
 110         .pWakeupChannel = FR_CHANNEL_A,
 111         .pWakeupPattern = 2,
 112         .pdAcceptedStartupRange = 0x81,
 113         .pdListenTimeout = 0x36DA2,
 114         .pdMaxDrift = 0x151,
 115         .pDecodingCorrection = 0x33
 116 };
 117
 118 /**
 119  * This structure contains local configuration of the FlexRay node for
 120  * the motor driving. This node receives control data from the FlexRay
 121  * and configures the H-bridge.
 122  */
 123 static Fr_TMS570LS_NodeConfigType Fr_node_motor_config = {
 124         .pAllowHaltDueToClock = 0,
 125         .pAllowPassiveToActive = 0xF,
 126         .pChannels = FR_CHANNEL_AB,
 127         .pClusterDriftDamping = 0x1,
 128         .pDelayCompensationA = 0x3,
 129         .pDelayCompensationB = 0x3,
 130         .pExternOffsetCorrection = 0,
 131         .pExternRateCorrection = 0,
 132         .pKeySlotUsedForStartup = TRUE,
 133         .pKeySlotUsedForSync = TRUE,
 134         .pLatestTx = 0x10D,
 135         .pMacroInitialOffsetA = 0x6,
 136         .pMacroInitialOffsetB = 0x6,
 137         .pMicroInitialOffsetA = 0x18,
 138         .pMicroInitialOffsetB = 0x18,
 139         .pMicroPerCycle = 0x36B00,
 140         .pRateCorrectionOut = 0xCD,
 141         .pOffsetCorrectionOut = 0x151,
 142         .pSamplesPerMicrotick = 0,          // 10 mbit/sec
 143         .pSingleSlotEnabled = TRUE,
 144         .pWakeupChannel = FR_CHANNEL_A,
 145         .pWakeupPattern = 2,
 146         .pdAcceptedStartupRange = 0x81,
 147         .pdListenTimeout = 0x36DA2,
 148         .pdMaxDrift = 0x151,
 149         .pDecodingCorrection = 0x33
 150 };
 151
 152 /**
 153  * Message RAM configuration for the node reading the control panel.
 154  */
 155 static Fr_TMS570LS_MsgRAMConfig Fr_node_control_msgRAM_config = {
 156         .dynSegmentBufferCount = 0,
 157         .fifoBufferCount = 0,
 158         .secureBuffers = FR_SB_ALL_REC_DISABLED,
 159         .statSegmentBufferCount = 2,
 160         .syncFramePayloadMultiplexEnabled = 0
 161 };
 162
 163 /**
 164  * Message RAM configuration for the node receiving the control data and driving the H=bridge.
 165  */
 166 static Fr_TMS570LS_MsgRAMConfig Fr_node_motor_msgRAM_config = {
 167         .dynSegmentBufferCount = 0,
 168         .fifoBufferCount = 0,
 169         .secureBuffers = FR_SB_ALL_REC_DISABLED,
 170         .statSegmentBufferCount = 2,
 171         .syncFramePayloadMultiplexEnabled = 0
 172 };
 173
 174 /**
 175  * Static buffers configuration for the node reading the control panel.
 176  * The node is configured as coldstarter and sync node. This is why the first buffer
 177  * is configured as TX.
 178  * The second buffer is configured as TX in single-shot mode. The buffer is used to
 179  * transfer the control data.
 180  */
 181 static Fr_TMS570LS_BufferConfigType Fr_node_control_static_buffers_config[] = {
 182         {
 183                 .channel = FR_CHANNEL_AB,
 184                 .cycleCounterFiltering = 0,
 185                 .isTx = TRUE,
 186                 .maxPayload = 2,
 187                 .msgBufferInterrupt = TRUE,
 188                 .payloadPreambleIndicatorTr = FALSE,
 189                 .rejectNullFrames = FALSE,
 190                 .rejectStaticSegment = FALSE,
 191                 .singleTransmit = FALSE,
 192                 .slotId = 1
 193         },
 194         {
 195                 .channel = FR_CHANNEL_AB,
 196                 .cycleCounterFiltering = 0,
 197                 .isTx = TRUE,
 198                 .maxPayload = 2,
 199                 .msgBufferInterrupt = TRUE,
 200                 .payloadPreambleIndicatorTr = FALSE,
 201                 .rejectNullFrames = FALSE,
 202                 .rejectStaticSegment = FALSE,
 203                 .singleTransmit = FALSE,
 204                 .slotId = 3
 205         }
 206 };
 207
 208 /**
 209  * Static buffers configuration for the node driving the motor.
 210  * The node is configured as coldstarter and sync node. This is why the first buffer
 211  * is configured as TX.
 212  * The second buffer is configured as RX and is receiving the control data.
 213  */
 214 static Fr_TMS570LS_BufferConfigType Fr_node_motor_static_buffers_config[] = {
 215         {
 216                 .channel = FR_CHANNEL_AB,
 217                 .cycleCounterFiltering = 0,
 218                 .isTx = TRUE,
 219                 .maxPayload = 2,
 220                 .msgBufferInterrupt = TRUE,
 221                 .payloadPreambleIndicatorTr = FALSE,
 222                 .rejectNullFrames = FALSE,
 223                 .rejectStaticSegment = FALSE,
 224                 .singleTransmit = FALSE,
 225                 .slotId = 2
 226         },
 227         {
 228                 .channel = FR_CHANNEL_AB,
 229                 .cycleCounterFiltering = 0,
 230                 .isTx = FALSE,
 231                 .maxPayload = 2,
 232                 .msgBufferInterrupt = TRUE,
 233                 .payloadPreambleIndicatorTr = FALSE,
 234                 .rejectNullFrames = FALSE,
 235                 .rejectStaticSegment = FALSE,
 236                 .singleTransmit = FALSE,
 237                 .slotId = 3
 238         }
 239 };
 240
 241 /**
 242  * Unifying configuration structure for the node reading the control panel.
 243  */
 244 static Fr_ConfigType Fr_config_node_control = {
 245         .clusterConfiguration = &Fr_cluster_config,
 246         .dynamicBufferConfigs = NULL,
 247         .fifoBufferConfigs = NULL,
 248         .msgRAMConfig = &Fr_node_control_msgRAM_config,
 249         .nodeConfiguration = &Fr_node_control_config,
 250         .staticBufferConfigs = Fr_node_control_static_buffers_config
 251 };
 252
 253 /**
 254  * Unifying configuration structure for the node driving the motor.
 255  */
 256 static Fr_ConfigType Fr_config_node_motor = {
 257         .clusterConfiguration = &Fr_cluster_config,
 258         .dynamicBufferConfigs = NULL,
 259         .fifoBufferConfigs = NULL,
 260         .msgRAMConfig = &Fr_node_motor_msgRAM_config,
 261         .nodeConfiguration = &Fr_node_motor_config,
 262         .staticBufferConfigs = Fr_node_motor_static_buffers_config
 263 };
 264
 265 /**
 266  *  Loads data into TX buffers for FlexRay node sending the control data.
 267  *
 268  *  After data are copied into TX buffer, the TX request is enabled, which
 269  *  means that message will be transmitted as soon as the frame occures in the
 270  *  communication cycle.
 271  *
 272  *  @param      enable  enable flag to be send
 273  *  @param  direction   direction flag to be send
 274  *  @param      duty    duty cycle value to be send
 275  *
 276  *  @return SUCCESS when data were succesfully sent
 277  *          FAILURE when some error occured.
 278  */
 279 int8_t transmit_control_data(int enable, int direction, int duty)
 280 {
 281         uint8_t data[3];
 282
 283         // Write payload for buffer associated with frame 1
 284         data[0] = duty;
 285         data[1] = direction;
 286         data[2] = enable;
 287         return rpp_fr_transmit_lpdu(0, 3, data, 3);
 288 }
 289
 290 /**
 291  *      Checks if some message was received on message buffer 2 of the FlexRay motor controller node.
 292  *
 293  *      If new message was received, the function copies the data from the buffer.
 294  *
 295  *      @param enable   pointer to variable, where received enable flag will be stored
 296  *      @param direction pointer to variable, where received direction flag will be stored
 297  *      @param duty     pointer to variable, where received duty cycle value will be stored
 298  *      @return 0 if message was received and is correct
 299  *                      1 if message no message was received
 300  *                      2 if receiving failed
 301  */
 302 int receive_control_data(int *enable, int *direction, int *duty)
 303 {
 304         uint8_t data[3];
 305         Fr_RxLPduStatusType status;
 306         uint8_t size;
 307
 308         if (rpp_fr_receive_lpdu(0, 3, data, &status, &size) == FAILURE)
 309                 return 2;
 310         if (status == FR_NOT_RECEIVED)
 311                 return 1;
 312         else {
 313                 *duty = data[0];
 314                 *direction = data[1];
 315                 *enable = data[2];
 316                 return 0;
 317         }
 318 }
 319
 320 /**
 321  * @brief       Read data from control panel and send them to motor controller through FlexRay
 322  *
 323  * This node reads periodically each 10ms the ADC1 channel and computes
 324  * the duty cycle. Then reads DIN0, which is connected to the red
 325  * button on the control panel (connected as switch to battery) and then
 326  * it reads the DIN1, which is connected to the black button (connected as
 327  * switch to ground).
 328  * Finally all data are sent on the Frame 3.
 329  *
 330  *
 331  * @param[in]   cmd_io  Pointer to IO stack
 332  * @param[in]   des             Pointer to command descriptor
 333  * @param[in]   param   Parameters of command
 334  * @return      0 when OK or error code
 335  */
 336 int cmd_do_control(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 337 {
 338         int ret;
 339         int duty = 0;
 340         int button = 0;
 341         int enable = 0;
 342         int direction = 1;
 343         int enable_cnt = 0;         // Debounce counter for enable button
 344         int dir_cnt = 0;            // Debounce counter for direction button
 345         uint32_t error;
 346
 347         if (rpp_din_setup(0, FALSE, TRUE, FALSE) == FAILURE) {  // For the red button, which is switch to battery
 348                 rpp_sci_printf("Din 0 setup failed.\n");
 349                 return -CMDERR_BADCFG;
 350         }
 351         if (rpp_din_setup(1, FALSE, TRUE, FALSE) == FAILURE) {  // For the black button, which is switch to ground
 352                 rpp_sci_printf("Din 1 setup failed.\n");
 353                 return -CMDERR_BADCFG;
 354         }
 355         if (rpp_din_update() == FAILURE) {
 356                 rpp_sci_printf("Din update failed.\n");
 357                 return -CMDERR_BADCFG;
 358         }
 359
 360         if (rpp_fr_init_driver(&Fr_config_node_control, &error) == FAILURE) {
 361                 rpp_sci_printf("Fray driver initialization failed: %#x.\n", error);
 362                 return -CMDERR_BADCFG;
 363         }
 364         if (rpp_fr_init_controller(0, &error) == FAILURE) {
 365                 rpp_sci_printf("Fray control node initialization failed: %#x.\n", error);
 366                 return -CMDERR_BADCFG;
 367         }
 368         rpp_sci_printf("Fray control node initialized.\r\n");
 369         rpp_sci_printf("Waiting for network connection...\r\n");
 370
 371         if (rpp_fr_start_communication(0, &error) == FAILURE) {
 372                 rpp_sci_printf("Integration to the network failed: %#x.\n", error);
 373                 return -CMDERR_BADCFG;
 374         }
 375         if (rpp_fr_all_slots(0) == FAILURE) {
 376                 rpp_sci_printf("All slots mode selection failed.\n");
 377                 return -CMDERR_BADCFG;
 378         }
 379
 380         rpp_sci_printf("Connected.\r\n");
 381
 382         while (1) {
 383                 const struct port_desc *port = &port_desc[PORT_ID_ADC];
 384                 uint16_t adc_values[port->numchn];
 385                 vTaskDelay(10/portTICK_RATE_MS);
 386
 387                 ret = port->get(port, adc_values, sizeof(adc_values));
 388                 if (ret < 0) {
 389                         rpp_sci_printf("ADC read failed!\n");
 390                         break;
 391                 }
 392                 duty = (100*adc_values[0])/ADC_MAX_VALUE;
 393                 if (rpp_din_update() == FAILURE) {
 394                         rpp_sci_printf("Din update failed.\n");
 395                         break;
 396                 }
 397                 button = !rpp_din_get(0);
 398                 if (button == 1) {
 399                         if (enable_cnt++ == ENABLE_DEBOUNCE_TIME)
 400                                 enable = !enable;
 401                         else
 402                                 enable_cnt++;
 403                 }
 404                 else
 405                         enable_cnt = 0;
 406                 button = rpp_din_get(1);
 407                 if (button == 1) {
 408                         if (dir_cnt++ == DIRECTION_DEBOUNCE_TIME)
 409                                 direction = !direction;
 410                         else
 411                                 dir_cnt++;
 412                 }
 413                 else
 414                         dir_cnt = 0;
 415
 416                 if (transmit_control_data(enable, direction, duty) == FAILURE) {
 417                         rpp_sci_printf("Data transmission failed!\n");
 418                         break;
 419                 }
 420                 rpp_sci_printf("Enable: %d, Direction: %s, Duty: %d%%\r", enable, (direction == 1) ? "L" : "R", duty);
 421         }
 422
 423         if (rpp_fr_halt_communication(0) == FAILURE)
 424                 rpp_sci_printf("FlexRay HALT command failed, please reset the board to stop transmission.\n");
 425         else
 426                 rpp_sci_printf("FlexRay halted, reset the board to make FlexRay usable again.\r\n");
 427         return 0;
 428 }
 429
 430 /**
 431  * @brief       Receive data from FlexRay and control motor on HBR
 432  *
 433  * This FlexRay node receives the control data from the FlexRay network
 434  * and configures the H-bridge to drive the DC motor according the
 435  * parameters from the received message.
 436  *
 437  * @param[in]   cmd_io  Pointer to IO stack
 438  * @param[in]   des             Pointer to command descriptor
 439  * @param[in]   param   Parameters of command
 440  * @return      0 when OK or error code
 441  */
 442 int cmd_do_motor(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 443 {
 444         double hbr_period = 50; // us
 445         double control = 0.0;
 446         int hbr_duty = 0;
 447         int direction = 1;
 448         int enable = 0;
 449         uint32_t rx_error = 0;
 450         int ret;
 451         uint32_t error;
 452
 453         if (rpp_hbr_enable(hbr_period) == FAILURE ||
 454                 rpp_hbr_control(0) == FAILURE
 455                 ) {
 456                 rpp_sci_printf("H-bridge initialization failed.\n");
 457                 return -CMDERR_BADCFG;
 458         }
 459
 460         if (rpp_fr_init_driver(&Fr_config_node_motor, &error) == FAILURE) {
 461                 rpp_sci_printf("Fray driver initialization failed: %#x.\n", error);
 462                 return -CMDERR_BADCFG;
 463         }
 464         if (rpp_fr_init_controller(0, &error) == FAILURE) {
 465                 rpp_sci_printf("Fray motor node initialization failed: %#x.\n", error);
 466                 return -CMDERR_BADCFG;
 467         }
 468         rpp_sci_printf("Fray motor node initialized.\r\n");
 469         rpp_sci_printf("Waiting for network connection...\r\n");
 470
 471         if (rpp_fr_start_communication(0, &error) == FAILURE) {
 472                 rpp_sci_printf("Integration to the network failed: %#x.\n", error);
 473                 return -CMDERR_BADCFG;
 474         }
 475         if (rpp_fr_all_slots(0) == FAILURE) {
 476                 rpp_sci_printf("All slots mode selection failed.\n");
 477                 return -CMDERR_BADCFG;
 478         }
 479
 480         rpp_sci_printf("Connected.\r\n");
 481
 482         while (1) {
 483                 ret = receive_control_data(&enable, &direction, &hbr_duty);
 484                 if (ret == 2) { // receiving failed
 485                         rx_error++;
 486                         if (rx_error > RX_ERROR_MAX) {
 487                                 rpp_sci_printf("Maximum RX errors reached!\n");
 488                                 break;
 489                         }
 490                 }
 491                 else if (ret == 0) {    // message received
 492                         control = hbr_duty/((double)100) * ((direction == 1) ? -1 : 1) * enable;
 493                         rpp_sci_printf("Enable: %d, Direction: %s, Duty: %d%%\r", enable, (direction == 1) ? "L" : "R", hbr_duty);
 494                         if (rpp_hbr_control(control) == FAILURE) {
 495                                 rpp_sci_printf("H-bridge control failed!\n");
 496                                 break;
 497                         }
 498                 }
 499         }
 500         if (rpp_hbr_disable() == FAILURE)
 501                 rpp_sci_printf("H-bridge disabling failed!\n");
 502         else
 503                 rpp_sci_printf("H-bridge disabled.\n");
 504         if (rpp_fr_halt_communication(0) == FAILURE)
 505                 rpp_sci_printf("FlexRay HALT command failed, please reset the board to stop transmission.\n");
 506         else
 507                 rpp_sci_printf("FlexRay halted, reset the board to make FlexRay usable again.\r\n");
 508         return 0;
 509 }
 510
 511 #endif  /* DOCGEN */
 512
 513 /** Command descriptor for control */
 514 cmd_des_t const cmd_des_control = {
 515         0, 0,
 516         "demomotctrl","Run motor control demo - reads input and sends it",
 517         "### Command syntax ###\n"
 518         "\n"
 519         "    demomotctrl\n"
 520         "\n"
 521         "### Description ###\n"
 522         "\n"
 523         "This command creates a FlexRay node and starts to read buttons\n"
 524         "(connected to DIN0 and DIN1) and a potentiometer (ADC1) from a control\n"
 525         "panel. The read data are sent via FlexRay to the second node, created\n"
 526         "by running demomotdrive command.\n"
 527         "\n"
 528         "The purpose of this pair of commands is to demonstrate functionality\n"
 529         "of the FlexRay, ADC, DIN and HBR peripherals.\n",
 530         CMD_HANDLER(cmd_do_control), (void *)&cmd_list_motor_example
 531 };
 532
 533 /** Command descriptor for motor */
 534 cmd_des_t const cmd_des_motor = {
 535         0, 0,
 536         "demomotdrive","Run motor control demo - drives the DC motor",
 537         "### Command syntax ###\n"
 538         "\n"
 539         "    demomotdrive\n"
 540         "\n"
 541         "### Description ###\n"
 542         "\n"
 543         "This command creates a FlexRay node and starts to receive the data\n"
 544         "from another node created by command demomotctrl. The received data\n"
 545         "are applied to HBR to control the DC motor.\n"
 546         "\n"
 547         "The purpose of this pair of commands is to demonstrate functionality\n"
 548         "of the FlexRay, ADC, DIN and HBR peripherals.\n",
 549         CMD_HANDLER(cmd_do_motor), (void *)&cmd_list_motor_example
 550 };
 551
 552 /** List of commands for example, defined as external */
 553 cmd_des_t const *cmd_list_motor_example[] = {
 554         &cmd_des_control,
 555         &cmd_des_motor,
 556         NULL
 557 };