mirror of
https://github.com/rene-dev/stmbl.git
synced 2026-08-18 09:29:10 +08:00
Merge branch 'no_volatile'
This commit is contained in:
crinq
@@ -50,6 +50,7 @@ else
|
||||
CFLAGS += -DV4
|
||||
endif
|
||||
|
||||
COMPS += src/comps/usart.c
|
||||
COMPS += src/comps/encm.c
|
||||
COMPS += src/comps/smartabs.c
|
||||
COMPS += src/comps/adc.c
|
||||
|
||||
+4
-1
@@ -57,4 +57,7 @@ fmove0.max_usr_acc = 5
|
||||
fmove0.force_th = 2.5
|
||||
term0.send_step = 0
|
||||
fmove0.print_freq = 10
|
||||
fmove0.force_offset_lpf = 0.05
|
||||
fmove0.force_offset_lpf = 0.05
|
||||
conf0.j = 0.00003
|
||||
conf0.vel_p = 1500
|
||||
conf0.pos_p = 10
|
||||
|
||||
@@ -110,6 +110,7 @@ static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
PIN(print_timer) += period;
|
||||
|
||||
if(PIN(en) <= 0.0){
|
||||
PIN(force_offset) += PIN(force) * 0.1 * period;
|
||||
PIN(pos) = 0.0;
|
||||
PIN(mpos) = 0.0;
|
||||
PIN(vel) = 0.0;
|
||||
|
||||
+59
-22
@@ -27,10 +27,15 @@
|
||||
|
||||
HAL_COMP(hx);
|
||||
|
||||
HAL_PIN(out);
|
||||
HAL_PIN(out0);
|
||||
HAL_PIN(out1);
|
||||
HAL_PIN(gain);
|
||||
HAL_PIN(offset);
|
||||
HAL_PIN(sleep);
|
||||
HAL_PIN(clk_inv);
|
||||
HAL_PIN(data_inv);
|
||||
HAL_PIN(timer);
|
||||
HAL_PIN(time);
|
||||
|
||||
struct hx_ctx_t {
|
||||
uint32_t error;
|
||||
@@ -66,6 +71,7 @@ static void hw_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
GPIO_Init(FB1_Z_PORT, &GPIO_InitStruct);
|
||||
|
||||
PIN(sleep) = 20;
|
||||
PIN(time) = 1.0 / 80.0;
|
||||
}
|
||||
|
||||
//TODO: plausibility, saturation, channel/gain config, 2 chips
|
||||
@@ -73,33 +79,64 @@ static void hw_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
//struct hx_ctx_t *ctx = (struct hx_ctx_t *)ctx_ptr;
|
||||
struct hx_pin_ctx_t *pins = (struct hx_pin_ctx_t *)pin_ptr;
|
||||
uint32_t value = 0;
|
||||
uint32_t value0 = 0;
|
||||
uint32_t value1 = 0;
|
||||
|
||||
int sleep = CLAMP(PIN(sleep), 1, 100);
|
||||
int sleep = CLAMP(PIN(sleep), 1, 50);
|
||||
|
||||
if(!GPIO_ReadInputDataBit(FB1_A_PORT, FB1_A_PIN)){//data line low = conversion ready
|
||||
for(int i = 0 ; i < 24;i++){
|
||||
nopsleep(sleep);
|
||||
GPIO_SetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
nopsleep(sleep);
|
||||
GPIO_ResetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
if(GPIO_ReadInputDataBit(FB1_A_PORT, FB1_A_PIN)){//dout = 1
|
||||
value++;
|
||||
if(PIN(timer) > PIN(time)){
|
||||
if((PIN(data_inv) > 0.0) ? (GPIO_ReadInputDataBit(FB1_A_PORT, FB1_A_PIN)) : (!GPIO_ReadInputDataBit(FB1_A_PORT, FB1_A_PIN))){//data line low = conversion ready
|
||||
for(int i = 0 ; i < 24;i++){
|
||||
if(PIN(clk_inv) > 0.0){
|
||||
nopsleep(sleep);
|
||||
GPIO_ResetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
nopsleep(sleep);
|
||||
GPIO_SetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
}
|
||||
else{
|
||||
nopsleep(sleep);
|
||||
GPIO_SetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
nopsleep(sleep);
|
||||
GPIO_ResetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
}
|
||||
if((PIN(data_inv) > 0.0) ? (GPIO_ReadInputDataBit(FB1_A_PORT, FB1_A_PIN)) : (!GPIO_ReadInputDataBit(FB1_A_PORT, FB1_A_PIN))){//dout = 1
|
||||
value0++;
|
||||
}
|
||||
value0 = value0 << 1;
|
||||
if((PIN(data_inv) > 0.0) ? (GPIO_ReadInputDataBit(FB1_B_PORT, FB1_B_PIN)) : (!GPIO_ReadInputDataBit(FB1_B_PORT, FB1_B_PIN))){//dout = 1
|
||||
value1++;
|
||||
}
|
||||
value1 = value1 << 1;
|
||||
}
|
||||
//clock additional config bits
|
||||
if(PIN(clk_inv) > 0.0){
|
||||
nopsleep(sleep);
|
||||
GPIO_ResetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
nopsleep(sleep);
|
||||
GPIO_SetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
}
|
||||
else{
|
||||
nopsleep(sleep);
|
||||
GPIO_SetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
nopsleep(sleep);
|
||||
GPIO_ResetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
}
|
||||
value = value << 1;
|
||||
}
|
||||
//clock additional config bits
|
||||
nopsleep(sleep);
|
||||
GPIO_SetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
nopsleep(sleep);
|
||||
GPIO_ResetBits(FB1_Z_PORT, FB1_Z_PIN);
|
||||
|
||||
if(value & 0x800000){//if 24th bit is set, pad others, to get 2 complement number
|
||||
value |= 0xff000000;
|
||||
if(value0 & 0x800000){//if 24th bit is set, pad others, to get 2 complement number
|
||||
value0 |= 0xff000000;
|
||||
}
|
||||
int32_t sint = *((int32_t*)(&value0));
|
||||
PIN(out0) = ((float)sint/(float)0x7fffff)*PIN(gain)+PIN(offset);
|
||||
|
||||
if(value1 & 0x800000){//if 24th bit is set, pad others, to get 2 complement number
|
||||
value1 |= 0xff000000;
|
||||
}
|
||||
sint = *((int32_t*)(&value1));
|
||||
PIN(out1) = ((float)sint/(float)0x7fffff)*PIN(gain)+PIN(offset);
|
||||
PIN(timer) = 0.0;
|
||||
}
|
||||
int32_t sint = *((int32_t*)(&value));
|
||||
PIN(out) = ((float)sint/(float)0x7fffff)*PIN(gain)+PIN(offset);
|
||||
}
|
||||
PIN(timer) += period;
|
||||
}
|
||||
|
||||
hal_comp_t hx_comp_struct = {
|
||||
|
||||
@@ -0,0 +1,184 @@
|
||||
#include "commands.h"
|
||||
#include "hal.h"
|
||||
#include "math.h"
|
||||
#include "defines.h"
|
||||
#include "angle.h"
|
||||
#include "stm32f4xx_conf.h"
|
||||
#include "hw/hw.h"
|
||||
|
||||
HAL_COMP(usart);
|
||||
|
||||
HAL_PIN(req);
|
||||
HAL_PIN(print);
|
||||
HAL_PIN(freq);
|
||||
HAL_PIN(dma); //dma transfers left
|
||||
|
||||
HAL_PIN(pos_offset);
|
||||
HAL_PIN(pos_len);
|
||||
HAL_PIN(pos);
|
||||
|
||||
struct usart_ctx_t {
|
||||
uint8_t rxbuf[16];
|
||||
uint8_t rxbuf2[16];
|
||||
int8_t print;
|
||||
uint8_t dma;
|
||||
};
|
||||
|
||||
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
struct usart_pin_ctx_t * pins = (struct usart_pin_ctx_t *)pin_ptr;
|
||||
PIN(freq) = 2500000;
|
||||
PIN(req) = 0x2A; // 0x32
|
||||
PIN(pos_offset) = 2;
|
||||
PIN(pos_len) = 20;
|
||||
}
|
||||
|
||||
|
||||
static void hw_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
struct usart_ctx_t *ctx = (struct usart_ctx_t *)ctx_ptr;
|
||||
// struct usart_pin_ctx_t * pins = (struct usart_pin_ctx_t *)pin_ptr;
|
||||
GPIO_InitTypeDef GPIO_InitStruct;
|
||||
DMA_InitTypeDef DMA_InitStructure;
|
||||
|
||||
//TX enable
|
||||
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15;
|
||||
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT;
|
||||
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
|
||||
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
|
||||
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
|
||||
GPIO_Init(GPIOD, &GPIO_InitStruct);
|
||||
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART6, ENABLE);
|
||||
|
||||
//USART TX
|
||||
GPIO_PinAFConfig(GPIOC, GPIO_PinSource6, GPIO_AF_USART6);
|
||||
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6;
|
||||
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
|
||||
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
|
||||
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
|
||||
GPIO_Init(GPIOC, &GPIO_InitStruct);
|
||||
|
||||
// DMA-Disable
|
||||
DMA_Cmd(DMA2_Stream1, DISABLE);
|
||||
DMA_DeInit(DMA2_Stream1);
|
||||
|
||||
// DMA2-Config
|
||||
DMA_InitStructure.DMA_Channel = DMA_Channel_5;
|
||||
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) & (USART6->DR);
|
||||
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&ctx->rxbuf;
|
||||
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
|
||||
DMA_InitStructure.DMA_BufferSize = ARRAY_SIZE(ctx->rxbuf);
|
||||
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
|
||||
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
|
||||
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
|
||||
DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte;
|
||||
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
|
||||
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
|
||||
DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
|
||||
DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
|
||||
DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
|
||||
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
|
||||
DMA_Init(DMA2_Stream1, &DMA_InitStructure);
|
||||
|
||||
USART_DMACmd(USART6, USART_DMAReq_Rx, ENABLE);
|
||||
}
|
||||
|
||||
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
struct usart_ctx_t *ctx = (struct usart_ctx_t *)ctx_ptr;
|
||||
struct usart_pin_ctx_t *pins = (struct usart_pin_ctx_t *)pin_ptr;
|
||||
|
||||
USART_InitTypeDef USART_InitStruct;
|
||||
|
||||
PIN(dma) = DMA_GetCurrDataCounter(DMA2_Stream1);
|
||||
|
||||
if(PIN(print) <= 1 && ctx->print == -1){
|
||||
ctx->print = 0;
|
||||
}
|
||||
|
||||
union{
|
||||
uint64_t pos;
|
||||
uint8_t data[8];
|
||||
} foo;
|
||||
|
||||
for(int i = 0; i < PIN(pos_len); i++){
|
||||
foo.data[i] = ctx->rxbuf[i + (int)PIN(pos_offset)];
|
||||
}
|
||||
|
||||
foo.pos &= (2 << (int)PIN(pos_len)) - 1;
|
||||
|
||||
PIN(pos) = foo.pos * 2.0 * M_PI / pow(2, PIN(pos_len)) - M_PI;
|
||||
|
||||
if(PIN(print) > 0 && ctx->print == 0){
|
||||
for(int i = 0; i < ARRAY_SIZE(ctx->rxbuf); i++){
|
||||
ctx->rxbuf2[i] = ctx->rxbuf[i];
|
||||
ctx->rxbuf[i] = 0;
|
||||
}
|
||||
ctx->dma = PIN(dma);
|
||||
ctx->print = 1;
|
||||
PIN(print) = 0;
|
||||
}
|
||||
|
||||
USART_Cmd(USART6, DISABLE);
|
||||
|
||||
USART_InitStruct.USART_BaudRate = PIN(freq);
|
||||
USART_InitStruct.USART_WordLength = USART_WordLength_8b;
|
||||
USART_InitStruct.USART_StopBits = USART_StopBits_1;
|
||||
USART_InitStruct.USART_Parity = USART_Parity_No;
|
||||
USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
|
||||
USART_InitStruct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
|
||||
USART_Init(USART6, &USART_InitStruct);
|
||||
|
||||
/* Enable the USART */
|
||||
USART_Cmd(USART6, ENABLE);
|
||||
USART_HalfDuplexCmd(USART6, ENABLE);
|
||||
|
||||
|
||||
//TODO: irq here will cause problems
|
||||
GPIO_SetBits(GPIOD, GPIO_Pin_15); //tx enable
|
||||
USART_SendData(USART6, (uint8_t)PIN(req));
|
||||
while(USART_GetFlagStatus(USART6, USART_FLAG_TC) == RESET);
|
||||
GPIO_ResetBits(GPIOD, GPIO_Pin_15); //tx disable
|
||||
|
||||
//start rx dma
|
||||
DMA_Cmd(DMA2_Stream1, DISABLE);
|
||||
DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF1);
|
||||
DMA_Cmd(DMA2_Stream1, ENABLE);
|
||||
}
|
||||
|
||||
|
||||
static void nrt_func(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
||||
struct usart_ctx_t *ctx = (struct usart_ctx_t *)ctx_ptr;
|
||||
struct usart_pin_ctx_t *pins = (struct usart_pin_ctx_t *)pin_ptr;
|
||||
|
||||
if(ctx->print > 0){
|
||||
ctx->print = -1;
|
||||
printf("req: 0x%02X: rx: %u : ", (uint8_t) PIN(req), ctx->dma);
|
||||
for(int i = 0; i < ARRAY_SIZE(ctx->rxbuf); i++){
|
||||
for(int j = 0; j < 8; j++){
|
||||
if(ctx->rxbuf2[i] & (1 << j % 8)) {
|
||||
printf("1");
|
||||
} else {
|
||||
printf("0");
|
||||
}
|
||||
}
|
||||
printf(" ");
|
||||
//printf("%02X ", ctx->rxbuf2[i]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
hal_comp_t usart_comp_struct = {
|
||||
.name = "usart",
|
||||
.nrt = nrt_func,
|
||||
.rt = rt_func,
|
||||
.frt = 0,
|
||||
.nrt_init = nrt_init,
|
||||
.hw_init = hw_init,
|
||||
.rt_start = 0,
|
||||
.frt_start = 0,
|
||||
.rt_stop = 0,
|
||||
.frt_stop = 0,
|
||||
.ctx_size = sizeof(struct usart_ctx_t),
|
||||
.pin_count = sizeof(struct usart_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
||||
};
|
||||
+18
-14
@@ -1,18 +1,22 @@
|
||||
#!/usr/bin/env python
|
||||
import serial.tools.list_ports
|
||||
done = False
|
||||
import time
|
||||
|
||||
for port in serial.tools.list_ports.comports():
|
||||
if "483:5740" in port[2]:
|
||||
print ("Reseting stmbl at " + port[2])
|
||||
try:
|
||||
stmbl = serial.Serial(port[0])
|
||||
stmbl.write('bootloader\n'.encode())
|
||||
stmbl.close()
|
||||
done = True
|
||||
except OSError:
|
||||
print ("Cannot open port, check permissions, or disconnect Servoterm")
|
||||
done = True
|
||||
for i in range(0, 20):
|
||||
done = False
|
||||
time.sleep(1)
|
||||
for port in serial.tools.list_ports.comports():
|
||||
if "483:5740" in port[2]:
|
||||
print ("Reseting stmbl at " + port[2])
|
||||
try:
|
||||
stmbl = serial.Serial(port[0])
|
||||
stmbl.write('bootloader\n'.encode())
|
||||
stmbl.close()
|
||||
done = True
|
||||
except OSError:
|
||||
print ("Cannot open port, check permissions, or disconnect Servoterm")
|
||||
done = True
|
||||
|
||||
if not done:
|
||||
print ("No boards found, or board is already in bootloader")
|
||||
if not done:
|
||||
print ("No boards found, or board is already in bootloader")
|
||||
break
|
||||
|
||||
Reference in New Issue
Block a user