Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F79133657
magnetic_encoder.c
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Sat, Aug 24, 22:23
Size
10 KB
Mime Type
text/x-c
Expires
Mon, Aug 26, 22:23 (2 d)
Engine
blob
Format
Raw Data
Handle
20153057
Attached To
R6619 Oncilla Motordriver Firmware
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
=
0
b111
;
//Select a secondary prescaler of 1:1
SPI1CON1bits
.
PPRE
=
0
b00
;
//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
=
0
b00
;
// Register indirect with posincrement
DMA2CONbits
.
MODE
=
0
b01
;
// 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
);
uint8_t
mes1_reg
=
MDV_ME_Q2
;
uint8_t
mes2_reg
=
MDV_ME_Q3
;
if
(
sbcp_reg_table
[
MDV_Q2_Q3_CALIBRATION
].
u
&
0x8000
)
{
mes1_reg
=
MDV_ME_Q3
;
mes2_reg
=
MDV_ME_Q2
;
}
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
,
mes1_reg
);
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
,
mes2_reg
);
//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
magnetic_encoder_swap_2_and_3
(
int
swapped
)
{
if
(
swapped
)
{
me2
=
&
(
mes
[
2
]);
me3
=
&
(
mes
[
1
]);
}
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
);
}
void
me_load_persistent_sbcp_settings
(){
if
(
sbcp_reg_table
[
MDV_Q2_Q3_CALIBRATION
].
u
&
0x8000
)
{
me2
=
&
(
mes
[
2
]);
me3
=
&
(
mes
[
1
]);
}
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
);
}
Event Timeline
Log In to Comment