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