330 lines
8.7 KiB
C
330 lines
8.7 KiB
C
/*
|
|
* Licensed under the GNU General Public License version 2 with exceptions. See
|
|
* LICENSE file in the project root for full license information
|
|
*/
|
|
|
|
/** \file
|
|
* \brief
|
|
* ESC hardware layer functions.
|
|
*
|
|
* Function to read and write commands to the ESC. Used to read/write ESC
|
|
* registers and memory.
|
|
*/
|
|
#include <string.h>
|
|
#include "cc.h"
|
|
#include "esc_hw.h"
|
|
#include "esc_eep.h"
|
|
#include "options.h"
|
|
#include "ecat_slv.h"
|
|
#include "tiescbsp.h"
|
|
#include "tieschw.h"
|
|
|
|
extern PRUICSS_Handle pruIcss1Handle;
|
|
extern uint32_t pd_read_addr_err, pd_write_addr_err;
|
|
extern uint32_t pdi_read_fail_cnt, pdi_write_fail_cnt;
|
|
|
|
static PRUICSS_Handle escHwPruIcssHandle;
|
|
|
|
int bRunApplication;
|
|
|
|
static void Esc_readmbx(uint8_t *pData, uint16_t Address, uint16_t Len)
|
|
{
|
|
bsp_pdi_mbx_read_start(escHwPruIcssHandle);
|
|
bsp_read(escHwPruIcssHandle, pData, Address, Len);
|
|
|
|
if(Len >= MBX0_sml - 2)
|
|
{
|
|
bsp_pdi_mbx_read_complete(escHwPruIcssHandle);
|
|
}
|
|
}
|
|
|
|
static void ESC_readRXPDO(uint8_t *pData, uint16_t Address, uint16_t Len)
|
|
{
|
|
int16_t sm_index;
|
|
uint16_t ActAddress = bsp_get_process_data_address(escHwPruIcssHandle, Address, Len,
|
|
&sm_index);
|
|
|
|
if(ActAddress < ESC_ADDR_MEMORY)
|
|
{
|
|
pd_read_addr_err++;
|
|
return;
|
|
}
|
|
|
|
bsp_read(escHwPruIcssHandle, pData, ActAddress, Len);
|
|
bsp_process_data_access_complete(escHwPruIcssHandle, Address, Len, sm_index);
|
|
}
|
|
|
|
static void ESC_writembox(uint8_t *pData, uint16_t Address, uint16_t Len)
|
|
{
|
|
|
|
//Do not write to mailbox if already full
|
|
if((bsp_read_byte(escHwPruIcssHandle,
|
|
ESC_ADDR_SM1_STATUS) & SM_STATUS_MBX_FULL))
|
|
{
|
|
return;
|
|
}
|
|
|
|
bsp_pdi_mbx_write_start(escHwPruIcssHandle);
|
|
bsp_write(escHwPruIcssHandle, pData, Address, Len);
|
|
|
|
if(Len >= MBX1_sml - 2)
|
|
{
|
|
bsp_pdi_mbx_write_complete(escHwPruIcssHandle);
|
|
}
|
|
}
|
|
|
|
static void ESC_writeTXPDO(uint8_t *pData, uint16_t Address, uint16_t Len)
|
|
{
|
|
int16_t sm_index;
|
|
uint16_t ActualAddr = bsp_get_process_data_address(escHwPruIcssHandle, Address, Len,
|
|
&sm_index);
|
|
|
|
if(ActualAddr < ESC_ADDR_MEMORY)
|
|
{
|
|
pd_write_addr_err++;
|
|
return;
|
|
}
|
|
|
|
bsp_write(escHwPruIcssHandle, pData, ActualAddr, Len);
|
|
bsp_process_data_access_complete(escHwPruIcssHandle, Address, Len, sm_index);
|
|
}
|
|
|
|
|
|
/** ESC read function used by the Slave stack.
|
|
*
|
|
* @param[in] address = address of ESC register to read
|
|
* @param[out] buf = pointer to buffer to read in
|
|
* @param[in] len = number of bytes to read
|
|
*/
|
|
void ESC_read (uint16_t address, void *buf, uint16_t len)
|
|
{
|
|
uint16_t alevent;
|
|
|
|
switch(address)
|
|
{
|
|
case MBX0_sma:
|
|
/* TODO *//*case MBX0_sma_b:*/
|
|
{
|
|
Esc_readmbx(buf, address, len);
|
|
break;
|
|
}
|
|
case SM2_sma:
|
|
{
|
|
ESC_readRXPDO(buf, address, len);
|
|
break;
|
|
}
|
|
case ESCREG_SM0:
|
|
case ESCREG_SM1:
|
|
case ESCREG_SM2:
|
|
case ESCREG_SM3:
|
|
{
|
|
bsp_read(escHwPruIcssHandle, buf, address, len);
|
|
/* Handle special case when SOES batch read SM settings,
|
|
* indicate that SM ACTIVATE is read
|
|
*/
|
|
if(len > 5)
|
|
{
|
|
uint8_t n = (address - ESC_ADDR_SYNCMAN) >> 3;
|
|
bsp_pdi_post_read_indication(escHwPruIcssHandle, ESCREG_SM0ACTIVATE + (n << 3));
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
switch(len)
|
|
{
|
|
case 1:
|
|
{
|
|
uint8_t *p = buf;
|
|
*p = bsp_read_byte(escHwPruIcssHandle, address);
|
|
break;
|
|
}
|
|
case 2:
|
|
{
|
|
uint16_t *p = buf;
|
|
*p = bsp_read_word(escHwPruIcssHandle, address);
|
|
break;
|
|
}
|
|
case 4:
|
|
{
|
|
uint32_t *p = buf;
|
|
*p = bsp_read_dword(escHwPruIcssHandle, address);
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
bsp_read(escHwPruIcssHandle, buf, address, len);
|
|
bsp_pdi_post_read_indication(escHwPruIcssHandle, address);
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
alevent = bsp_read_word_isr(escHwPruIcssHandle, ESCREG_ALEVENT);
|
|
CC_ATOMIC_SET(ESCvar.ALevent, (etohs(alevent)));
|
|
}
|
|
|
|
/** ESC write function used by the Slave stack.
|
|
*
|
|
* @param[in] address = address of ESC register to write
|
|
* @param[out] buf = pointer to buffer to write from
|
|
* @param[in] len = number of bytes to write
|
|
*/
|
|
void ESC_write (uint16_t address, void *buf, uint16_t len)
|
|
{
|
|
uint16_t alevent;
|
|
|
|
switch(address)
|
|
{
|
|
case MBX1_sma:
|
|
/* TODO *//*case MBX1_sma_b:*/
|
|
{
|
|
if(len != 1)
|
|
{
|
|
ESC_writembox(buf, address, len);
|
|
}
|
|
else
|
|
{
|
|
bsp_pdi_mbx_write_start(escHwPruIcssHandle);
|
|
}
|
|
break;
|
|
}
|
|
case (MBX1_sma + MBX1_sml - 1):
|
|
{
|
|
/* Handle SM end byte is written */
|
|
uint8_t * p = buf;
|
|
bsp_write_byte(escHwPruIcssHandle, *p, address);
|
|
bsp_pdi_mbx_write_complete(escHwPruIcssHandle);
|
|
break;
|
|
}
|
|
case SM3_sma:
|
|
{
|
|
ESC_writeTXPDO(buf, address, len);
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
switch(len)
|
|
{
|
|
case 1:
|
|
{
|
|
uint8_t * p = buf;
|
|
bsp_write_byte(escHwPruIcssHandle, *p, address);
|
|
bsp_pdi_write_indication(escHwPruIcssHandle, address, *p);
|
|
break;
|
|
}
|
|
case 2:
|
|
{
|
|
uint16_t * p = buf;
|
|
bsp_write_word(escHwPruIcssHandle, *p, address);
|
|
bsp_pdi_write_indication(escHwPruIcssHandle, address, *p);
|
|
break;
|
|
}
|
|
case 4:
|
|
{
|
|
uint32_t * p = buf;
|
|
bsp_write_dword(escHwPruIcssHandle, *p, address);
|
|
bsp_pdi_write_indication(escHwPruIcssHandle, address, *p);
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
bsp_write(escHwPruIcssHandle, buf, address, len);
|
|
bsp_pdi_write_indication(escHwPruIcssHandle, address, 0);
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
}
|
|
alevent = bsp_read_word_isr(escHwPruIcssHandle, ESCREG_ALEVENT);
|
|
CC_ATOMIC_SET(ESCvar.ALevent, (etohs(alevent)));
|
|
}
|
|
|
|
/** ESC interrupt enable function by the Slave stack in IRQ mode.
|
|
*
|
|
* @param[in] mask = of interrupts to enable
|
|
*/
|
|
void ESC_interrupt_enable (uint32_t mask)
|
|
{
|
|
uint32_t readmask;
|
|
readmask = bsp_read_dword_isr(escHwPruIcssHandle,ESCREG_ALEVENTMASK);
|
|
bsp_write_dword(escHwPruIcssHandle, (mask | readmask), ESCREG_ALEVENTMASK);
|
|
}
|
|
|
|
/** ESC interrupt disable function by the Slave stack in IRQ mode.
|
|
*
|
|
* @param[in] mask = interrupts to disable
|
|
*/
|
|
void ESC_interrupt_disable (uint32_t mask)
|
|
{
|
|
uint32_t readmask;
|
|
readmask = bsp_read_dword_isr(escHwPruIcssHandle,ESCREG_ALEVENTMASK);
|
|
bsp_write_dword(escHwPruIcssHandle, (~mask & readmask), ESCREG_ALEVENTMASK);
|
|
}
|
|
|
|
/** ESC emulated EEPROM handler
|
|
*/
|
|
void ESC_eep_handler(void)
|
|
{
|
|
EEP_process ();
|
|
EEP_hw_process();
|
|
}
|
|
|
|
/** SYNC0 ISR handler
|
|
*
|
|
* @param[in] arg = NOT USED
|
|
*/
|
|
void Sync0_Isr (void)
|
|
{
|
|
}
|
|
|
|
/** SYNC1 ISR handler
|
|
*
|
|
* @param[in] arg = NOT USED
|
|
*/
|
|
void Sync1_Isr (void)
|
|
{
|
|
}
|
|
|
|
/** PDI ISR handler
|
|
*
|
|
* @param[in] arg = NOT USED
|
|
*/
|
|
void PDI_Isr(void)
|
|
{
|
|
uint16_t alevent;
|
|
|
|
alevent = bsp_read_word_isr(escHwPruIcssHandle,ESCREG_ALEVENT);
|
|
CC_ATOMIC_SET(ESCvar.ALevent, etohs(alevent));
|
|
|
|
if(ESCvar.ALevent & ESCREG_ALEVENT_SM2)
|
|
{
|
|
DIG_process(DIG_PROCESS_OUTPUTS_FLAG |
|
|
DIG_PROCESS_APP_HOOK_FLAG |
|
|
DIG_PROCESS_INPUTS_FLAG);
|
|
}
|
|
}
|
|
|
|
|
|
/** ESC and CPU related HW init
|
|
*
|
|
* @param[in] arg = esc_cfg provided by the application
|
|
*/
|
|
void ESC_init (const esc_cfg_t * config)
|
|
{
|
|
escHwPruIcssHandle = pruIcss1Handle;
|
|
bsp_set_sm_properties(escHwPruIcssHandle, 0, MBX0_sma, MBX0_sml);
|
|
bsp_set_sm_properties(escHwPruIcssHandle, 1, MBX1_sma, MBX1_sml);
|
|
bsp_set_sm_properties(escHwPruIcssHandle, 2, SM2_sma, MAX_RXPDO_SIZE * 3);
|
|
bsp_set_sm_properties(escHwPruIcssHandle, 3, SM3_sma, MAX_TXPDO_SIZE * 3);
|
|
|
|
bsp_write_dword(escHwPruIcssHandle, 0 , ESCREG_ALEVENTMASK);
|
|
}
|
|
|
|
|
|
|