Homec4science
Diffusion Oncilla Motordriver Firmware (load-bad-commit) Browse

oncilla-motordriver-fw/src/magnetic_encoder.c
edc3f097732eload-bad-commit

Last Change
Raw File

magnetic_encoder.c
View Options

#include "magnetic_encoder.h"
#include "sbcp_mdv.h"
#include "misc_math.h"
#define timer_2_start() do { T2CONbits.TON = 1; }while(0)
#define timer_2_stop() do { T2CONbits.TON = 0; }while(0)
#define timer_2_stop_and_clear() do{ \
timer_2_stop(); \
TMR2 = 0x00; \
}while(0)
struct magnetic_encoder {
int position;
me_error_flags flags;
int mult;
int div;
};
magnetic_encoder mes[3];
magnetic_encoder * me1 = &(mes[0]);
magnetic_encoder * me2 = &(mes[1]);
magnetic_encoder * me3 = &(mes[2]);
/**
* Th state machine states for Magnetic encoders reception
*/
typedef enum DMA_SPI_ME_rx_state {
DMA_SPI_ME_IDLE, ///< No reading is beeing performed.
DMA_SPI_ME_WAITING_FOR_DEVICES, ///< Reading is started, we send the start order to device, we putting a latency for them to read their data.
DMA_SPI_ME_READING_DEVICES, ///< We are actually reading the data from teh devices
DMA_SPI_ME_DATA_READY_TO_BE_PROCESSED ///< We have finish to read the data, and we need to process it to extract atual data in the spi_main function
}DMA_SPI_ME_rx_state;
struct DMA_SPI_ME_data {
gpio spi_cs;
DMA_SPI_ME_rx_state state;
};
struct DMA_SPI_ME_data dma_spi_me;
typedef union DMA_SPI_ME_rx_buffer{
struct{
//First word definition little endian representation
unsigned Q1_LIN : 1; ///< their is a linearity read alarm, data invalid
unsigned Q1_COF : 1; ///< their is CORDIC Overflow, data invalid
unsigned Q1_OCF : 1; ///< Algorithm is finished. If zero, data invalid
unsigned Q1_ENC_DATA_LOW : 8; ///< first 8 bits of data
unsigned Q1_ENC_DATA_HIGH : 4; ///< last 4 bits of data
unsigned Q1_DUMMY_BIT : 1; ///< unimplemented
//------ 1 word size
//second word definition, little endian representation
unsigned Q2_ENC_DATA_LOW : 8; ///< first 8 bits of data
unsigned Q2_ENC_DATA_HIGH : 4; ///< last 4 bits of data
unsigned Q2_DUMMY_BIT : 1; ///< unimplemented
unsigned Q1_EVEN_PAR : 1; ///< even parity bit
unsigned Q1_MAG_DEC : 1; ///< Maginute amplitude decreased.
unsigned Q1_MAG_INC : 1; ///< Maginute amplitude increased.
//------ 1 word size
//third word little endian representation
unsigned Q3_ENC_DATA_MID : 5; ///<central 5 bits of data
unsigned Q3_ENC_DATA_HIGH : 4; ///< last 4 bits of data
unsigned Q3_DUMMY_BIT : 1; ///< unimplemented
//19 bits for the first knee encoder.
unsigned Q2_EVEN_PAR : 1; ///< even parity bit
unsigned Q2_MAG_DEC : 1; ///< Maginute amplitude decreased.
unsigned Q2_MAG_INC : 1; ///< Maginute amplitude increased.
unsigned Q2_LIN : 1; ///< their is a linearity read alarm, data invalid
unsigned Q2_COF : 1; ///< their is CORDIC Overflow, data invalid
unsigned Q2_OCF : 1; ///< Algorithm is finished. If zero, data invalid
//------ 1word size
// 4th and last word, little endian representation
//trailing 7 bits that should be ignored
unsigned DUMMY_BITS : 7; ///< unimplemented
//19 bits for the 2nd knee encoder.
unsigned Q3_EVEN_PAR : 1; ///< even parity bit
unsigned Q3_MAG_DEC : 1; ///< Maginute amplitude decreased.
unsigned Q3_MAG_INC : 1; ///< Maginute amplitude increased.
unsigned Q3_LIN : 1; ///< their is a linearity read alarm, data invalid
unsigned Q3_COF : 1; ///< their is CORDIC Overflow, data invalid
unsigned Q3_OCF : 1; ///< Algorithm is finished. If zero, data invalid
unsigned Q3_ENC_DATA_LOW : 3; ///< first 3 bits of data
//------ 1word size
} b; ///< bits representation
unsigned char B[8];///<byte representation
unsigned int w[4];///<word representation
} DMA_SPI_ME_rx_buffer;
DMA_SPI_ME_rx_buffer dma_spi_me_rx_buffer __attribute__((space(dma)));
#define dma_spi_me_start() do{ \
gpio_clear(dma_spi_me.spi_cs); \
}while(0)
#define dma_spi_me_stop() do{ \
gpio_set(dma_spi_me.spi_cs); \
}while(0)
#define dma_spi_me_start_actual_reading() do{ \
SPI1BUF = 0x0000; /*force the transfer of dma channel */ \
dma_spi_me.state = DMA_SPI_ME_READING_DEVICES; \
}while(0)
void init_magnetic_encoder(magnetic_encoder * e){
e->position = 0;
e->flags = ME_F_ONBOARD_PROCESSING_UNFINISHED;
e->mult = 1;
e->div = 1;
}
void init_magnetic_encoders(gpio spi_cs){
dma_spi_me.spi_cs = spi_cs;
dma_spi_me_stop();
dma_spi_me.state = DMA_SPI_ME_IDLE;
//SPI 1 is for magentic encoders :
IEC0bits.SPI1IE = 0; //disable interrupt. This is needed because we use DMA
IFS0bits.SPI1IF = 0; //clear the interrupt flag.
SPI1CON1bits.DISSCK = 0; //Us the clock
SPI1CON1bits.DISSDO = 1; //Disable data output. This is not needed by the application, no pin are routed
SPI1CON1bits.MODE16 = 1; //Transmit word. Rx buffer is in byte, but we don't care because we are transmitting an even number of byte
SPI1CON1bits.SMP = 0; //data is sampled at middle of clock time
SPI1CON1bits.CKE = 1; //clock cycle start when going from active to idle clock state.
SPI1CON1bits.CKP = 1; //clock idle state is at high level
SPI1CON1bits.MSTEN = 1; //enable master mode
//Choose a clock frequency of 625 kHz, max accepted by AS5045 is 1Mhz
SPI1CON1bits.SPRE = 0b111; //Select a secondary prescaler of 1:1
SPI1CON1bits.PPRE = 0b00 ; //select a primary prescaler of 64:1
SPI1CON2bits.FRMEN = 0; //No frame support
SPI1STATbits.SPIEN = 1; //Enable SPI 1 module
DMA2CONbits.CHEN = 0;
DMA2CONbits.SIZE = 0;
DMA2CONbits.DIR = 0;
DMA2CONbits.HALF = 0;
DMA2CONbits.NULLW = 1;
DMA2CONbits.AMODE = 0b00;// Register indirect with posincrement
DMA2CONbits.MODE = 0b01;// One - shot, No Ping Pong
DMA2STA = __builtin_dmaoffset(dma_spi_me_rx_buffer.w);
DMA2PAD = (volatile unsigned int) & SPI1BUF;
DMA2REQ = 0x000A;
DMA2CNT = 3;
IFS1bits.DMA2IF = 0;
IEC1bits.DMA2IE = 1;
init_magnetic_encoder(&(mes[0]));
init_magnetic_encoder(&(mes[1]));
init_magnetic_encoder(&(mes[2]));
}
#define fill_status(me,ocf_bit,cof_bit,lin_bit,amp_incr_bit,amp_decr_bit) do{ \
me.flags = ME_F_OK; \
if(!(ocf_bit)){ \
me.flags |= ME_F_ONBOARD_PROCESSING_UNFINISHED; \
} \
if(cof_bit){ \
me.flags |= ME_F_CORDIC_OVERFLOW; \
} \
if(lin_bit){ \
me.flags |= ME_F_LINEARITY_READ_ALARM; \
} \
if( (amp_incr_bit) && (amp_decr_bit) ) { \
me.flags |= ME_F_MAGNETIC_AMPLITUDE_ERROR; \
} \
}while(0)
void magnetic_encoders_start_reading(){
if(dma_spi_me.state == DMA_SPI_ME_IDLE){
dma_spi_me.state = DMA_SPI_ME_WAITING_FOR_DEVICES;
DMA2CONbits.CHEN = 1;
dma_spi_me_start();
timer_2_start();
return;
}
}
#define set_me_value(me,value,adr) do{ \
if((me).flags == ME_F_OK){ \
unsigned int _TMP_val = value; \
if((me).mult < 0){ \
_TMP_val = 4096 - _TMP_val; \
} \
(me).position = (long) _TMP_val * abs((me).mult) / (me).div; \
(me).position &= 0x3fff; \
} \
sbcp_me_reg(adr,MEX_POSITION).u = (me).position; \
if((me).flags & (ME_F_LINEARITY_READ_ALARM | ME_F_CORDIC_OVERFLOW | ME_F_ONBOARD_PROCESSING_UNFINISHED)) { \
sbcp_me_reg(adr,MEX_POSITION).u |= 1 << 15; \
} \
if((me).flags & ME_F_MAGNETIC_AMPLITUDE_ERROR){ \
sbcp_me_reg(adr,MEX_POSITION).u |= 1 << 14; \
} \
}while(0)
void magnetic_encoders_process(){
if(dma_spi_me.state == DMA_SPI_ME_DATA_READY_TO_BE_PROCESSED){
//we assume that the parity is always correct.
/// \todo : check the parity of the data, buts it's rather complicated.
//first we check the status
fill_status(mes[0],
dma_spi_me_rx_buffer.b.Q1_OCF,
dma_spi_me_rx_buffer.b.Q1_COF,
dma_spi_me_rx_buffer.b.Q1_LIN,
dma_spi_me_rx_buffer.b.Q1_MAG_INC,
dma_spi_me_rx_buffer.b.Q1_MAG_DEC);
fill_status(mes[1],
dma_spi_me_rx_buffer.b.Q2_OCF,
dma_spi_me_rx_buffer.b.Q2_COF,
dma_spi_me_rx_buffer.b.Q2_LIN,
dma_spi_me_rx_buffer.b.Q2_MAG_INC,
dma_spi_me_rx_buffer.b.Q2_MAG_DEC);
fill_status(mes[2],
dma_spi_me_rx_buffer.b.Q3_OCF,
dma_spi_me_rx_buffer.b.Q3_COF,
dma_spi_me_rx_buffer.b.Q3_LIN,
dma_spi_me_rx_buffer.b.Q3_MAG_INC,
dma_spi_me_rx_buffer.b.Q3_MAG_DEC);
set_me_value(mes[0], (dma_spi_me_rx_buffer.b.Q1_ENC_DATA_HIGH << 8 ) + dma_spi_me_rx_buffer.b.Q1_ENC_DATA_LOW , MDV_ME_Q1);
set_me_value(mes[1], (dma_spi_me_rx_buffer.b.Q2_ENC_DATA_HIGH << 8 ) + dma_spi_me_rx_buffer.b.Q2_ENC_DATA_LOW , MDV_ME_Q2);
set_me_value(mes[2], (dma_spi_me_rx_buffer.b.Q3_ENC_DATA_HIGH << 8 ) + ( dma_spi_me_rx_buffer.b.Q3_ENC_DATA_MID << 3 ) + dma_spi_me_rx_buffer.b.Q3_ENC_DATA_LOW , MDV_ME_Q3);
//mark that new data is available in the table.
sbcp_mark_new_low_latency_data_available();
//we have readen the data, nothing more to do
dma_spi_me.state = DMA_SPI_ME_IDLE;
}
}
/*******************************************************************************
*
* Interruption
*
******************************************************************************/
/**
* Interruption when the SPI 1 finished its reading
*/
void __attribute__((__interrupt__ , no_auto_psv)) _DMA2Interrupt(){
dma_spi_me.state = DMA_SPI_ME_DATA_READY_TO_BE_PROCESSED;
dma_spi_me_stop();
//clear the interruption flag
IFS1bits.DMA2IF = 0;
}
/* Timer 2 is used to create the delay to communicate over the SPI1 bus */
void __attribute__((__interrupt__, no_auto_psv)) _T2Interrupt(void){
/* Interrupt Service Routine code goes here */
timer_2_stop_and_clear();
dma_spi_me_start_actual_reading();
IFS0bits.T2IF = 0; // Clear Timer 2 Interrupt Flag
}
void me_set_gear_ratio(magnetic_encoder * e, int mult, int div){
if(div == 0) {
div = 1;
}
e->mult = mult;
e->div = div;
}
int me_get_gear_mult(magnetic_encoder * e){
return e->mult;
}
int me_get_gear_div(magnetic_encoder * e){
return e->div;
}
int me_get_value(magnetic_encoder * e){
return e->position;
}
me_error_flags me_get_error(magnetic_encoder * e){
return e->flags;
}
void me_load_persistent_sbcp_settings(){
me_set_gear_ratio(me1,
sbcp_me_reg(MDV_ME_Q1,MEX_GEAR_MULT).i,
sbcp_me_reg(MDV_ME_Q1,MEX_GEAR_DIV).i);
me_set_gear_ratio(me2,
sbcp_me_reg(MDV_ME_Q2,MEX_GEAR_MULT).i,
sbcp_me_reg(MDV_ME_Q2,MEX_GEAR_DIV).i);
me_set_gear_ratio(me3,
sbcp_me_reg(MDV_ME_Q3,MEX_GEAR_MULT).i,
sbcp_me_reg(MDV_ME_Q3,MEX_GEAR_DIV).i);
}
c4science · Help