External/motor-control-sdk
8
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
aa5b15313d
motor-control-sdk/examples/position_sense/endat_diagnostic/endat_diagnostic.c
Achala Ram 394ada13f3 am64x/am243x: EnDat: Resolve channel mask checks 
- Resolve mask check conditions for periodic mode testing

Fixes: PINDSW-6569

Signed-off-by: Achala Ram <a-ram@ti.com>
2023-08-30 20:47:32 +05:30

2261 lines
72 KiB
C
Raw Blame History

/*
* 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 <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdbool.h>
#include <kernel/dpl/DebugP.h>
#include <drivers/soc.h>
#include <drivers/sciclient.h>
#include <kernel/dpl/TaskP.h>
#include <drivers/pinmux.h>
#include <drivers/hw_include/hw_types.h>
#include "ti_drivers_open_close.h"
#include "ti_board_open_close.h"
#include <position_sense/endat/include/endat_drv.h>
#include "endat_periodic_trigger.h"
#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 <position_sense/endat/firmware/endat_master_multi_bin.h>
#endif
#if (CONFIG_ENDAT0_CHANNEL0) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU)
#include <position_sense/endat/firmware/endat_master_multi_RTU_bin.h>
#endif
#if (CONFIG_ENDAT0_CHANNEL1) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU)
#include <position_sense/endat/firmware/endat_master_multi_PRU_bin.h>
#endif
#if (CONFIG_ENDAT0_CHANNEL2) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU)
#include <position_sense/endat/firmware/endat_master_multi_TXPRU_bin.h>
#endif
#if CONFIG_ENDAT0_MODE == ENDAT_MODE_SINGLE_CHANNEL_SINGLE_PRU
#include <position_sense/endat/firmware/endat_master_bin.h>
#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_PERIODIC_CMD(x) ((x) == 200)
#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)||((x) == 200))
#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_0,
sizeof(EnDatFirmwareMultiMakeRTU_0));
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_0,
sizeof(EnDatFirmwareMultiMakePRU_0));
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_0,
sizeof(EnDatFirmwareMultiMakeTXPRU_0));
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_0,
sizeof(EnDatFirmwareMulti_0));
#else
status = PRUICSS_writeMemory(gPruIcssXHandle, PRUICSS_IRAM_PRU(PRUICSS_SLICEx),
0, (uint32_t *) EnDatFirmware_0,
sizeof(EnDatFirmware_0));
#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_0 + 2);
#endif
#if (CONFIG_ENDAT0_CHANNEL0) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU)
return *((unsigned long *)EnDatFirmwareMultiMakeRTU_0 + 2);
#endif
#if (CONFIG_ENDAT0_CHANNEL1) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU)
return *((unsigned long *)EnDatFirmwareMultiMakePRU_0 + 2);
#endif
#if (CONFIG_ENDAT0_CHANNEL2) && (CONFIG_ENDAT0_MODE == ENDAT_MODE_MULTI_CHANNEL_MULTI_PRU)
return *((unsigned long *)EnDatFirmwareMultiMakeTXPRU_0 + 2);
#endif
#if CONFIG_ENDAT0_MODE == ENDAT_MODE_SINGLE_CHANNEL_SINGLE_PRU
return *((unsigned long *)EnDatFirmware_0 + 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");
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|200: Start periodic continuous mode |\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) || VALID_PERIODIC_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<<priv->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<<priv->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<<priv->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<<priv->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;
case 200:
if(gEndat_is_load_share_mode)
{
if(gEndat_multi_ch_mask & (1<<0))
{
DebugP_log("\r| Enter IEP cycle count for Channel0: \n");
if(DebugP_scanf("%u\n", &cmd_supplement->cmp3) < 0)
{
DebugP_log("\r| ERROR: invalid value\n|\n|\n|\n");
return -EINVAL;
}
}
if(gEndat_multi_ch_mask & (1<<1))
{
DebugP_log("\r| Enter IEP cycle count for Channel1: \n");
if(DebugP_scanf("%u\n", &cmd_supplement->cmp5) < 0)
{
DebugP_log("\r| ERROR: invalid value\n|\n|\n|\n");
return -EINVAL;
}
}
if(gEndat_multi_ch_mask & (1<<2))
{
DebugP_log("\r| Enter IEP cycle count for Channel2: \n");
if(DebugP_scanf("%u\n", &cmd_supplement->cmp6) < 0)
{
DebugP_log("\r| ERROR: invalid value\n|\n|\n|\n");
return -EINVAL;
}
}
}
else
{
DebugP_log("\r| Enter IEP cycle count: ");
if(DebugP_scanf("%u\n", &cmd_supplement->cmp3) < 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)
{
float ct = (priv->rx_en_cnt)/2; /*one endat clock cycle time = 1/endat frequency = 2*rx_en_cnt*/
/* if propagation delay is more than half clock cycle time (2/endat frequency) then we have to reduce clock cycles for rx*/
if(prop_delay > (ct/2))
{
uint16_t dis = round(prop_delay/ct);
endat_config_rx_arm_cnt(priv, prop_delay);
/* propagation delay/cycle_time */
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_periodic_command(int32_t cmd,
struct cmd_supplement *cmd_supplement, struct endat_priv *priv)
{
if(cmd == 200)
{
endat_config_periodic_trigger(priv);
int32_t status;
int32_t pos_cmd = 1;
DebugP_assert(endat_command_process(priv, pos_cmd, NULL) >= 0);
if(gEndat_is_load_share_mode)
{
priv->cmp3 = gEndat_multi_ch_mask & (1<<0) ? cmd_supplement->cmp3 : 0;
priv->cmp5 = gEndat_multi_ch_mask & (1<<1) ? cmd_supplement->cmp5 : 0;
priv->cmp6 = gEndat_multi_ch_mask & (2<<1) ? cmd_supplement->cmp6 : 0;
}
else
{
priv->cmp3 = cmd_supplement->cmp3;
}
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;
}
struct endat_periodic_interface endat_periodic_interface;
endat_periodic_interface.pruss_cfg = priv->pruss_cfg;
endat_periodic_interface.pruss_iep = priv->pruss_iep;
endat_periodic_interface.pruss_dmem = priv->pruss_xchg;
endat_periodic_interface.load_share = priv->load_share;
endat_periodic_interface.cmp3 = priv->cmp3;
endat_periodic_interface.cmp5 = priv->cmp5;
endat_periodic_interface.cmp6 = priv->cmp6;
status = endat_config_periodic_mode(&endat_periodic_interface, gPruIcssXHandle);
DebugP_assert(0 != status);
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_periodic_continuous_mode(&endat_periodic_interface);
endat_config_host_trigger(priv);
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);
endat_recvd_process(priv, 1, &gEndat_format_data_mtrctrl[j]);
i += endat_get_position_loop_chars(priv, 0, 0);
DebugP_log("| ");
DebugP_log("\r|\n\r| Channel: %10u\r\n| ", j);
endat_print_position_loop(priv, 1, 0, j);
DebugP_log(" ");
DebugP_log("\n");
i += 3;
}
}
}
else
{
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);
}
}
}
}
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(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
{
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);
endat_recvd_process(priv, 1, &gEndat_format_data_mtrctrl[j]);
i += endat_get_position_loop_chars(priv, 0, 0);
DebugP_log("| ");
DebugP_log("\r|\n\r| Channel: %10u\r\n| ", j);
endat_print_position_loop(priv, 1, 0, j);
DebugP_log(" ");
DebugP_log("\n");
i += 3;
}
}
}
else
{
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;
void *pruss_iep;
/* 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);
pruss_iep = (void *)(((PRUICSS_HwAttrs *)(gPruIcssXHandle->hwAttrs))->iep0RegBase);
#if PRU_ICSSGx_PRU_SLICE
priv = endat_init((struct endat_pruss_xchg *)((PRUICSS_HwAttrs *)(
gPruIcssXHandle->hwAttrs))->pru1DramBase, pruss_cfg, pruss_iep, PRUICSS_SLICEx);
#else
priv = endat_init((struct endat_pruss_xchg *)((PRUICSS_HwAttrs *)(
gPruIcssXHandle->hwAttrs))->pru0DramBase, pruss_cfg, pruss_iep, 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(VALID_PERIODIC_CMD(cmd))
{
endat_process_periodic_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();
}
Reference in New Issue View Git Blame Copy Permalink