rpp-test-sw/commands/cmd_motor_example.c

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