Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-02-06 10:23:58 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

endat

Browse Source
This commit is contained in:
crinq
2019-07-01 22:33:18 +02:00
parent 3e9b209e91
commit d82f8e35af
3 changed files with 120 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
shared/endat.c
View File

@@ -4,14 +4,12 @@
uint32_t endat_tx(endat_cmd_t cmd, uint8_t p1, uint16_t p2, uint8_t* buf, endat_data_t* data){
uint32_t len = 0;
buf[0] = cmd;
buf[0] = flip8(cmd);
p1 = flip8(p1);
buf[1] = p1;
buf[1] = flip8(p1);
p2 = flip16(p2);
buf[2] = p2 & 0xff;
buf[3] = (p2 >> 8) & 0xff;
buf[2] = flip16(p2) & 0xff;
buf[3] = (flip16(p2) >> 8) & 0xff;
data->current_cmd = cmd;
data->current_addr = p1;
@@ -24,6 +22,7 @@ uint32_t endat_tx(endat_cmd_t cmd, uint8_t p1, uint16_t p2, uint8_t* buf, endat_
case ENDAT_SELECT_MEM:
len = 2 + 6 + 8 + 16;
data->current_mem = p1;
break;
case ENDAT_READ_ADDR:
@@ -128,6 +127,7 @@ uint32_t endat_rx(uint8_t* buf, uint32_t max_len, endat_data_t* data){
case ENDAT_SELECT_MEM:
p1 = (df.data16[0] >> 1) & 0xff;
data->crc = (df.data8[3] >> 1) & 0b11111;
p1 = flip8(p1);
//check crc
if(data->current_mem != p1){
@@ -140,14 +140,14 @@ uint32_t endat_rx(uint8_t* buf, uint32_t max_len, endat_data_t* data){
p1 = (df.data16[0] >> 1) & 0xff;
p2 = (df.data32[0] >> (1 + 8)) & 0xffff;
data->crc = (df.data8[3] >> 1) & 0b11111;
p1 = flip8(p1);
p2 = flip16(p2);
//check crc
if(addr != p1){
return(0);
}
p1 = flip8(p1);
p2 = flip16(p2);
switch(data->current_mem){
case ENDAT_MEM_STATE:
switch(p1){
@@ -168,12 +168,12 @@ uint32_t endat_rx(uint8_t* buf, uint32_t max_len, endat_data_t* data){
case ENDAT_MEM_PARAM0:
switch(p1){
case 13:
case ENDAT_ADDR_POS_LEN:
data->pos_len = p2;
data->pos_bits = data->pos_len - data->mpos_bits;
break;
case 14:
case ENDAT_ADDR_TYPE:
data->fb_type = p2;
break;
@@ -184,16 +184,16 @@ uint32_t endat_rx(uint8_t* buf, uint32_t max_len, endat_data_t* data){
case ENDAT_MEM_PARAM1:
switch(p1){
case 17 - 16:
case ENDAT_ADDR_MULTITURN:
data->mpos_bits = log2f(p2) + 0.99;
data->pos_bits = data->pos_len - data->mpos_bits;
break;
case 20 - 16:
case ENDAT_ADDR_RES_LOW:
data->pos_res = (data->pos_res & 0xffff0000) | p2;
break;
case 21 - 16:
case ENDAT_ADDR_RES_HIGH:
data->pos_res = (data->pos_res & 0xffff) | (p2 << 16);
break;
@@ -204,7 +204,7 @@ uint32_t endat_rx(uint8_t* buf, uint32_t max_len, endat_data_t* data){
case ENDAT_MEM_PARAM2:
switch(p1){
case 32 - 16 * 2:
case ENDAT_ADDR_MAX_VEL:
data->max_vel = p2;
break;
@@ -223,6 +223,8 @@ uint32_t endat_rx(uint8_t* buf, uint32_t max_len, endat_data_t* data){
p1 = (df.data16[0] >> 1) & 0xff;
p2 = (df.data32[0] >> (1 + 8)) & 0xffff;
data->crc = (df.data8[3] >> 1) & 0b11111;
p1 = flip8(p1);
p2 = flip16(p2);
//check crc
if(addr != p1){

42
shared/endat.h
View File

@@ -4,18 +4,18 @@
#define ENDAT_h
typedef enum{
ENDAT_READ_POS = 0b11100000,// 0b000111,
ENDAT_SELECT_MEM = 0b00011100,// 0b001110,
ENDAT_READ_ADDR = 0b00110001,// 0b100011,
ENDAT_WRITE_ADDR = 0b00001110,// 0b011100,
ENDAT_RESET = 0b00010101,// 0b101010,
ENDAT_READ_POS = 0b00000111, // 0b11100000,// 0b000111,
ENDAT_SELECT_MEM = 0b00001110, // 0b01110000,// 0b001110,
ENDAT_READ_ADDR = 0b00100011, // 0b11000100,// 0b100011,
ENDAT_WRITE_ADDR = 0b00011100, // 0b00111000,// 0b011100,
ENDAT_RESET = 0b00101010, // 0b01010100,// 0b101010,
} endat_cmd_t;
typedef enum{
ENDAT_MEM_STATE = 0b10011101,// 0b10111001,
ENDAT_MEM_PARAM0 = 0b10000101,// 0b10100001,
ENDAT_MEM_PARAM1 = 0b11000101,// 0b10100011,
ENDAT_MEM_PARAM2 = 0b10100101,// 0b10100101,
ENDAT_MEM_STATE = 0b10111001, // 0b10011101,// 0b10111001,
ENDAT_MEM_PARAM0 = 0b10100001, // 0b10000101,// 0b10100001,
ENDAT_MEM_PARAM1 = 0b10100011, // 0b11000101,// 0b10100011,
ENDAT_MEM_PARAM2 = 0b10100101, // 0b10100101,// 0b10100101,
// ENDAT_MEM_PARAM_3 = 0b10100111,
} endat_mem_t;
@@ -82,44 +82,44 @@ uint32_t endat_tx(endat_cmd_t cmd, uint8_t p1, uint16_t p2, uint8_t* buf, endat_
uint32_t endat_rx(uint8_t* buf, uint32_t max_len, endat_data_t* data);
inline uint8_t flip8(uint8_t d){
uint8_t r = 0;
for(int i = 0; i < sizeof(r) * 8; i++){
r |= d & 1;
uint8_t r = d & 1;
for(int i = 0; i < sizeof(r) * 8 - 1; i++){
r <<= 1;
d >>= 1;
r |= d & 1;
}
return(r);
}
inline uint16_t flip16(uint16_t d){
uint16_t r = 0;
for(int i = 0; i < sizeof(r) * 8; i++){
r |= d & 1;
uint16_t r = d & 1;
for(int i = 0; i < sizeof(r) * 8 - 1; i++){
r <<= 1;
d >>= 1;
r |= d & 1;
}
return(r);
}
inline uint32_t flip32(uint32_t d){
uint32_t r = 0;
for(int i = 0; i < sizeof(r) * 8; i++){
r |= d & 1;
uint32_t r = d & 1;
for(int i = 0; i < sizeof(r) * 8 - 1; i++){
r <<= 1;
d >>= 1;
r |= d & 1;
}
return(r);
}
inline uint64_t flip64(uint64_t d){
uint64_t r = 0;
for(int i = 0; i < sizeof(r) * 8; i++){
r |= d & 1;
uint64_t r = d & 1;
for(int i = 0; i < sizeof(r) * 8 - 1; i++){
r <<= 1;
d >>= 1;
r |= d & 1;
}
return(r);

106
src/comps/endat.c
View File

@@ -35,6 +35,8 @@ HAL_PIN(pos_res);
HAL_PIN(type);
HAL_PIN(max_vel);
HAL_PIN(req);
HAL_PIN(print_time);
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
@@ -91,10 +93,11 @@ static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
}
union{
uint64_t data;
uint8_t dataa[8];
} df, df1;
uint64_t data;
uint8_t dataa[8];
} df, df1, df2;
endat_cmd_t req;
struct endat_ctx_t {
endat_data_t data;
@@ -104,79 +107,88 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
struct endat_ctx_t *ctx = (struct endat_ctx_t *)ctx_ptr;
struct endat_pin_ctx_t *pins = (struct endat_pin_ctx_t *)pin_ptr;
endat_cmd_t req = 0;
req = 0;
uint8_t addr = 0;
switch((int)PIN(endat_state)){
case 0: // select mem state
case 0: // reset error
req = ENDAT_RESET;
break;
case 1: // select mem state
req = ENDAT_SELECT_MEM;
addr = ENDAT_MEM_STATE;
break;
case 1: // read error
case 2: // read error
req = ENDAT_READ_ADDR;
addr = ENDAT_ADDR_ERROR;
break;
case 2: // read warning
case 3: // read warning
req = ENDAT_READ_ADDR;
addr = ENDAT_ADDR_WARNING;
break;
case 3: // select mem 0
case 4: // select mem 0
req = ENDAT_SELECT_MEM;
addr = ENDAT_MEM_PARAM0;
break;
case 4: // read len
case 5: // read len
req = ENDAT_READ_ADDR;
addr = ENDAT_ADDR_POS_LEN;
break;
case 5: // read type
case 6: // read type
req = ENDAT_READ_ADDR;
addr = ENDAT_ADDR_TYPE;
break;
case 6: // select mem 1
case 7: // select mem 1
req = ENDAT_SELECT_MEM;
addr = ENDAT_MEM_PARAM1;
break;
case 7: // read multi
case 8: // read multi
req = ENDAT_READ_ADDR;
addr = ENDAT_ADDR_MULTITURN;
break;
case 8: // read res low
case 9: // read res low
req = ENDAT_READ_ADDR;
addr = ENDAT_ADDR_RES_LOW;
break;
case 9: // read res high
case 10: // read res high
req = ENDAT_READ_ADDR;
addr = ENDAT_ADDR_RES_HIGH;
break;
case 10: // select mem 2
case 11: // select mem 2
req = ENDAT_SELECT_MEM;
addr = ENDAT_MEM_PARAM2;
break;
case 11: // read max vel
case 12: // read max vel
req = ENDAT_READ_ADDR;
addr = ENDAT_ADDR_MAX_VEL;
break;
case 12: // read pos
case 13: // read pos
req = ENDAT_READ_POS;
break;
}
if(PIN(req) > 0){
req = PIN(req);
}
SPI3->CR1 &= ~SPI_CR1_SPE;//disable spi
SPI3->CR1 = SPI_CR1_LSBFIRST | SPI_CR1_MSTR | SPI_CR1_CPOL | SPI_CR1_CPHA | SPI_CR1_SSI | SPI_CR1_SSM | SPI_CR1_BIDIMODE | SPI_BaudRatePrescaler_32;
uint32_t bits = endat_tx(req, addr, 0, df.dataa, &(ctx->data));
df2.data = df.data;
GPIO_SetBits(GPIOD, GPIO_Pin_15);//tx enable
SPI3->CR1 |= SPI_CR1_BIDIOE;//enable output
@@ -227,7 +239,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
PIN(error) = 0;
}
if(ret){
if(ret == 0){
PIN(endat_state) = 0;
PIN(error) = 1;
PIN(state) = 0;
@@ -235,8 +247,13 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
break;
default:
PIN(endat_state)++;
PIN(state) = 0;
if(ret == 0){
PIN(endat_state) = 0;
}
else{
PIN(endat_state)++;
}
}
PIN(mpos) = ctx->data.mpos;
@@ -270,14 +287,60 @@ static void nrt_func(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
PIN(timer) = 0.0;
uint64_t data = df.data;
uint64_t reqdata = df2.data;
uint64_t pos1 = ctx->data.pos;
uint64_t pos2 = ctx->data.mpos;
uint64_t crc = ctx->data.crc;
uint32_t shift = PIN(shift);
printf("req: ");
switch(req){
case ENDAT_SELECT_MEM:
printf("ENDAT_SELECT_MEM\n");
break;
case ENDAT_READ_ADDR:
printf("ENDAT_READ_ADDR\n");
break;
case ENDAT_WRITE_ADDR:
printf("ENDAT_WRITE_ADDR\n");
break;
case ENDAT_READ_POS:
printf("ENDAT_READ_POS\n");
break;
default:
printf("unkonwn req\n");
}
printf("req data: ");
for(int i = 0; i < 64; i++){
if(reqdata & 1){
printf("1");
}
else{
printf("0");
}
reqdata >>= 1;
if(i == 8 - 1){
printf("-");
}
if(i == 8 + 8 - 1){
printf("-");
}
if(i == 8 + 8 + 16 - 1){
printf("-");
}
if(i == 8 + 8 + 16 + 5 - 1){
printf("-");
}
}
printf("\n");
printf("data: ");
for(int i = 0; i < 64; i++){
for(int i = 0; i < 32; i++){
if(data & 1){
printf("1");
}
@@ -297,9 +360,6 @@ static void nrt_func(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
if(i == shift + 2 + ctx->data.pos_bits + ctx->data.mpos_bits - 1){
printf("-");
}
if(i == shift + 2 + ctx->data.pos_bits + ctx->data.mpos_bits + 5 - 1){
printf("-");
}
}
printf("\n");
printf("pos1: ");