/* * Copyright (C) 2021-23 Texas Instruments Incorporated * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the * distribution. * * Neither the name of Texas Instruments Incorporated nor the names of * its contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPgResS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include #include #include #include #include #include #include #include #include "ti_drivers_open_close.h" #include "ti_board_open_close.h" #include #if PRU_ICSSGx_PRU_SLICE #define PRUICSS_PRUx PRUICSS_PRU1 #define PRUICSS_TXPRUx PRUICSS_TX_PRU1 #define PRUICSS_RTUPRUx PRUICSS_RTU_PRU1 #else #define PRUICSS_PRUx PRUICSS_PRU0 #define PRUICSS_TXPRUx PRUICSS_TX_PRU0 #define PRUICSS_RTUPRUx PRUICSS_RTU_PRU0 #endif #define PRUICSS_SLICEx PRU_ICSSGx_PRU_SLICE #if CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_SINGLE_PRU #include #endif #if (CONFIG_ENDAT0_CHANNEL0) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU) #include #endif #if (CONFIG_ENDAT0_CHANNEL1) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU) #include #endif #if (CONFIG_ENDAT0_CHANNEL2) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU) #include #endif #if CONFIG_ENDAT0_MODE == ENDAT_MODE_SINGLE_CHANNEL_SINGLE_PRU #include #endif #define WAIT_5_SECOND (5000) #define TASK_STACK_SIZE (4096) #define TASK_PRIORITY (6) #define ENDAT_RX_SAMPLE_SIZE 7 #define ENDAT_RX_SESQUI_DIV (1 << 15) #define MRS_POS_VAL2_WORD1 0x42 #define MRS_POS_VAL2_WORD2 0x43 #define MRS_POS_VAL2_WORD3 0x44 static union endat_format_data gEndat_format_data_mtrctrl[3]; static uint32_t gEndat_mtrctrl_crc_err[3]; static uint32_t gEndat_2_2_crc_position_err_cnt[3]; static uint32_t gEndat_2_2_crc_addinfo1_err_cnt[3]; static uint64_t gEndat_2_2_pos_val2[3]; static int32_t gEndat_2_2_loop_mrs; static void (*endat_fn_position_loop)(unsigned int); uint32_t gTaskFxnStack[TASK_STACK_SIZE/sizeof(uint32_t)] __attribute__((aligned(32))); TaskP_Object gTaskObject; #define VALID_HOST_CMD(x) ((x == 100) || ((x) == 101) || ((x) == 102) || ((x) == 103) || ((x) == 104) || ((x) == 105) || \ ((x) == 106) || ((x) == 107) || ((x) == 108) || ((x) == 109) || ((x) == 110)) #define HAVE_COMMAND_SUPPLEMENT(x) (((x) == 2) || ((x) == 3) || ((x) == 4) || ((x) == 7) || \ ((x) == 9) || ((x) == 10) || ((x) == 11) || ((x) == 13) || ((x) == 14) || \ ((x) == 100) || ((x) == 101) || ((x)== 103) || ((x) == 105) || ((x) == 106) || ((x) == 107) || ((x) == 108) || ((x) == 109)) #define ENDAT_INPUT_CLOCK_UART_FREQUENCY 192000000 /* use uart clock only to start with */ #define ENDAT_INPUT_CLOCK_FREQUENCY ENDAT_INPUT_CLOCK_UART_FREQUENCY #define ENDAT_POSITION_LOOP_STOP 0 #define ENDAT_POSITION_LOOP_START 1 union position { float angle; uint64_t length; }; struct endat_priv *priv; #define PRUSS_PRU1_CTRL_CTPPR1 (CSL_ICSS_G_PR1_PDSP1_IRAM_REGS_BASE + 0x2C) #define ENDAT_MULTI_CH0 (1 << 0) #define ENDAT_MULTI_CH1 (1 << 1) #define ENDAT_MULTI_CH2 (1 << 2) static uint32_t gEndat_prop_delay[3]; static uint32_t gEndat_prop_delay_max; static uint8_t gEndat_is_multi_ch; static uint8_t gEndat_multi_ch_mask; static uint8_t gEndat_is_load_share_mode; static char gUart_buffer[256]; /** \brief Global Structure pointer holding PRUSS1 memory Map. */ PRUICSS_Handle gPruIcssXHandle; /* buffer to handle long long printf */ char gPrintf_dump_buffer[21]; /* This function is used to convert uint64_t * variable tp a string as printf doesn't support * printing 64-bit variables */ char * uint64_to_str (uint64_t x) { char *b = gPrintf_dump_buffer + sizeof(gPrintf_dump_buffer); *(--b) = '\0'; do { *(--b) = '0' + (x % 10); x /= 10; } while (x); return b; } static void endat_pruss_init(void) { gPruIcssXHandle = PRUICSS_open(CONFIG_PRU_ICSS0); /* Configure g_mux_en to 1 in ICSSG_SA_MX_REG Register. */ PRUICSS_setSaMuxMode(gPruIcssXHandle, PRUICSS_SA_MUX_MODE_SD_ENDAT); /* Set in constant table C30 to shared RAM 0x40300000 */ PRUICSS_setConstantTblEntry(gPruIcssXHandle, PRUICSS_PRUx, PRUICSS_CONST_TBL_ENTRY_C30, ((0x40300000 & 0x00FFFF00) >> 8)); if(gEndat_is_load_share_mode) { PRUICSS_setConstantTblEntry(gPruIcssXHandle, PRUICSS_TXPRUx, PRUICSS_CONST_TBL_ENTRY_C30, ((0x40300000 & 0x00FFFF00) >> 8)); PRUICSS_setConstantTblEntry(gPruIcssXHandle, PRUICSS_RTUPRUx, PRUICSS_CONST_TBL_ENTRY_C30, ((0x40300000 & 0x00FFFF00) >> 8)); /*Set in constant table C29 for tx pru*/ PRUICSS_setConstantTblEntry(gPruIcssXHandle, PRUICSS_TXPRUx, PRUICSS_CONST_TBL_ENTRY_C28, 0x258); } /* clear ICSS0 PRU1 data RAM */ PRUICSS_initMemory(gPruIcssXHandle, PRUICSS_DATARAM(PRUICSS_SLICEx)); if(gEndat_is_load_share_mode) { PRUICSS_disableCore(gPruIcssXHandle, PRUICSS_RTUPRUx); PRUICSS_disableCore(gPruIcssXHandle, PRUICSS_TXPRUx); } PRUICSS_disableCore(gPruIcssXHandle, PRUICSS_PRUx); } void endat_pre_init(void) { endat_pruss_init(); } uint32_t endat_pruss_load_run_fw(struct endat_priv *priv) { uint32_t status = SystemP_FAILURE; #if CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU /*enable loadshare mode*/ #if(CONFIG_ENDAT0_CHANNEL0) status = PRUICSS_disableCore(gPruIcssXHandle, PRUICSS_RTUPRUx); DebugP_assert(SystemP_SUCCESS == status); status=PRUICSS_writeMemory(gPruIcssXHandle, PRUICSS_IRAM_RTU_PRU(PRUICSS_SLICEx), 0, (uint32_t *) EnDatFirmwareMultiMakeRTU, sizeof(EnDatFirmwareMultiMakeRTU)); DebugP_assert(0 != status); status = PRUICSS_resetCore(gPruIcssXHandle, PRUICSS_RTUPRUx); DebugP_assert(SystemP_SUCCESS == status); status = PRUICSS_enableCore(gPruIcssXHandle, PRUICSS_RTUPRUx); DebugP_assert(SystemP_SUCCESS == status); #endif #if(CONFIG_ENDAT0_CHANNEL1) status=PRUICSS_disableCore(gPruIcssXHandle, PRUICSS_PRUx ); DebugP_assert(SystemP_SUCCESS == status); status = PRUICSS_writeMemory(gPruIcssXHandle, PRUICSS_IRAM_PRU(PRUICSS_SLICEx), 0, (uint32_t *) EnDatFirmwareMultiMakePRU, sizeof(EnDatFirmwareMultiMakePRU)); DebugP_assert(0 != status); status = PRUICSS_resetCore(gPruIcssXHandle, PRUICSS_PRUx); DebugP_assert(SystemP_SUCCESS == status); status = PRUICSS_enableCore(gPruIcssXHandle, PRUICSS_PRUx); DebugP_assert(SystemP_SUCCESS == status); #endif #if(CONFIG_ENDAT0_CHANNEL2) status = PRUICSS_disableCore(gPruIcssXHandle, PRUICSS_TXPRUx); DebugP_assert(SystemP_SUCCESS == status); status = PRUICSS_writeMemory(gPruIcssXHandle, PRUICSS_IRAM_TX_PRU(PRUICSS_SLICEx), 0, (uint32_t *) EnDatFirmwareMultiMakeTXPRU, sizeof(EnDatFirmwareMultiMakeTXPRU)); DebugP_assert(0 != status); status = PRUICSS_resetCore(gPruIcssXHandle, PRUICSS_TXPRUx); DebugP_assert(SystemP_SUCCESS == status); status = PRUICSS_enableCore(gPruIcssXHandle, PRUICSS_TXPRUx); DebugP_assert(SystemP_SUCCESS == status); #endif status=endat_wait_initialization(priv, WAIT_5_SECOND, gEndat_multi_ch_mask); #else status = PRUICSS_disableCore(gPruIcssXHandle, PRUICSS_PRUx); DebugP_assert(SystemP_SUCCESS == status); #if(CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_SINGLE_PRU) status = PRUICSS_writeMemory(gPruIcssXHandle, PRUICSS_IRAM_PRU(PRUICSS_SLICEx), 0, (uint32_t *) EnDatFirmwareMulti, sizeof(EnDatFirmwareMulti)); #else status = PRUICSS_writeMemory(gPruIcssXHandle, PRUICSS_IRAM_PRU(PRUICSS_SLICEx), 0, (uint32_t *) EnDatFirmware, sizeof(EnDatFirmware)); #endif DebugP_assert(0 != status); status = PRUICSS_resetCore(gPruIcssXHandle, PRUICSS_PRUx); DebugP_assert(SystemP_SUCCESS == status); /*Run firmware */ status = PRUICSS_enableCore(gPruIcssXHandle, PRUICSS_PRUx); DebugP_assert(SystemP_SUCCESS == status); /* check initialization ack from firmware, with a timeout of 5 second */ status = endat_wait_initialization(priv, WAIT_5_SECOND, gEndat_multi_ch_mask); #endif return status; } uint64_t endat_get_fw_version(void) { #if CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_SINGLE_PRU return *((unsigned long *)EnDatFirmwareMulti + 2); #endif #if (CONFIG_ENDAT0_CHANNEL0) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU) return *((unsigned long *)EnDatFirmwareMultiMakeRTU + 2); #endif #if (CONFIG_ENDAT0_CHANNEL1) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU) return *((unsigned long *)EnDatFirmwareMultiMakePRU + 2); #endif #if (CONFIG_ENDAT0_CHANNEL2) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU) return *((unsigned long *)EnDatFirmwareMultiMakeTXPRU + 2); #endif #if CONFIG_ENDAT0_MODE == ENDAT_MODE_SINGLE_CHANNEL_SINGLE_PRU return *((unsigned long *)EnDatFirmware + 2); #endif } static void endat_print_menu(void) { DebugP_log("\r|------------------------------------------------------------------------------|\n"); DebugP_log("\r| Select value for the encoder command from following |\n"); DebugP_log("\r|------------------------------------------------------------------------------|\n"); DebugP_log("\r| 1 : Encoder send position values |\n"); DebugP_log("\r| 2 : Selection of memory area |\n"); DebugP_log("\r| 3 : Encoder receive parameter |\n"); DebugP_log("\r| 4 : Encoder send parameter |\n"); DebugP_log("\r| 5 : Encoder receive reset |\n"); DebugP_log("\r| 6 : Encoder send test values |\n"); DebugP_log("\r| 7 : Encoder receive test command |\n"); if(priv->cmd_set_2_2) { DebugP_log("\r| 8 : Encoder to send position + AI(s) |\n"); DebugP_log("\r| 9 : Encoder to send position + AI(s) and receive selection of memory area |\n"); DebugP_log("\r| 10: Encoder to send position + AI(s) and receive parameter |\n"); DebugP_log("\r| 11: Encoder to send position + AI(s) and send parameter |\n"); DebugP_log("\r| 12: Encoder to send position + AI(s) and receive error reset |\n"); DebugP_log("\r| 13: Encoder to send position + AI(s) and receive test command |\n"); DebugP_log("\r| 14: Encoder receive communication command |\n"); } DebugP_log("\r| |\n"); DebugP_log("\r|100: Configure clock |\n"); DebugP_log("\r|101: Simulate motor control 2.1 position loop |\n"); DebugP_log("\r|102: Toggle raw data display |\n"); DebugP_log("\r|103: Configure tST delay |\n"); if((!gEndat_is_multi_ch) && (!gEndat_is_load_share_mode)) { DebugP_log("\r|104: Start continuous mode |\n"); } DebugP_log("\r|105: Configure rx arm counter (account tD) |\n"); DebugP_log("\r|106: Configure rx clock disable time (for tD) |\n"); if(priv->cmd_set_2_2) { DebugP_log("\r|107: Simulate motor control 2.2 position loop %s |\n", gEndat_is_multi_ch || gEndat_is_load_share_mode ? " " : "(safety)"); } DebugP_log("\r|108: Configure propagation delay (tD) |\n"); if((gEndat_is_multi_ch) || (gEndat_is_load_share_mode)) { DebugP_log("\r|109: Configure wire delay |\n"); } DebugP_log("\r|110: Recovery Time (RT) |\n"); DebugP_log("\r|------------------------------------------------------------------------------|\n\r|\n"); DebugP_log("\r| enter value: "); } static inline int32_t endat_get_command(void) { volatile int32_t cmd = -1; if(DebugP_scanf("%d", &cmd) < 0) { cmd = -1; } if(VALID_2_1_CMD(cmd) || (priv->cmd_set_2_2 && VALID_2_2_CMD(cmd)) || VALID_HOST_CMD(cmd)) { return cmd; } DebugP_log("\r| WARNING: invalid command, EnDat 2.1 send position values command will be sent\n"); return 1; } static void endat_recvd_print(int32_t cmd, struct endat_priv *priv, union endat_format_data *u, int32_t crc) { uint32_t addinfo, byte1; uint64_t max = pow(2, priv->single_turn_res); union position position; /* this would give wrong values if cmd is not position related, but that is okay as then this value won't be used */ if(priv->type == rotary) { position.angle = ((float) u->position_addinfo.position.position) / (float)max * (float)360; } else { position.length = u->position_addinfo.position.position * priv->step; } DebugP_log("\r|\n\r|\n"); switch(cmd) { case 2: case 3: case 5: case 7: case 14: DebugP_log("\r| crc: %s\n", crc & 0x1 ? "success" : "failure"); DebugP_logInfo("| crc: %x\n", u->addr_params.crc); break; case 4: DebugP_log("\r| parameter: 0x%x, crc: %s\n", u->addr_params.params, crc & 0x1 ? "success" : "failure"); DebugP_logInfo("\r| crc: %x\n", u->addr_params.crc); break; case 6: DebugP_log("\r| test value: 0x%02x%08x, crc: %s\n", (uint32_t)((u->test.value & 0xFF00000000) >> 32), (uint32_t)(u->test.value & 0xFFFFFFFF), crc & 0x1 ? "success" : "failure"); DebugP_logInfo("\r| crc: %x\n", u->test.crc); break; case 1: if(priv->multi_turn_res) { sprintf(gUart_buffer, "\r| position: %.12f, revolution: %s, ", position.angle, uint64_to_str(u->position_addinfo.position.revolution)); } else { if(priv->type == rotary) { sprintf(gUart_buffer, "\r| position: %.12f ", position.angle); } else { sprintf(gUart_buffer, "\r| position: %s ", uint64_to_str(position.length)); } } DebugP_log("%s", gUart_buffer); DebugP_log("f1: %x, crc: %s\n", u->position_addinfo.position.f1, crc & 0x1 ? "success" : "failure"); DebugP_logInfo("| crc: %x\n", u->position_addinfo.position.crc); break; case 8: case 9: case 10: case 11: case 12: case 13: if(priv->multi_turn_res) { sprintf(gUart_buffer, "\r| position: %.12f, revolution: %s, ", position.angle, uint64_to_str(u->position_addinfo.position.revolution)); } else { if(priv->type == rotary) { sprintf(gUart_buffer, "\r| position: %.12f ", position.angle); } else { sprintf(gUart_buffer, "\r| position: %s ", uint64_to_str(position.length)); } } DebugP_log("%s", gUart_buffer); DebugP_log("f1/f2: %x/%x, crc: %s\n", u->position_addinfo.position.f1, u->position_addinfo.position.f2, crc & 0x1 ? "success" : "failure"); DebugP_logInfo("| crc: %x\n", u->position_addinfo.position.crc); if(priv->flags.info1) { addinfo = u->position_addinfo.addinfo1.addinfo; byte1 = (addinfo >> 16) & ((1 << 8) - 1); DebugP_log("\r|\n| WRN: %x RM: %x Busy: %x I4-I0: %x\n", (byte1 & ENDAT_STATUS_WARN_MASK) >> ENDAT_STATUS_WARN_SHIFT, (byte1 & ENDAT_STATUS_RM_MASK) >> ENDAT_STATUS_RM_SHIFT, (byte1 & ENDAT_STATUS_BUSY_MASK) >> ENDAT_STATUS_BUSY_SHIFT, byte1 & ENDAT_INFORMATION_MASK); DebugP_log("\r|\n| Additional Information 1: 0x%x, crc: %s\n", addinfo, crc & 0x2 ? "success" : "failure"); DebugP_logInfo("| addinfo1 crc: %x\n", u->position_addinfo.addinfo1.crc); } if(priv->flags.info2) { addinfo = u->position_addinfo.addinfo2.addinfo; byte1 = (addinfo >> 16) & ((1 << 8) - 1); DebugP_log("\r|\n| Additional Information 2: 0x%x, crc: %s\n", addinfo, crc & 0x4 ? "success" : "failure"); DebugP_logInfo("| addinfo2 crc: %x\n", u->position_addinfo.addinfo2.crc); } break; default: DebugP_log("\r|\n| ERROR: print requested for invalid command\n"); break; } DebugP_log("\r|\n\r|\n"); } static void endat_display_raw_data(int32_t cmd, struct endat_priv *priv) { int32_t ch = priv->channel; struct endat_pruss_xchg *pruss_xchg = priv->pruss_xchg; switch(cmd) { case 1: case 2: case 3: case 4: case 5: case 6: case 7: case 8: case 9: case 10: case 11: case 12: case 13: case 14: DebugP_log("\r|\n| raw data: %x %x %x %x\n|\n", pruss_xchg->ch[ch].pos_word0, pruss_xchg->ch[ch].pos_word1, pruss_xchg->ch[ch].pos_word2, pruss_xchg->ch[ch].pos_word3); break; default: DebugP_log("\r|\n| Nothing raw to display - this is not a valid command\n|\n"); break; } } /* * check 2.2 command case with 2.2 capability in encoder, can live w/o as endat_get_command * will handle and it is assumed that this function will be called after endat_get_command */ static int32_t endat_get_command_supplement(int32_t cmd, struct cmd_supplement *cmd_supplement) { /* erase previous cmd supplements */ memset(cmd_supplement, 0, sizeof(*cmd_supplement)); switch(cmd) { case 2: DebugP_log("\r| enter MRS code (hex value): "); if(DebugP_scanf("%x\n", &cmd_supplement->address) < 0) { DebugP_log("\r| ERROR: invalid MRS code\n|\n|\n|\n"); return -EINVAL; } if(cmd_supplement->address > 0xFF) { DebugP_log("\r| ERROR: invalid MRS code\n|\n|\n|\n"); return -EINVAL; } break; case 9: DebugP_log("\r| enter MRS code (hex value): "); if(DebugP_scanf("%x\n", &cmd_supplement->address) < 0) { DebugP_log("\r| ERROR: invalid MRS code\n|\n|\n|\n"); return -EINVAL; } if(cmd_supplement->address > 0xFF) { DebugP_log("\r| ERROR: invalid MRS code\n|\n|\n|\n"); return -EINVAL; } if(cmd_supplement->address == ENDAT_SECTION2_MEMORY) { DebugP_log("\r| enter block address (hex value): "); if(DebugP_scanf("%x\n", &cmd_supplement->block) < 0) { DebugP_log("\r| ERROR: invalid block address\n|\n|\n|\n"); return -EINVAL; } /* better compare it with number of blocks information available in eeprom */ if(cmd_supplement->block > 0xFF) { DebugP_log("\r| ERROR: invalid block address\n|\n|\n|\n"); return -EINVAL; } cmd_supplement->has_block_address = TRUE; } break; case 3: case 10: DebugP_log("\r| enter parameter address (hex value): "); if(DebugP_scanf("%x\n", &cmd_supplement->address) < 0) { DebugP_log("\r| ERROR: invalid parameter address\n|\n|\n|\n"); return -EINVAL; } if(cmd_supplement->address > 0xFF) { DebugP_log("\r| ERROR: invalid parameter address\n|\n|\n|\n"); return -EINVAL; } DebugP_log("\r| enter parameter (hex value): "); if(DebugP_scanf("%x\n", &cmd_supplement->data) < 0) { DebugP_log("\r| ERROR: invalid parameter\n|\n|\n|\n"); return -EINVAL; } if(cmd_supplement->data > 0xFFFF) { DebugP_log("\r| ERROR: invalid parameter\n|\n|\n|\n"); return -EINVAL; } break; case 4: case 11: DebugP_log("\r| enter parameter address (hex value): "); if(DebugP_scanf("%x\n", &cmd_supplement->address) < 0) { DebugP_log("\r| ERROR: invalid parameter address\n|\n|\n|\n"); return -EINVAL; } if(cmd_supplement->address > 0xFF) { DebugP_log("\r| ERROR: invalid parameter address\n|\n|\n|\n"); return -EINVAL; } break; case 7: case 13: DebugP_log("\r| enter port address (hex value):"); if(DebugP_scanf("%x\n", &cmd_supplement->address) < 0) { DebugP_log("\r| ERROR: invalid port address\n|\n|\n|\n"); return -EINVAL; } if(cmd_supplement->address > 0xFF) { DebugP_log("| ERROR: invalid port address\n|\n|\n|\n"); return -EINVAL; } break; case 14: DebugP_log("\r| enter encoder address (hex value): "); if(DebugP_scanf("%x\n", &cmd_supplement->address) < 0) { DebugP_log("\r| ERROR: invalid encoder address\n|\n|\n|\n"); return -EINVAL; } if(cmd_supplement->address > 0xFF) { DebugP_log("\r| ERROR: invalid encoder address\n|\n|\n|\n"); return -EINVAL; } DebugP_log("\r| enter instruction (hex value): "); if(DebugP_scanf("%x\n", &cmd_supplement->data) < 0) { DebugP_log("\r| ERROR: invalid instruction\n|\n|\n|\n"); return -EINVAL; } if(cmd_supplement->data > 0xFFFF) { DebugP_log("\r| ERROR: invalid instruction\n|\n|\n|\n"); return -EINVAL; } break; case 100: case 101: case 107: DebugP_log("\r| enter frequency in Hz: "); if(DebugP_scanf("%u\n", &cmd_supplement->frequency) < 0) { DebugP_log("\r| ERROR: invalid frequency\n|\n|\n|\n"); return -EINVAL; } break; case 103: DebugP_log("\r| enter tST delay counter value in ns: "); if(DebugP_scanf("%u\n", &cmd_supplement->frequency) < 0) { DebugP_log("\r| ERROR: invalid value\n|\n|\n|\n"); return -EINVAL; } if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { DebugP_log("\r| Select Channel: "); if(DebugP_scanf("%u\n", &priv->channel) < 0) { DebugP_log("\r| ERROR: invalid channel\n|\n|\n|\n"); return -EINVAL; } if(!((gEndat_multi_ch_mask) & (1<channel))) { DebugP_log("\r| ERROR: invalid channel\n|\n|\n|\n"); return -EINVAL; } } break; case 105: DebugP_log("\r| enter rx arm counter in ns: "); if(DebugP_scanf("%u\n", &cmd_supplement->frequency) < 0) { DebugP_log("\r| ERROR: invalid value\n|\n|\n|\n"); return -EINVAL; } if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { DebugP_log("\r| Select Channel: "); if(DebugP_scanf("%u\n", &priv->channel) < 0) { DebugP_log("\r| ERROR: invalid channel\n|\n|\n|\n"); return -EINVAL; } if(!((gEndat_multi_ch_mask) & (1<channel))) { DebugP_log("\r| ERROR: invalid channel\n|\n|\n|\n"); return -EINVAL; } } break; case 106: DebugP_log("\r| enter rx clock disable time (for tD) in ns: "); if(DebugP_scanf("%u\n", &cmd_supplement->frequency) < 0) { DebugP_log("\r| ERROR: invalid value\n|\n|\n|\n"); return -EINVAL; } if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { DebugP_log("\r| Select Channel: "); if(DebugP_scanf("%u\n", &priv->channel) < 0) { DebugP_log("\r| ERROR: invalid channel\n|\n|\n|\n"); return -EINVAL; } if(!((gEndat_multi_ch_mask) & (1<channel))) { DebugP_log("\r| ERROR: invalid channel\n|\n|\n|\n"); return -EINVAL; } } break; case 108: DebugP_log("\r| enter propagation delay in ns: "); if(DebugP_scanf("%u\n", &cmd_supplement->frequency) < 0) { DebugP_log("\r| ERROR: invalid value\n|\n|\n|\n"); return -EINVAL; } if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { DebugP_log("\r| Select Channel: "); if(DebugP_scanf("%u\n", &priv->channel) < 0) { DebugP_log("\r| ERROR: invalid channel\n|\n|\n|\n"); return -EINVAL; } if(!((gEndat_multi_ch_mask) & (1<channel))) { DebugP_log("\r| ERROR: invalid channel\n|\n|\n|\n"); return -EINVAL; } } break; case 109: DebugP_log("\r| enter channel number: "); if(DebugP_scanf("%x\n", &cmd_supplement->address) < 0) { DebugP_log("\r| ERROR: invalid channel\n|\n|\n|\n"); return -EINVAL; } if(!(gEndat_multi_ch_mask & 1 << cmd_supplement->address)) { DebugP_log("\r| ERROR: invalid channel\n|\n|\n|\n"); return -EINVAL; } DebugP_log("\r| enter wire delay in ns: "); if(DebugP_scanf("%u\n", &cmd_supplement->frequency) < 0) { DebugP_log("\r| ERROR: invalid value\n|\n|\n|\n"); return -EINVAL; } break; default: cmd = -EINVAL; DebugP_log("\r| ERROR: no command data required for the command\n"); break; } return cmd; } static int32_t endat_handle_user(struct cmd_supplement *cmd_supplement) { int32_t cmd; endat_print_menu(); cmd = endat_get_command(); if(HAVE_COMMAND_SUPPLEMENT(cmd)) { return endat_get_command_supplement(cmd, cmd_supplement); } return cmd; } static int32_t endat_calc_clock(uint32_t freq, struct endat_clk_cfg *clk_cfg) { uint32_t ns; if(freq > 16000000 || (freq > 12000000 && freq < 16000000)) { DebugP_log("\r| ERROR: frequency above 16MHz, between 12 & 16MHz not allowed\n|\n|\n"); return -1; } if((freq != 16000000) && (ENDAT_INPUT_CLOCK_FREQUENCY % (freq * 8))) DebugP_log("\r| WARNING: exact clock divider is not possible, frequencies set would be tx: %u\trx: %u\n", ENDAT_INPUT_CLOCK_FREQUENCY / (ENDAT_INPUT_CLOCK_FREQUENCY / freq), ENDAT_INPUT_CLOCK_FREQUENCY / (ENDAT_INPUT_CLOCK_FREQUENCY / (freq * 8))); ns = 2 * 1000000000 / freq; /* rx arm >= 2 clock */ /* should be divisible by 5 */ if(ns % 5) { ns /= 5, ns += 1, ns *= 5; } clk_cfg->tx_div = ENDAT_INPUT_CLOCK_FREQUENCY / freq - 1; clk_cfg->rx_div = ENDAT_INPUT_CLOCK_FREQUENCY / (freq * 8) - 1; clk_cfg->rx_en_cnt = ns; clk_cfg->rx_div_attr = ENDAT_RX_SAMPLE_SIZE; if(freq == 16000000) { clk_cfg->rx_div_attr |= ENDAT_RX_SESQUI_DIV; } DebugP_logInfo("\r| clock config values - tx_div: %u\trx_div: %u\trx_en_cnt: %u\trx_div_attr: %x\n", clk_cfg->tx_div, clk_cfg->rx_div, clk_cfg->rx_en_cnt, clk_cfg->rx_div_attr); return 0; } static uint32_t endat_do_sanity_tst_delay(uint32_t delay) { /* (unsigned short)~0 is also a multiple of 5 */ if(delay > (unsigned short)~0) { DebugP_log("\r| ERROR: delay greater than %uns, enter lesser value\n|\n|\n", (unsigned short)~0); return delay; } if(delay % 5) { delay += 5, delay /= 5, delay *= 5; DebugP_log("\r| WARNING: delay not multiple of 5ns, rounding to %uns\n|\n|\n", delay); } return delay; } static int32_t endat_position_loop_status; /* position period in microsecond */ static int32_t endat_calc_position_period(uint32_t freq) { /* 16KHz limitation due to the timer */ if(freq > 16000) { DebugP_log("\r| ERROR: enter frequency less than or equal 16KHz\n|\n|\n"); return -1; } else if((gEndat_is_multi_ch || gEndat_is_load_share_mode)&& freq > 8000) { DebugP_log("\r| ERROR: enter frequency less than or equal 8KHz in multi channel configuration\n|\n|\n"); return -1; } return 1000000 / freq; } static void endat_position_loop_decide_termination(void *args) { char c; while(1) { DebugP_scanf("%c", &c); endat_position_loop_status = ENDAT_POSITION_LOOP_STOP; break; } TaskP_exit(); } void endat_process_position_command(uint32_t a0) { uint32_t crc; endat_command_process(priv, 1, NULL); endat_recvd_process(priv, 1, &gEndat_format_data_mtrctrl[a0]); crc = endat_recvd_validate(priv, 1, &gEndat_format_data_mtrctrl[a0]); if(!(crc & 0x1)) { gEndat_mtrctrl_crc_err[a0]++; } } uint16_t _endat_process_2_2_position_command(int32_t cmd, struct cmd_supplement *cmd_supplement, uint32_t a0) { uint32_t crc; endat_command_process(priv, cmd, cmd_supplement); endat_recvd_process(priv, cmd, &gEndat_format_data_mtrctrl[a0]); crc = endat_recvd_validate(priv, cmd, &gEndat_format_data_mtrctrl[a0]); if(!(crc & 0x1)) { gEndat_2_2_crc_position_err_cnt[a0]++; } if(priv->flags.info1 && !(crc & 0x2)) { gEndat_2_2_crc_addinfo1_err_cnt[a0]++; } endat_addinfo_track(priv, cmd, cmd_supplement); return gEndat_format_data_mtrctrl[a0].position_addinfo.addinfo1.addinfo & 0xFFFF; } void endat_process_2_2_position_command(uint32_t a0) { uint32_t cmd; struct cmd_supplement cmd_supplement; uint16_t pos_word; if(((!gEndat_is_multi_ch || !gEndat_is_load_share_mode) && priv->has_safety) ) { cmd = 9, cmd_supplement.address = gEndat_2_2_loop_mrs; } else { cmd = 8; } pos_word = _endat_process_2_2_position_command(cmd, &cmd_supplement, a0); if((gEndat_is_multi_ch) || (!priv->has_safety) || (gEndat_is_load_share_mode)) { return; } /* WORD3 in addinfo1 */ if(gEndat_2_2_loop_mrs == MRS_POS_VAL2_WORD1) { gEndat_2_2_pos_val2[a0] &= 0xFFFF0000FFFFFFFF; gEndat_2_2_pos_val2[a0] |= (unsigned long long)pos_word << 32; gEndat_2_2_loop_mrs = MRS_POS_VAL2_WORD2; /* WORD1 in addinfo1 */ } else if(gEndat_2_2_loop_mrs == MRS_POS_VAL2_WORD2) { gEndat_2_2_pos_val2[a0] &= 0xFFFFFFFFFFFF0000; gEndat_2_2_pos_val2[a0] |= pos_word; gEndat_2_2_loop_mrs = MRS_POS_VAL2_WORD3; /* WORD2 in addinfo1 */ } else if(gEndat_2_2_loop_mrs == MRS_POS_VAL2_WORD3) { gEndat_2_2_pos_val2[a0] &= 0xFFFFFFFF0000FFFF; gEndat_2_2_pos_val2[a0] |= (unsigned long long)pos_word << 16; gEndat_2_2_loop_mrs = MRS_POS_VAL2_WORD1; } } void endat_position_loop(uint32_t a0) { if(endat_fn_position_loop != NULL) { if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; for(j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { endat_multi_channel_set_cur(priv, j); endat_fn_position_loop(j); } } } else { endat_fn_position_loop(0); } } } static int32_t endat_loop_task_create(void) { uint32_t status; TaskP_Params taskParams; TaskP_Params_init(&taskParams); taskParams.name = "endat_position_loop_decide_termination"; taskParams.stackSize = TASK_STACK_SIZE; taskParams.stack = (uint8_t *)gTaskFxnStack; taskParams.priority = TASK_PRIORITY; taskParams.taskMain = (TaskP_FxnMain)endat_position_loop_decide_termination; status = TaskP_construct(&gTaskObject, &taskParams); if(status != SystemP_SUCCESS) { DebugP_log("\rTask2 creation failed\n"); } return status ; } static void endat_loop_timer_create(int32_t us) { TimerP_Params timerParams; TimerP_Params_init(&timerParams); timerParams.inputPreScaler = CONFIG_TIMER0_INPUT_PRE_SCALER; timerParams.inputClkHz = CONFIG_TIMER0_INPUT_CLK_HZ; timerParams.periodInUsec = us; timerParams.oneshotMode = 0; timerParams.enableOverflowInt = 1; TimerP_setup(gTimerBaseAddr[CONFIG_TIMER0], &timerParams); return ; } static int32_t endat_get_position_loop_chars(struct endat_priv *priv, int32_t continuous, int32_t is_2_2) { int32_t i; if(priv->multi_turn_res) { i = 34; } else { i = 16; } if(!continuous) { i += 12; } i += 4; if(!continuous && is_2_2) { i += 4; } return i; } static void endat_print_position_loop(struct endat_priv *priv, int32_t continuous, int32_t is_2_2, int32_t ch) { uint64_t max = pow(2, priv->single_turn_res); union position position; if(priv->type == rotary) { position.angle = ((float) gEndat_format_data_mtrctrl[ch].position_addinfo.position.position) / (float)max * (float)360; } else { position.length = gEndat_format_data_mtrctrl[ch].position_addinfo.position.position * priv->step; } /* max value is 2x48, has 15 digits, so 16 is safe */ if(priv->multi_turn_res) { sprintf(gUart_buffer, "%16.12f, %16s", position.angle, uint64_to_str(gEndat_format_data_mtrctrl[ch].position_addinfo.position.revolution)); } else { if(priv->type == rotary) { sprintf(gUart_buffer, "%16.12f", position.angle); } else { sprintf(gUart_buffer, "%16s", uint64_to_str(position.length)); } } DebugP_log("\r%s", gUart_buffer); if(!continuous) { if(is_2_2) { DebugP_log(", %10u", gEndat_2_2_crc_position_err_cnt[ch]); } else { DebugP_log(", %10u", gEndat_mtrctrl_crc_err[ch]); } } DebugP_log(",%3u", gEndat_format_data_mtrctrl[ch].position_addinfo.position.f1); if(!continuous && is_2_2) { DebugP_log(",%3u", gEndat_format_data_mtrctrl[ch].position_addinfo.position.f2); } } static void endat_print_position_loop_channel_info(struct endat_priv *priv, int32_t is_2_2) { int32_t k, j, i = endat_get_position_loop_chars(priv, 0, is_2_2); /* add 3 extra to account for spacing b/n channels */ i += 3; /* find mid point */ i /= 2; /* account for "CHANNEL x" display */ i -= 4; for(k = i, j = 0; j < 3; j++, k = i) if(gEndat_multi_ch_mask & 1 << j) { while(k--) { DebugP_log("%c", ' '); } DebugP_log("\rCHANNEL %d", j); /* 3 - extra to account for spacing b/n channels, 9 - "CHANNEL x" length */ k = endat_get_position_loop_chars(priv, 0, is_2_2) + 3 - i - 9; while(k--) { DebugP_log("%c", ' '); } } } static void endat_handle_prop_delay(struct endat_priv *priv, uint16_t prop_delay) { if(prop_delay > priv->rx_en_cnt) { uint16_t dis = (prop_delay - priv->rx_en_cnt) * 2 / priv->rx_en_cnt; endat_config_rx_arm_cnt(priv, prop_delay); /* propagation delay - 2T */ endat_config_rx_clock_disable(priv, dis); } else { endat_config_rx_arm_cnt(priv, priv->rx_en_cnt); endat_config_rx_clock_disable(priv, 0); } } static void endat_process_host_command(int32_t cmd, struct cmd_supplement *cmd_supplement, struct endat_priv *priv) { struct endat_clk_cfg clk_cfg; static int32_t timer_init; /* clock configuration */ if(cmd == 100) { if(endat_calc_clock(cmd_supplement->frequency, &clk_cfg) < 0) { return; } endat_config_clock(priv, &clk_cfg); priv->rx_en_cnt = clk_cfg.rx_en_cnt; if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; uint16_t d; for(j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { endat_multi_channel_set_cur(priv, j); endat_handle_prop_delay(priv, gEndat_prop_delay[priv->channel]); d = gEndat_prop_delay_max - gEndat_prop_delay[j]; endat_config_wire_delay(priv, d); } } } else { endat_handle_prop_delay(priv, gEndat_prop_delay[priv->channel]); } /* set tST to 2us if frequency > 1MHz, else turn it off */ if(cmd_supplement->frequency >= 1000000) { cmd_supplement->frequency = 2000; } else { cmd_supplement->frequency = 0; } /* control loop */ if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; for(j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { endat_multi_channel_set_cur(priv, j); endat_process_host_command(103, cmd_supplement, priv); } } } else { endat_process_host_command(103, cmd_supplement, priv); } } else if(cmd == 101) { int32_t us = endat_calc_position_period(cmd_supplement->frequency); if(us < 0) { return; } if(endat_loop_task_create() != SystemP_SUCCESS) { DebugP_log("\r| ERROR: OS not allowing position loop as related Task creation failed\n|\n|\n"); DebugP_log("\rTask_create() failed!\n"); return; } endat_fn_position_loop = endat_process_position_command; if(!timer_init) { endat_loop_timer_create(us); timer_init = 1; } TimerP_start(gTimerBaseAddr[CONFIG_TIMER0]); endat_position_loop_status = ENDAT_POSITION_LOOP_START; if(priv->multi_turn_res) { DebugP_log("\r|\r\n| press enter to stop the position display|\n"); if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; DebugP_log("\r|\n\r|"); endat_print_position_loop_channel_info(priv, 0); DebugP_log("\r\n|\n"); for(j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { DebugP_log("| "); DebugP_log(" position, revolution, crc errors, f1\n"); DebugP_log(" "); } } DebugP_log("\n"); } else { DebugP_log("\r| position, revolution, crc errors, f1|\r\n "); } } else { DebugP_log("|\n| press enter to stop the position display\n|\n"); if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; DebugP_log("\r|\n\r|"); endat_print_position_loop_channel_info(priv, 0); DebugP_log("\r\n|\n"); for(j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { DebugP_log("| "); DebugP_log(" position, crc errors, f1\n"); DebugP_log(" "); } } DebugP_log("\n"); } else { DebugP_log("| position, crc errors, f1\n| "); } } while(1) if(endat_position_loop_status == ENDAT_POSITION_LOOP_STOP) { TimerP_stop(gTimerBaseAddr[CONFIG_TIMER0]); return; } else { int32_t i; if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; for(i = 0, j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { endat_multi_channel_set_cur(priv, j); i += endat_get_position_loop_chars(priv, 0, 0); DebugP_log("| "); endat_print_position_loop(priv, 0, 0, j); DebugP_log(" "); DebugP_log("\n"); i += 3; } } } else { i = endat_get_position_loop_chars(priv, 0, 0); endat_print_position_loop(priv, 0, 0, 0); } /* increase sleep value if glitches in display to be prevented (and would result in slower position display freq) */ ClockP_usleep(500); while(i--) { DebugP_log("%c", 8); } } } else if(cmd == 102) { priv->raw_data ^= 1; } else if(cmd == 103) { uint32_t delay; delay = endat_do_sanity_tst_delay(cmd_supplement->frequency); if(delay <= (uint16_t)~0) { endat_config_tst_delay(priv, (uint16_t) delay); } } else if(cmd == 104) { if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { DebugP_log("\r| ERROR: Multi channel configuration does not support continuous clock mode\r\n|\r\n|\n"); return; } if(endat_loop_task_create() != SystemP_SUCCESS) { DebugP_log("\r| ERROR: OS not allowing continuous mode as related Task creation failed\r\n|\r\n|\n"); DebugP_log("Task_create() failed!\n"); return; } endat_start_continuous_mode(priv); endat_position_loop_status = ENDAT_POSITION_LOOP_START; if(priv->multi_turn_res) { DebugP_log("\r|\n\r| press enter to stop the continuous mode\r\n|\r\n| position, revolution, f1\r\n| "); } else { DebugP_log("\r|\n\r| press enter to stop the continuous mode\r\n|\r\n| position, f1\r\n| "); } while(1) if(endat_position_loop_status == ENDAT_POSITION_LOOP_STOP) { endat_stop_continuous_mode(priv); return; } else { int i; endat_recvd_process(priv, 1, &gEndat_format_data_mtrctrl[0]); i = endat_get_position_loop_chars(priv, 1, 0); endat_print_position_loop(priv, 1, 0, 0); /* increase sleep value if glitches in display to be prevented (and would result in slower position display freq) */ ClockP_usleep(100); while(i--) { DebugP_log("%c", 8); } } } else if(cmd == 105) { uint32_t val; /* reuse tST delay sanity check */ val = endat_do_sanity_tst_delay(cmd_supplement->frequency); if(val <= (uint16_t)~0) { endat_config_rx_arm_cnt(priv, (uint16_t)val); } } else if(cmd == 106) { uint16_t dis = cmd_supplement->frequency * 2 / priv->rx_en_cnt; endat_config_rx_clock_disable(priv, dis); } else if(cmd == 108) { /* reuse tST delay sanity check */ uint32_t val = endat_do_sanity_tst_delay(cmd_supplement->frequency); if(val > (uint16_t)~0) { return; } endat_handle_prop_delay(priv, (uint16_t)val); } else if(cmd == 109) { /* reuse tST delay sanity check */ uint32_t val = endat_do_sanity_tst_delay(cmd_supplement->frequency); endat_multi_channel_set_cur(priv, cmd_supplement->address); endat_config_wire_delay(priv, val); } else if(cmd == 107) { int32_t us = endat_calc_position_period(cmd_supplement->frequency); if(us < 0) { return; } if(endat_loop_task_create() != SystemP_SUCCESS) { DebugP_log("\r| ERROR: OS not allowing position loop as related Task creation failed\n|\n|\n"); DebugP_log("\rTask_create() failed!\n"); return; } endat_fn_position_loop = endat_process_2_2_position_command; if(!timer_init) { endat_loop_timer_create(us); timer_init = 1; } /* reset additional info's if present */ endat_command_process(priv, 5, NULL); endat_addinfo_track(priv, 5, NULL); gEndat_2_2_loop_mrs = MRS_POS_VAL2_WORD1; TimerP_start(gTimerBaseAddr[CONFIG_TIMER0]); endat_position_loop_status = ENDAT_POSITION_LOOP_START; /* so that proper position value 2 is displayed from the begining */ ClockP_usleep(us * 3); if((!gEndat_is_multi_ch && !priv->has_safety) || (!gEndat_is_load_share_mode && !priv->has_safety)) { DebugP_log("\r|\n| encoder does not support safety, position value 2 would not be displayed\n|\n"); } DebugP_log("\r|\n\r| press enter to stop the position display\n\r|\n"); if(priv->multi_turn_res) { if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; DebugP_log("\r|\n\r|"); endat_print_position_loop_channel_info(priv, 1); DebugP_log("\n\r|\n"); for(j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { DebugP_log("\r| "); DebugP_log(" position, revolution, crc errors, f1, f2"); DebugP_log(" "); } } DebugP_log("\r\n"); } else { DebugP_log("\r| position, revolution, crc errors, f1, f2"); if(priv->has_safety) { DebugP_log(", position(2), revolution(2), crc errors(2)"); } DebugP_log("\r\n| "); } } else { if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; DebugP_log("\r|\n\r|"); endat_print_position_loop_channel_info(priv, 1); DebugP_log("\r\n|\r\n"); for(j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { DebugP_log("| "); DebugP_log(" position, crc errors, f1"); DebugP_log(" "); } } DebugP_log("\n"); } else { DebugP_log("| position, crc errors, f1, f2 "); if(priv->has_safety) { DebugP_log(", position(2), crc errors(2)"); } DebugP_log("\r\n| "); } } while(1) if(endat_position_loop_status == ENDAT_POSITION_LOOP_STOP) { TimerP_stop(gTimerBaseAddr[CONFIG_TIMER0]); /* reset additional info1 */ endat_command_process(priv, 5, NULL); endat_addinfo_track(priv, 5, NULL); return; } else { int32_t i; if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; for(i = 0, j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { endat_multi_channel_set_cur(priv, j); i += endat_get_position_loop_chars(priv, 0, 1); DebugP_log("| "); endat_print_position_loop(priv, 0, 1, j); DebugP_log(" "); DebugP_log("\n"); i += 3; } } } else { i = endat_get_position_loop_chars(priv, 0, 1); endat_print_position_loop(priv, 0, 1, 0); } if((!gEndat_is_multi_ch && priv->has_safety) || (!gEndat_is_load_share_mode && priv->has_safety)) { uint64_t multi_turn, single_turn; union position position2; uint64_t max = pow(2, priv->single_turn_res); multi_turn = ENDAT_GET_POS_MULTI_TURN(gEndat_2_2_pos_val2[0], priv); single_turn = ENDAT_GET_POS_SINGLE_TURN(gEndat_2_2_pos_val2[0], priv); if(priv->type == rotary) { position2.angle = (float)single_turn / (float)max * (float)360; } else { position2.length = single_turn * priv->step; } DebugP_log(", "); if(priv->multi_turn_res) { sprintf(gUart_buffer, "%16.12f, %16s", position2.angle, uint64_to_str(multi_turn)); } else { if(priv->type == rotary) { sprintf(gUart_buffer, "%16.12f", position2.angle); } else { sprintf(gUart_buffer, "%16s", uint64_to_str(position2.length)); } } DebugP_log("%s", gUart_buffer); DebugP_log(", %10u", gEndat_2_2_crc_addinfo1_err_cnt[0]); } /* increase sleep value if glitches in display to be prevented (and would result in slower position display freq) */ ClockP_usleep(100); if((!gEndat_is_multi_ch && priv->has_safety) || (!gEndat_is_load_share_mode && priv->has_safety)) { if(priv->multi_turn_res) { i += 2 + 46 + 3; } else { i += 2 + 28 + 3; } } while(i--) { DebugP_log("%c", 8); } } } else if(cmd == 110) { uint32_t recovery_time; if(gEndat_is_multi_ch||gEndat_is_load_share_mode) { int32_t j; for( j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { endat_multi_channel_set_cur(priv, j); DebugP_log("channel: %d",priv->channel); DebugP_log("\t"); recovery_time = endat_get_recovery_time(priv); DebugP_log("Recovery Time: %d ns", recovery_time); DebugP_log("\n"); } } } else { recovery_time = endat_get_recovery_time(priv); DebugP_log("Recovery Time: %d ns", recovery_time); DebugP_log("\n"); } } else { DebugP_log("\r| ERROR: non host command being requested to be handled as host command\n|\n|\n"); } } static void endat_handle_rx(struct endat_priv *priv, int32_t cmd) { uint32_t crc; union endat_format_data endat_format_data; if(priv->raw_data) { endat_display_raw_data(cmd, priv); } endat_recvd_process(priv, cmd, &endat_format_data); crc = endat_recvd_validate(priv, cmd, &endat_format_data); endat_recvd_print(cmd, priv, &endat_format_data, crc); return; } static void endat_print_encoder_info(struct endat_priv *priv) { DebugP_log("EnDat 2.%d %s encoder\tID: %u %s\tSN: %c %u %c\n\n", priv->cmd_set_2_2 ? 2 : 1, (priv->type == rotary) ? "rotary" : "linear", priv->id.binary, (char *)&priv->id.ascii, (char)priv->sn.ascii_msb, priv->sn.binary, (char)priv->sn.ascii_lsb); DebugP_log("\rPosition: %d bits ", priv->pos_res); if(priv->type == rotary) { DebugP_log("(singleturn: %d, multiturn: %d) ", priv->single_turn_res, priv->multi_turn_res); } DebugP_log("[resolution: %d %s]", priv->step, priv->type == rotary ? "M/rev" : "nm"); DebugP_log("\r\n\nPropagation delay: %dns", gEndat_prop_delay[priv->channel]); DebugP_log("\n\n\n"); } void endat_main(void *args) { int32_t i; struct cmd_supplement cmd_supplement; uint64_t icssgclk; void *pruss_cfg; /* Open drivers to open the UART driver for console */ Drivers_open(); Board_driversOpen(); /*C16 pin High for Enabling ch0 in booster pack */ #if (CONFIG_ENDAT0_BOOSTER_PACK) GPIO_setDirMode(ENC1_EN_BASE_ADDR, ENC1_EN_PIN, ENC1_EN_DIR); GPIO_pinWriteHigh(ENC1_EN_BASE_ADDR, ENC1_EN_PIN); #endif i = endat_get_fw_version(); DebugP_log("\n\n\n"); DebugP_log("EnDat firmware \t: %x.%x.%x (%s)\n\n", (i >> 24) & 0x7F, (i >> 16) & 0xFF, i & 0xFFFF, i & (1 << 31) ? "internal" : "release"); gEndat_is_multi_ch = CONFIG_ENDAT0_MODE & 1; gEndat_is_load_share_mode = CONFIG_ENDAT0_MODE & 2; endat_pre_init(); if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { gEndat_multi_ch_mask=(CONFIG_ENDAT0_CHANNEL0<<0|CONFIG_ENDAT0_CHANNEL1<<1|CONFIG_ENDAT0_CHANNEL2<<2); DebugP_log("\r\nchannels %s %s %s selected\n", gEndat_multi_ch_mask & ENDAT_MULTI_CH0 ? "0" : "", gEndat_multi_ch_mask & ENDAT_MULTI_CH1 ? "1" : "", gEndat_multi_ch_mask & ENDAT_MULTI_CH2 ? "2" : ""); if(!gEndat_multi_ch_mask) { DebugP_log("\r\nERROR: please select channels to be used in multi channel configuration -\n\n"); DebugP_log("\rexit %s as no channel selected in multichannel configuration\n", __func__); return; } } else { i = CONFIG_ENDAT0_CHANNEL0 & 0; i += CONFIG_ENDAT0_CHANNEL1; i += CONFIG_ENDAT0_CHANNEL2<<1; if(i < 0 || i > 2) { DebugP_log("\r\nWARNING: invalid channel selected, defaulting to Channel 0\n"); i = 0; } } DebugP_log("\r\n\n"); pruss_cfg = (void *)(((PRUICSS_HwAttrs *)(gPruIcssXHandle->hwAttrs))->cfgRegBase); #if PRU_ICSSGx_PRU_SLICE priv = endat_init((struct endat_pruss_xchg *)((PRUICSS_HwAttrs *)( gPruIcssXHandle->hwAttrs))->pru1DramBase, pruss_cfg, PRUICSS_SLICEx); #else priv = endat_init((struct endat_pruss_xchg *)((PRUICSS_HwAttrs *)( gPruIcssXHandle->hwAttrs))->pru0DramBase, pruss_cfg, PRUICSS_SLICEx); #endif if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { endat_config_multi_channel_mask(priv, gEndat_multi_ch_mask, gEndat_is_load_share_mode); } else { endat_config_channel(priv, i); } endat_config_host_trigger(priv); /* Read the ICSSG configured clock frequency. */ if(gPruIcssXHandle->hwAttrs->instance) { SOC_moduleGetClockFrequency(TISCI_DEV_PRU_ICSSG1, TISCI_DEV_PRU_ICSSG1_CORE_CLK, &icssgclk); } else { SOC_moduleGetClockFrequency(TISCI_DEV_PRU_ICSSG0, TISCI_DEV_PRU_ICSSG0_CORE_CLK, &icssgclk); } /* Configure Delays based on the ICSSG frequency*/ /* Count = ((required delay * icssgclk)/1000) */ priv->pruss_xchg->endat_delay_125ns = ((icssgclk*125)/1000000000); priv->pruss_xchg->endat_delay_51us = ((icssgclk*51)/1000000 ); priv->pruss_xchg->endat_delay_5us = ((icssgclk*5)/1000000); priv->pruss_xchg->endat_delay_1ms = ((icssgclk/1000) * 1); priv->pruss_xchg->endat_delay_2ms = ((icssgclk/1000) * 2); priv->pruss_xchg->endat_delay_12ms = ((icssgclk/1000) * 12); priv->pruss_xchg->endat_delay_50ms = ((icssgclk/1000) * 50); priv->pruss_xchg->endat_delay_380ms = ((icssgclk/1000) * 380); priv->pruss_xchg->endat_delay_900ms = ((icssgclk/1000) * 900); priv->pruss_xchg->icssg_clk = icssgclk; i = endat_pruss_load_run_fw(priv); if(i < 0) { DebugP_log("\rERROR: EnDat initialization failed -\n\n"); if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { uint8_t tmp; tmp = endat_multi_channel_detected(priv) & gEndat_multi_ch_mask; tmp ^= gEndat_multi_ch_mask; DebugP_log("\r\tunable to detect encoder in channel %s %s %s\n", tmp & ENDAT_MULTI_CH0 ? "0" : "", tmp & ENDAT_MULTI_CH1 ? "1" : "", tmp & ENDAT_MULTI_CH2 ? "2" : ""); } else { DebugP_log("\r\tcheck whether encoder is connected and ensure proper connections\n"); } DebugP_log("\rexit %s due to failed firmware initialization\n", __func__); return; } /* read encoder info at low frequency so that cable length won't affect */ cmd_supplement.frequency = 200 * 1000; endat_process_host_command(100, &cmd_supplement, priv); if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; for(j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { endat_multi_channel_set_cur(priv, j); if(endat_get_encoder_info(priv) < 0) { DebugP_log("\rEnDat initialization channel %d failed\n", j); DebugP_log("\rexit %s due to failed initialization\n", __func__); return; } /*convert cnt to time in ns ((cnt*1000000000)/icssgclk) before use*/ gEndat_prop_delay[priv->channel] = endat_get_prop_delay(priv)*((float)(1000000000)/icssgclk); DebugP_log("\n\t\t\t\tCHANNEL %d\n\n", j); endat_print_encoder_info(priv); } } gEndat_prop_delay_max = gEndat_prop_delay[0] > gEndat_prop_delay[1] ? gEndat_prop_delay[0] : gEndat_prop_delay[1]; gEndat_prop_delay_max = gEndat_prop_delay_max > gEndat_prop_delay[2] ? gEndat_prop_delay_max : gEndat_prop_delay[2]; } else { if(endat_get_encoder_info(priv) < 0) { DebugP_log("\rEnDat initialization failed\n"); DebugP_log("\rexit %s due to failed initialization\n", __func__); return; } /*convert cnt to time in ns ((cnt*1000000000)/icssgclk) before use*/ gEndat_prop_delay[priv->channel] = endat_get_prop_delay(priv)*((float)(1000000000)/icssgclk); endat_print_encoder_info(priv); } /* default frequency - 8MHz for 2.2 encoders, 1MHz for 2.1 encoders */ if(priv->cmd_set_2_2) { cmd_supplement.frequency = 8 * 1000 * 1000; } else { cmd_supplement.frequency = 1 * 1000 * 1000; } endat_process_host_command(100, &cmd_supplement, priv); while(1) { int32_t cmd; cmd = endat_handle_user(&cmd_supplement); if(cmd < 0) { continue; } if(VALID_HOST_CMD(cmd)) { endat_process_host_command(cmd, &cmd_supplement, priv); DebugP_log("\r|\n\r|\n"); continue; } if(endat_command_process(priv, cmd, &cmd_supplement) < 0) { continue; } if(gEndat_is_multi_ch || gEndat_is_load_share_mode) { int32_t j; DebugP_log("\r|\n"); for(j = 0; j < 3; j++) { if(gEndat_multi_ch_mask & 1 << j) { endat_multi_channel_set_cur(priv, j); DebugP_log("\r|\n|\t\t\t\tCHANNEL %d\n", j); endat_handle_rx(priv, cmd); } } } else { endat_handle_rx(priv, cmd); } /* this cannot be done except as last in loop; additional info becomes applicable from next command onwards only */ endat_addinfo_track(priv, cmd, &cmd_supplement); } Board_driversClose(); Drivers_close(); }