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 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.
  13  *
  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.
  18  *
  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/>.
  21  *
  22  * File : cmd_pin.c
  23  *
  24  * Abstract:
  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).
  29  */
  30
  31 #include "cmd_motor_example.h"
  32
  33 #ifndef DOCGEN
  34
  35 #include "rpp/rpp.h"
  36 #include "hal/hal.h"
  37 #include "drv/drv.h"
  38 #include "cmdproc.h"
  39
  40 #define ADC_MAX_VALUE 0x0983
  41 #define RX_ERROR_MAX 100
  42 #define ENABLE_DEBOUNCE_TIME    2
  43 #define DIRECTION_DEBOUNCE_TIME 2
  44
  45 /**
  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.
  49  */
  50 static Fr_TMS570LS_ClusterConfigType Fr_cluster_config = {
  51         .gColdStartAttempts = 0x2,
  52         .gListenNoise = 0xF,
  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,
  61         .gSyncNodeMax = 0xF,
  62         .gdActionPointOffset = 0x4,
  63         .gdCASRxLowMax = 0x43,
  64         .gdDynamicSlotIdlePhase = 0x1,
  65         .gdMinislot = 0x4,
  66         .gdMinislotActionPointOffset = 0x2,
  67         .gdNIT = 0xAE3,
  68         .gdSampleClockPeriod = 0,               // 10mbit/sec
  69         .gdStaticSlot = 0x56,
  70         .gdTSSTransmitter = 0xA,
  71         .gdWakeupSymbolRxIdle = 18,
  72         .gdWakeupSymbolRxLow = 18,
  73         .gdWakeupSymbolRxWindow = 76,
  74         .gdWakeupSymbolTxIdle = 180,
  75         .gdWakeupSymbolTxLow = 60
  76 };
  77
  78 /**
  79  * This structure contains local configuration of the FlexRay node for
  80  * the control panel reading and sending the data.
  81  */
  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,
  93         .pLatestTx = 0x10D,
  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,
 104         .pWakeupPattern = 2,
 105         .pdAcceptedStartupRange = 0x81,
 106         .pdListenTimeout = 0x36DA2,
 107         .pdMaxDrift = 0x151,
 108         .pDecodingCorrection = 0x33
 109 };
 110
 111 /**
 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.
 115  */
 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,
 127                 .pLatestTx = 0x10D,
 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,
 138                 .pWakeupPattern = 2,
 139                 .pdAcceptedStartupRange = 0x81,
 140                 .pdListenTimeout = 0x36DA2,
 141                 .pdMaxDrift = 0x151,
 142                 .pDecodingCorrection = 0x33
 143 };
 144
 145 /**
 146  * Message RAM configuration for the node reading the control panel.
 147  */
 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
 154 };
 155
 156 /**
 157  * Message RAM configuration for the node receiving the control data and driving the H=bridge.
 158  */
 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
 165 };
 166
 167 /**
 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.
 173  */
 174 static Fr_TMS570LS_BufferConfigType Fr_node_control_static_buffers_config[] = {
 175                 {
 176                         .channel = FR_CHANNEL_AB,
 177                         .cycleCounterFiltering = 0,
 178                         .isTx = TRUE,
 179                         .maxPayload = 2,
 180                         .msgBufferInterrupt = TRUE,
 181                         .payloadPreambleIndicatorTr = FALSE,
 182                         .rejectNullFrames = FALSE,
 183                         .rejectStaticSegment = FALSE,
 184                         .singleTransmit = FALSE,
 185                         .slotId = 1
 186                 },
 187                 {
 188                         .channel = FR_CHANNEL_AB,
 189                         .cycleCounterFiltering = 0,
 190                         .isTx = TRUE,
 191                         .maxPayload = 2,
 192                         .msgBufferInterrupt = TRUE,
 193                         .payloadPreambleIndicatorTr = FALSE,
 194                         .rejectNullFrames = FALSE,
 195                         .rejectStaticSegment = FALSE,
 196                         .singleTransmit = FALSE,
 197                         .slotId = 3
 198                 }
 199 };
 200
 201 /**
 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.
 206  */
 207 static Fr_TMS570LS_BufferConfigType Fr_node_motor_static_buffers_config[] = {
 208         {
 209                 .channel = FR_CHANNEL_AB,
 210                 .cycleCounterFiltering = 0,
 211                 .isTx = TRUE,
 212                 .maxPayload = 2,
 213                 .msgBufferInterrupt = TRUE,
 214                 .payloadPreambleIndicatorTr = FALSE,
 215                 .rejectNullFrames = FALSE,
 216                 .rejectStaticSegment = FALSE,
 217                 .singleTransmit = FALSE,
 218                 .slotId = 2
 219         },
 220         {
 221                 .channel = FR_CHANNEL_AB,
 222                 .cycleCounterFiltering = 0,
 223                 .isTx = FALSE,
 224                 .maxPayload = 2,
 225                 .msgBufferInterrupt = TRUE,
 226                 .payloadPreambleIndicatorTr = FALSE,
 227                 .rejectNullFrames = FALSE,
 228                 .rejectStaticSegment = FALSE,
 229                 .singleTransmit = FALSE,
 230                 .slotId = 3
 231         }
 232 };
 233
 234 /**
 235  * Unifying configuration structure for the node reading the control panel.
 236  */
 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
 244 };
 245
 246 /**
 247  * Unifying configuration structure for the node driving the motor.
 248  */
 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
 256 };
 257
 258 /**
 259  *      Loads data into TX buffers for FlexRay node sending the control data.
 260  *
 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.
 264  *
 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
 268  *
 269  *      @return SUCCESS when data were succesfully sent
 270  *              FAILURE when some error occured.
 271  */
 272 int8_t transmit_control_data(int enable, int direction, int duty) {
 273         uint8_t data[3];
 274
 275         // Write payload for buffer associated with frame 1
 276         data[0] = duty;
 277         data[1] = direction;
 278         data[2] = enable;
 279         return rpp_fr_transmit_lpdu(0, 3, data, 3);
 280 }
 281
 282 /**
 283  *      Checks if some message was received on message buffer 2 of the FlexRay motor controller node.
 284  *
 285  *      If new message was received, the function copies the data from the buffer.
 286  *
 287  *      @param enable   pointer to variable, where received enable flag will be stored
 288  *      @param direction pointer to variable, where received direction flag will be stored
 289  *      @param duty     pointer to variable, where received duty cycle value will be stored
 290  *      @return 0 if message was received and is correct
 291  *                      1 if message no message was received
 292  *                      2 if receiving failed
 293  */
 294 int receive_control_data(int* enable, int* direction, int* duty) {
 295         uint8_t data[3];
 296         Fr_RxLPduStatusType status;
 297         uint8_t size;
 298
 299         if (rpp_fr_receive_lpdu(0, 3, data, &status, &size) == FAILURE) {
 300                 return 2;
 301         }
 302         if (status == FR_NOT_RECEIVED) {
 303                 return 1;
 304         }
 305         else {
 306         *duty = data[0];
 307         *direction = data[1];
 308         *enable = data[2];
 309         return 0;
 310         }
 311 }
 312
 313 /**
 314  * @brief       Read data from control panel and send them to motor controller through FlexRay
 315  *
 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
 320  * switch to ground).
 321  * Finally all data are sent on the Frame 3.
 322  *
 323  *
 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
 328  */
 329 int cmd_do_control(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]) {
 330     port_desc_t* adc_desc;
 331     uint32_t adc_values[PORT_ADC_CHANNEL_NUM*2];
 332     int ret;
 333     int duty = 0;
 334     int button = 0;
 335     int enable = 0;
 336     int direction = 1;
 337     int enable_cnt = 0;                 // Debounce counter for enable button
 338     int dir_cnt = 0;                    // Debounce counter for direction button
 339     uint32_t error;
 340
 341     adc_desc = hal_port_get_dsc(PORT_NAME_ADC, -1);
 342
 343     if (rpp_din_setup(0, FALSE, TRUE, FALSE) == FAILURE) {      // For the red button, which is switch to battery
 344                 rpp_sci_printf("Din 0 setup failed.\n");
 345                 return -CMDERR_BADCFG;
 346     }
 347     if (rpp_din_setup(1, FALSE, TRUE, FALSE) == FAILURE) {      // For the black button, which is switch to ground
 348                 rpp_sci_printf("Din 1 setup failed.\n");
 349                 return -CMDERR_BADCFG;
 350     }
 351     if (rpp_din_update() == FAILURE) {
 352                 rpp_sci_printf("Din update failed.\n");
 353                 return -CMDERR_BADCFG;
 354     }
 355
 356     if (rpp_fr_init_driver(&Fr_config_node_control, &error) == FAILURE) {
 357                 rpp_sci_printf("Fray driver initialization failed: %#x.\n", error);
 358                 return -CMDERR_BADCFG;
 359     }
 360         if (rpp_fr_init_controller(0, &error) == FAILURE) {
 361                 rpp_sci_printf("Fray control node initialization failed: %#x.\n", error);
 362                 return -CMDERR_BADCFG;
 363         }
 364         rpp_sci_printf("Fray control node initialized.\r\n");
 365         rpp_sci_printf("Waiting for network connection...\r\n");
 366
 367         if (rpp_fr_start_communication(0, &error) == FAILURE) {
 368                 rpp_sci_printf("Integration to the network failed: %#x.\n", error);
 369                 return -CMDERR_BADCFG;
 370         }
 371         if (rpp_fr_all_slots(0) == FAILURE) {
 372                 rpp_sci_printf("All slots mode selection failed.\n");
 373                 return -CMDERR_BADCFG;
 374         }
 375
 376         rpp_sci_printf("Connected.\r\n");
 377
 378         while(1) {
 379                 vTaskDelay(10/portTICK_RATE_MS);
 380             ret = adc_desc->port_getfnc_ptr(adc_desc->config, PORT_ADC_CHANNEL_NUM, adc_values);
 381             if (ret < 0) {
 382                 rpp_sci_printf("ADC read failed!\n");
 383                 break;
 384             }
 385             duty = (100*adc_values[0])/ADC_MAX_VALUE;
 386             if (rpp_din_update() == FAILURE) {
 387                         rpp_sci_printf("Din update failed.\n");
 388                         break;
 389             }
 390             button = !rpp_din_get(0);
 391             if (button == 1) {
 392                 if (enable_cnt++ == ENABLE_DEBOUNCE_TIME) {
 393                         enable = !enable;
 394                 }
 395                 else {
 396                         enable_cnt++;
 397                 }
 398             }
 399             else {
 400                 enable_cnt = 0;
 401             }
 402             button = rpp_din_get(1);
 403             if (button == 1) {
 404                 if (dir_cnt++ == DIRECTION_DEBOUNCE_TIME) {
 405                         direction = !direction;
 406                 }
 407                 else {
 408                         dir_cnt++;
 409                 }
 410             }
 411             else {
 412                 dir_cnt = 0;
 413             }
 414
 415             if (transmit_control_data(enable, direction, duty) == FAILURE) {
 416                 rpp_sci_printf("Data transmission failed!\n");
 417                 break;
 418             }
 419             rpp_sci_printf("Enable: %d, Direction: %s, Duty: %d%%\r", enable, (direction == 1) ? "L" : "R", duty);
 420         }
 421
 422         if (rpp_fr_halt_communication(0) == FAILURE) {
 423                 rpp_sci_printf("FlexRay HALT command failed, please reset the board to stop transmission.\n");
 424         }
 425         else {
 426                 rpp_sci_printf("FlexRay halted, reset the board to make FlexRay usable again.\r\n");
 427         }
 428         return 0;
 429 }
 430
 431 /**
 432  * @brief       Receive data from FlexRay and control motor on HBR
 433  *
 434  * This FlexRay node receives the control data from the FlexRay network
 435  * and configures the H-bridge to drive the DC motor according the
 436  * parameters from the received message.
 437  *
 438  * @param[in]   cmd_io  Pointer to IO stack
 439  * @param[in]   des             Pointer to command descriptor
 440  * @param[in]   param   Parameters of command
 441  * @return      0 when OK or error code
 442  */
 443 int cmd_do_motor(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]) {
 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         }
 503         else {
 504                 rpp_sci_printf("H-bridge disabled.\n");
 505         }
 506         if (rpp_fr_halt_communication(0) == FAILURE) {
 507                 rpp_sci_printf("FlexRay HALT command failed, please reset the board to stop transmission.\n");
 508         }
 509         else {
 510                 rpp_sci_printf("FlexRay halted, reset the board to make FlexRay usable again.\r\n");
 511         }
 512         return 0;
 513 }
 514
 515 #endif  /* DOCGEN */
 516
 517 /** Command descriptor for control */
 518 cmd_des_t const cmd_des_control={
 519                 0, 0,
 520                 "demomotctrl","Run motor control demo - reads input and sends it",
 521                 "### Command syntax ###\n"
 522                 "\n"
 523                 "    demomotctrl\n"
 524                 "\n"
 525                 "### Description ###\n"
 526                 "\n"
 527                 "This command creates a FlexRay node and starts to read buttons\n"
 528                 "(connected to DIN0 and DIN1) and a potentiometer (ADC1) from a control\n"
 529                 "panel. The read data are sent via FlexRay to the second node, created\n"
 530                 "by running demomotdrive command.\n"
 531                 "\n"
 532                 "The purpose of this pair of commands is to demonstrate functionality\n"
 533                 "of the FlexRay, ADC, DIN and HBR peripherals.\n",
 534                 CMD_HANDLER(cmd_do_control), (void *)&cmd_list_motor_example
 535         };
 536
 537 /** Command descriptor for motor */
 538 cmd_des_t const cmd_des_motor={
 539                 0, 0,
 540                 "demomotdrive","Run motor control demo - drives the DC motor",
 541                 "### Command syntax ###\n"
 542                 "\n"
 543                 "    demomotdrive\n"
 544                 "\n"
 545                 "### Description ###\n"
 546                 "\n"
 547                 "This command creates a FlexRay node and starts to receive the data\n"
 548                 "from another node created by command demomotctrl. The received data\n"
 549                 "are applied to HBR to control the DC motor.\n"
 550                 "\n"
 551                 "The purpose of this pair of commands is to demonstrate functionality\n"
 552                 "of the FlexRay, ADC, DIN and HBR peripherals.\n",
 553                 CMD_HANDLER(cmd_do_motor), (void *)&cmd_list_motor_example
 554         };
 555
 556 /** List of commands for example, defined as external */
 557 cmd_des_t const *cmd_list_motor_example[]={
 558   &cmd_des_control,
 559   &cmd_des_motor,
 560   NULL
 561 };