shibosoftwaredev/f1c100s-linux-nema17-stepper-controller

firmware/stm32/main.c

#include "vendor/stm32g030xx.h"
#include "motion.h"
#include "current.h"
#include "limit.h"

uint32_t SystemCoreClock=16000000;
static Motion motion;
static volatile uint32_t milliseconds;
static volatile uint8_t rx[128],rx_write,rx_read;
static uint32_t configured_rate;
static bool temperature_ok;
static int32_t temperature_mc;
static uint32_t temperature_time;
static uint32_t led_wave[96],last_led_time;
static uint8_t last_led_state=255;
static CurrentTelemetry coil_current;
static uint16_t dac_code=0xffff,dac_requested=0xffff,dac_vdda;
static uint32_t dac_written_at,dac_checked_at;

typedef struct { unsigned pin, mode; } GpioMode;
static void gpio(GPIO_TypeDef *port, GpioMode config) {
  unsigned pin=config.pin,mode=config.mode;
  port->MODER=(port->MODER&~(3u<<(pin*2)))|(mode<<(pin*2));
  port->PUPDR&=~(3u<<(pin*2));
}
static void delay_us(unsigned us) { for(volatile unsigned n=0;n<us*4;n++)__NOP(); }
static void status_led(void) {
  if(motion.remaining || motion.step_high)return;
  uint8_t state=motion.faults?2:(motion.armed?1:0);
  if(state==last_led_state || (uint32_t)(milliseconds-last_led_time)<100)return;
  // TIM16 update request 0x2e is defined by ST's stm32g0xx_ll_dmamux.h.
  // Four 250 ns GPIO-DMA cells per WS2812 bit: 0=1000, 1=1110.
  uint32_t grb=state==2?0x000800:(state==1?0x080000:0x000008);
  for(unsigned bit=0;bit<24;bit++) {
    bool one=!!(grb&(1u<<(23-bit)));
    for(unsigned cell=0;cell<4;cell++)led_wave[bit*4+cell]=(cell==0 || (one && cell<3))?1u<<13:1u<<29;
  }
  GPIOA->BSRR=1u<<29;gpio(GPIOA,(GpioMode){13,1});
  DMA1_Channel2->CCR=0;DMA1->IFCR=DMA_IFCR_CGIF2;
  DMAMUX1_Channel1->CCR=0x2e;
  DMA1_Channel2->CPAR=(uint32_t)&GPIOA->BSRR;
  DMA1_Channel2->CMAR=(uint32_t)led_wave;DMA1_Channel2->CNDTR=96;
  DMA1_Channel2->CCR=DMA_CCR_DIR|DMA_CCR_MINC|DMA_CCR_PSIZE_1|DMA_CCR_MSIZE_1|DMA_CCR_PL_1|DMA_CCR_EN;
  TIM16->CR1=0;TIM16->PSC=0;TIM16->ARR=3;TIM16->EGR=TIM_EGR_UG;TIM16->SR=0;
  TIM16->DIER=TIM_DIER_UDE;TIM16->CR1=TIM_CR1_CEN;
  uint32_t start=milliseconds;
  while(!(DMA1->ISR&DMA_ISR_TCIF2) && (uint32_t)(milliseconds-start)<2){}
  TIM16->CR1=0;TIM16->DIER=0;DMA1_Channel2->CCR=0;GPIOA->BSRR=1u<<29;
  delay_us(300);last_led_time=milliseconds;last_led_state=state;
}
static void hardware_sync(void) {
  bool enabled=motion.armed && motion.limit_ready && motion.limit_io_ok && motion.limit_ma && !motion.faults;
  GPIOA->BSRR=enabled ? 1u<<4 : 1u<<20;
  GPIOA->BSRR=motion.reverse ? 1u<<6 : 1u<<22;
  GPIOA->BSRR=motion.step_high ? 1u<<7 : 1u<<23;
  if(!motion.armed || (!motion.remaining && !motion.step_high)) {
    TIM3->CR1=0; configured_rate=0; return;
  }
  if(configured_rate!=motion.rate || !(TIM3->CR1&TIM_CR1_CEN)) {
    TIM3->CR1=0;
    uint32_t cycles=SystemCoreClock/(motion.rate*2);
    uint32_t divider=(cycles+65534)/65535;
    TIM3->PSC=divider-1; TIM3->ARR=cycles/divider-1;
    TIM3->EGR=TIM_EGR_UG;TIM3->SR=0;TIM3->CNT=0;
    configured_rate=motion.rate;TIM3->CR1=TIM_CR1_CEN;
  }
}
void TIM3_IRQHandler(void) {
  TIM3->SR=0;
  motion_step_tick(&motion);
  GPIOA->BSRR=motion.step_high ? 1u<<7 : 1u<<23;
  if(!motion.armed || (!motion.remaining && !motion.step_high))TIM3->CR1=0;
}
void SysTick_Handler(void) {
  ++milliseconds;
  __disable_irq();motion_tick(&motion,milliseconds);hardware_sync();__enable_irq();
}
void USART2_IRQHandler(void) {
  uint32_t flags=USART2->ISR;
  if(flags&(USART_ISR_ORE|USART_ISR_FE|USART_ISR_NE|USART_ISR_PE)) {
    USART2->ICR=USART_ICR_ORECF|USART_ICR_FECF|USART_ICR_NECF|USART_ICR_PECF;
    motion_fault(&motion,PROTOCOL_FAULT);hardware_sync();
  }
  while(USART2->ISR&USART_ISR_RXNE_RXFNE) {
    uint8_t b=(uint8_t)USART2->RDR;
    uint8_t next=(uint8_t)((rx_write+1)&127);
    if(next==rx_read){motion_fault(&motion,PROTOCOL_FAULT);hardware_sync();}
    else {rx[rx_write]=b;rx_write=next;}
  }
}
void TIM17_IRQHandler(void) {
  TIM17->SR=0;
  if(motion.faults)GPIOB->ODR^=1u<<9;
  else GPIOB->BSRR=1u<<25;
}
static bool scl_high(void) {
  GPIOA->BSRR=1u<<11;
  for(unsigned i=0;i<100;i++) {if(GPIOA->IDR&(1u<<11))return true;delay_us(1);}
  return false;
}
static void i2c_stop(void) {
  GPIOA->BSRR=1u<<28;delay_us(5);(void)scl_high();delay_us(5);
  GPIOA->BSRR=1u<<12;delay_us(5);
}
static bool i2c_start(void) {
  GPIOA->BSRR=1u<<12;
  if(!scl_high())return false;
  delay_us(5);GPIOA->BSRR=1u<<28;delay_us(5);GPIOA->BSRR=1u<<27;return true;
}
static bool i2c_write(uint8_t data) {
  for(unsigned bit=0;bit<8;bit++) {
    GPIOA->BSRR=(data&0x80)?1u<<12:1u<<28;data<<=1;delay_us(5);
    if(!scl_high())return false;
    delay_us(5);GPIOA->BSRR=1u<<27;
  }
  GPIOA->BSRR=1u<<12;delay_us(5);
  if(!scl_high())return false;
  delay_us(5);bool ack=!(GPIOA->IDR&(1u<<12));GPIOA->BSRR=1u<<27;return ack;
}
static bool i2c_read(uint8_t *data, bool ack) {
  *data=0;GPIOA->BSRR=1u<<12;
  for(unsigned bit=0;bit<8;bit++) {
    delay_us(5);if(!scl_high())return false;
    delay_us(5);*data=(uint8_t)((*data<<1)|!!(GPIOA->IDR&(1u<<12)));GPIOA->BSRR=1u<<27;
  }
  GPIOA->BSRR=ack?1u<<28:1u<<12;delay_us(5);
  if(!scl_high())return false;
  delay_us(5);GPIOA->BSRR=1u<<27;GPIOA->BSRR=1u<<12;return true;
}
static bool temp_write(uint8_t reg, uint16_t value) {
  bool ok=i2c_start() && i2c_write(0x90) && i2c_write(reg)
    && i2c_write((uint8_t)(value>>8)) && i2c_write((uint8_t)value);
  i2c_stop();return ok;
}
static bool temp_read(void) {
  uint8_t high=0,low=0;
  bool ok=i2c_start() && i2c_write(0x90) && i2c_write(0)
    && i2c_start() && i2c_write(0x91) && i2c_read(&high,true) && i2c_read(&low,false);
  i2c_stop();
  if(ok)temperature_mc=((int16_t)((uint16_t)high<<8|low)>>4)*625/10;
  return ok;
}
static bool dac_write_code(uint16_t code) {
  /* MCP4725 A0 variant, A0 grounded: 0x60. Fast write NEVER writes EEPROM. */
  bool ok=i2c_start() && i2c_write(0xc0)
    && i2c_write((uint8_t)(code>>8)) && i2c_write((uint8_t)code);
  i2c_stop();return ok;
}
static bool dac_verify_code(uint16_t code) {
  uint8_t b[5]={0};
  bool ok=i2c_start() && i2c_write(0xc1);
  for(unsigned i=0;ok && i<5;i++)ok=i2c_read(&b[i],i<4);
  i2c_stop();return ok && limit_readback_ok(b,code);
}
static void dac_failure(void) {
  __disable_irq();motion.limit_io_ok=false;
  motion_fault(&motion,CURRENT_LIMIT_FAULT);hardware_sync();__enable_irq();
}
static void service_current_limit(uint16_t vdda_mv) {
  uint32_t now=milliseconds;
  uint16_t target=motion.faults?0:motion.limit_ma;
  bool changed=target!=dac_requested || !motion.limit_io_ok;
  if(!changed && (uint32_t)(now-dac_checked_at)<20) {
    __disable_irq();
    if(!motion.faults && (uint32_t)(now-dac_written_at)>=CURRENT_LIMIT_SETTLE_MS)
      motion.limit_ready=true;
    __enable_irq();
    return;
  }
  if(changed && target && (vdda_mv<2700 || vdda_mv>3600))return;
  dac_checked_at=now;
  if(changed) {
    uint16_t code=limit_code(target,vdda_mv);
    if(!dac_write_code(code) || !dac_verify_code(code)){dac_failure();return;}
    dac_code=code;dac_requested=target;dac_vdda=vdda_mv;dac_written_at=milliseconds;
    __disable_irq();motion.limit_io_ok=true;motion.limit_ready=false;__enable_irq();
  } else if(!dac_verify_code(dac_code))dac_failure();
}
static bool adc_read(unsigned channel, uint16_t *value) {
  ADC1->ISR=ADC_ISR_CCRDY;
  ADC1->CHSELR=1u<<channel;
  unsigned budget=30000;
  while(!(ADC1->ISR&ADC_ISR_CCRDY) && --budget){}
  if(!budget)return false;
  ADC1->ISR=ADC_ISR_EOC|ADC_ISR_EOS;ADC1->CR|=ADC_CR_ADSTART;
  budget=30000;while(!(ADC1->ISR&ADC_ISR_EOC) && --budget){}
  if(!budget)return false;
  *value=(uint16_t)ADC1->DR;return true;
}
static bool adc_init(void) {
  RCC->APBENR2|=RCC_APBENR2_ADCEN;
  ADC1->CR=ADC_CR_ADVREGEN;delay_us(30);
  ADC1->CFGR2=ADC_CFGR2_CKMODE_1;
  ADC1_COMMON->CCR=ADC_CCR_VREFEN;
  ADC1->SMPR=ADC_SMPR_SMP1;
  ADC1->CR|=ADC_CR_ADCAL;
  unsigned budget=30000;while((ADC1->CR&ADC_CR_ADCAL) && --budget){}
  if(!budget)return false;
  delay_us(5);ADC1->ISR=ADC_ISR_ADRDY;ADC1->CR|=ADC_CR_ADEN;
  budget=30000;while(!(ADC1->ISR&ADC_ISR_ADRDY) && --budget){}
  return budget!=0;
}
static void tx_bytes(const uint8_t *data, unsigned count) {
  for(unsigned i=0;i<count;i++) {
    uint32_t began=milliseconds;
    while(!(USART2->ISR&USART_ISR_TXE_TXFNF))if((uint32_t)(milliseconds-began)>10)return;
    USART2->TDR=data[i];
  }
}
static void put32(uint8_t *p,uint32_t n) {for(unsigned i=0;i<4;i++)p[i]=(uint8_t)(n>>(i*8));}
static void put16(uint8_t *p,uint16_t n) {p[0]=(uint8_t)n;p[1]=(uint8_t)(n>>8);}
static uint8_t saved_request[41],saved_reply[32],saved_count,saved_reply_count;
static void dispatch(const uint8_t *frame,unsigned count) {
  bool duplicate=count==saved_count;
  for(unsigned i=0;duplicate && i<count;i++)if(frame[i]!=saved_request[i])duplicate=false;
  // Reads may be repeated without replaying an old freshness flag. All
  // existing commands retain the original duplicate-reply semantics.
  if(duplicate && frame[4]!=0x21 && frame[4]!=0x23){tx_bytes(saved_reply,saved_reply_count);return;}
  Request r={frame[4],frame[5]|(uint16_t)frame[6]<<8,frame+7};
  __disable_irq();uint8_t status=motion_command(&motion,&r);hardware_sync();__enable_irq();
  uint8_t response[32]={0xa5,0x5a,1,frame[3],frame[4]|0x80,1,0,status};
  unsigned size=8;
  if(frame[4]==0x20 && !status) {
    response[5]=19;response[8]=motion.armed;response[9]=motion.faults;
    put32(response+10,motion.remaining);put32(response+14,motion.rate);
    put32(response+18,motion.inputs.pd_mv);put32(response+22,(uint32_t)motion.inputs.temperature_mc);
    size=26;
  }
  if(frame[4]==0x21 && !status) {
    CurrentTelemetry t=current_snapshot(&coil_current,milliseconds);
    response[5]=18;response[8]=t.flags;
    put32(response+9,(uint32_t)t.a_ma);put32(response+13,(uint32_t)t.b_ma);
    put16(response+17,t.raw_a);put16(response+19,t.raw_b);
    put16(response+21,t.vdda_mv);put16(response+23,t.age_ms);size=25;
  }
  if(frame[4]==0x23 && !status) {
    response[5]=10;
    response[8]=(motion.limit_ready?1:0)|(motion.limit_io_ok?2:0);
    put16(response+9,motion.limit_ma);put16(response+11,dac_code);
    put16(response+13,limit_nominal_ma(dac_code>4095?0:dac_code,dac_vdda));
    put16(response+15,dac_vdda);size=17;
  }
  uint16_t crc=motion_crc16(response+2,size-2);
  response[size++]=(uint8_t)crc;response[size++]=(uint8_t)(crc>>8);
  for(unsigned i=0;i<count;i++)saved_request[i]=frame[i];
  saved_count=(uint8_t)count;
  for(unsigned i=0;i<size;i++)saved_reply[i]=response[i];
  saved_reply_count=(uint8_t)size;
  tx_bytes(response,size);
}
static void receive(void) {
  static uint8_t frame[41];static unsigned used;static uint32_t last_byte;
  if(used && (uint32_t)(milliseconds-last_byte)>20) {
    used=0;__disable_irq();motion_fault(&motion,PROTOCOL_FAULT);hardware_sync();__enable_irq();
  }
  while(rx_read!=rx_write) {
    uint8_t byte=rx[rx_read];rx_read=(uint8_t)((rx_read+1)&127);last_byte=milliseconds;
    if(!used && byte!=0xa5)goto invalid;
    if(used==1 && byte!=0x5a)goto invalid;
    frame[used++]=byte;
    if(used==7 && (frame[2]!=1 || frame[6] || frame[5]>32))goto invalid;
    if(used>=9 && used==(unsigned)(9+frame[5])) {
      uint16_t crc=frame[used-2]|(uint16_t)frame[used-1]<<8;
      if(motion_crc16(frame+2,used-4)!=crc)goto invalid;
      dispatch(frame,used);used=0;
    }
    continue;
invalid:
    used=0;__disable_irq();motion_fault(&motion,PROTOCOL_FAULT);hardware_sync();__enable_irq();
  }
}
static void board_init(void) {
  RCC->IOPENR|=RCC_IOPENR_GPIOAEN|RCC_IOPENR_GPIOBEN;
  GPIOA->BSRR=(1u<<20)|(1u<<22)|(1u<<23);
  gpio(GPIOA,(GpioMode){4,1});gpio(GPIOA,(GpioMode){6,1});gpio(GPIOA,(GpioMode){7,1});
  // Bonded-together GPIOs stay analog except the one named in the schematic.
  for(unsigned pin=0;pin<16;pin++)if(pin!=14)gpio(GPIOB,(GpioMode){pin,3});
  gpio(GPIOB,(GpioMode){7,0});gpio(GPIOB,(GpioMode){3,0});gpio(GPIOB,(GpioMode){9,1});
  GPIOB->BSRR=1u<<25;
  gpio(GPIOA,(GpioMode){0,0});gpio(GPIOA,(GpioMode){1,3});
  // PA5=ADC5, PB0=ADC8. PB1/PB2/PA8 share the PB0 package pin and must
  // remain analog; never configure any of them as a digital output.
  gpio(GPIOA,(GpioMode){5,3});gpio(GPIOA,(GpioMode){8,3});
  gpio(GPIOA,(GpioMode){2,2});gpio(GPIOA,(GpioMode){3,2});
  GPIOA->AFR[0]=(GPIOA->AFR[0]&~((15u<<8)|(15u<<12)))|(1u<<8)|(1u<<12);
  gpio(GPIOA,(GpioMode){11,1});gpio(GPIOA,(GpioMode){12,1});
  GPIOA->OTYPER|=(1u<<11)|(1u<<12);GPIOA->BSRR=(1u<<11)|(1u<<12);
  RCC->APBENR2|=RCC_APBENR2_SYSCFGEN;
  SYSCFG->CFGR1&=~(SYSCFG_CFGR1_PA11_RMP|SYSCFG_CFGR1_PA12_RMP);
  RCC->APBENR1|=RCC_APBENR1_TIM3EN|RCC_APBENR1_USART2EN;
  RCC->APBENR2|=RCC_APBENR2_TIM17EN|RCC_APBENR2_TIM16EN;
  RCC->AHBENR|=RCC_AHBENR_DMA1EN;
  TIM3->DIER=TIM_DIER_UIE;NVIC_SetPriority(TIM3_IRQn,0);NVIC_EnableIRQ(TIM3_IRQn);
  TIM17->PSC=15;TIM17->ARR=249;TIM17->EGR=TIM_EGR_UG;TIM17->SR=0;
  TIM17->DIER=TIM_DIER_UIE;TIM17->CR1=TIM_CR1_CEN;
  NVIC_SetPriority(TIM17_IRQn,3);NVIC_EnableIRQ(TIM17_IRQn);
  USART2->BRR=139;USART2->CR1=USART_CR1_TE|USART_CR1_RE|USART_CR1_RXNEIE_RXFNEIE|USART_CR1_UE;
  NVIC_SetPriority(USART2_IRQn,2);NVIC_EnableIRQ(USART2_IRQn);
  SysTick_Config(SystemCoreClock/1000);NVIC_SetPriority(SysTick_IRQn,1);
  RCC->CSR|=RCC_CSR_LSION;
  IWDG->KR=0xcccc;IWDG->KR=0x5555;IWDG->PR=3;IWDG->RLR=99;IWDG->KR=0xaaaa;
}
int main(void) {
  motion_init(&motion);board_init();__enable_irq();
  bool adc_ok=adc_init();
  // Recover a slave interrupted mid-byte, then program comparator thresholds.
  for(unsigned i=0;i<9;i++){GPIOA->BSRR=1u<<27;delay_us(5);(void)scl_high();delay_us(5);}
  i2c_stop();
  /* Never trust EEPROM's retained/default midscale setting after power-up.
   * The bridge is hardware-pulled asleep until this write/readback and the
   * requested setting have both passed. No nonvolatile write is issued. */
  if(dac_write_code(0) && dac_verify_code(0)) {
    motion.limit_io_ok=true;dac_code=0;dac_requested=0;dac_written_at=milliseconds;
  } else dac_failure();
  bool temp_configured=temp_write(1,0x60a0)&&temp_write(2,0x3c00)&&temp_write(3,0x4600);
  uint32_t last_sample=0,last_current=0;
  uint16_t vdda_mv=0;
  for(;;) {
    receive();
    uint32_t now=milliseconds;
    if((uint32_t)(now-last_current)>=1) {
      last_current=now;uint16_t a=0,b=0;
      bool ok=adc_ok && adc_read(5,&a) && adc_read(8,&b);
      CurrentSample sample={.raw_a=a,.raw_b=b,.vdda_mv=vdda_mv,.now=now,.adc_ok=ok};
      current_update(&coil_current,&sample);
    }
    if((uint32_t)(now-last_sample)>=10) {
      last_sample=now;
      if((uint32_t)(now-temperature_time)>=100) {
        temperature_time=now;temperature_ok=temp_configured && temp_read() && now>=300;
      }
      uint16_t raw=0,ref=0;
      bool power_adc_ok=adc_ok && adc_read(1,&raw) && adc_read(13,&ref);
      uint16_t cal=*(const uint16_t*)0x1fff75aa;
      uint32_t vdda=ref ? (uint32_t)cal*3000/ref : 0;
      vdda_mv=vdda<=65535 ? (uint16_t)vdda:0;
      Inputs in={.pd_good=!!(GPIOB->IDR&(1u<<3)),.driver_ok=!!(GPIOB->IDR&(1u<<7)),
        .sensor_ok=temperature_ok && power_adc_ok && vdda>=2700 && vdda<=3600,
        .pd_mv=(uint32_t)(((uint64_t)raw*vdda*11)/4095),.temperature_mc=temperature_mc};
      if(!(GPIOA->IDR&1u))in.temperature_mc=70000;
      __disable_irq();motion_sample(&motion,&in);hardware_sync();__enable_irq();
    }
    service_current_limit(vdda_mv);
    status_led();IWDG->KR=0xaaaa;__WFI();
  }
}
extern uint32_t _estack,_sidata,_sdata,_edata,_sbss,_ebss;
static void Default_Handler(void) {GPIOA->BSRR=1u<<20;for(;;){} }
void Reset_Handler(void) {
  __disable_irq();uint32_t *src=&_sidata;
  for(uint32_t *p=&_sdata;p<&_edata;)*p++=*src++;
  for(uint32_t *p=&_sbss;p<&_ebss;)*p++=0;
  // Reset state selects HSI16; explicitly retain /1 rather than enabling a PLL.
  RCC->CR&=~RCC_CR_HSIDIV;SCB->VTOR=0x08000000;
  (void)main();Default_Handler();
}
__attribute__((section(".isr_vector"),used))
void (*const vectors[48])(void)={
  [0]=(void (*)(void))&_estack,[1]=Reset_Handler,[2]=Default_Handler,[3]=Default_Handler,
  [11]=Default_Handler,[14]=Default_Handler,[15]=SysTick_Handler,
  [16+TIM3_IRQn]=TIM3_IRQHandler,[16+TIM17_IRQn]=TIM17_IRQHandler,[16+USART2_IRQn]=USART2_IRQHandler,
};