shibosoftwaredev/f1c100s-linux-nema17-stepper-controller

firmware/stm32/motion.c

#include "motion.h"
#include "limit.h"

static uint32_t read32(const uint8_t *p) {
  return (uint32_t)p[0] | (uint32_t)p[1]<<8 | (uint32_t)p[2]<<16 | (uint32_t)p[3]<<24;
}
static uint8_t input_faults(const Motion *m) {
  return (!m->inputs.pd_good || m->inputs.pd_mv<10800 || m->inputs.pd_mv>16500 ? POWER_FAULT:0)
    | (!m->inputs.driver_ok ? DRIVER_FAULT:0)
    | (!m->inputs.sensor_ok ? SENSOR_FAULT:0)
    | (!m->limit_io_ok ? CURRENT_LIMIT_FAULT:0)
    | (m->inputs.temperature_mc>=70000 || ((m->faults&TEMP_FAULT) && m->inputs.temperature_mc>60000) ? TEMP_FAULT:0);
}
void motion_init(Motion *m) { *m=(Motion){.limit_ma=CURRENT_LIMIT_MAX_MA}; }
void motion_fault(Motion *m, uint8_t reason) {
  m->faults|=reason; m->armed=false; m->remaining=0; m->step_high=false; m->stopping=false;
  m->limit_ready=false;
}
void motion_sample(Motion *m, const Inputs *inputs) {
  m->inputs=*inputs;
  if(m->armed && input_faults(m)) motion_fault(m,input_faults(m));
}
void motion_tick(Motion *m, uint32_t now) {
  m->now=now;
  if(m->armed && (!m->heartbeat_seen || (uint32_t)(now-m->last_heartbeat)>=250))
    motion_fault(m,HEARTBEAT_FAULT);
  if(m->stopping && (uint32_t)(now-m->last_ramp)>=10) {
    m->last_ramp=now;
    if(m->rate<=10) {m->remaining=0; m->stopping=false; m->armed=false; m->step_high=false;}
    else m->rate-=m->rate/8 ? m->rate/8 : 1;
  }
}
uint8_t motion_command(Motion *m, const Request *r) {
  switch(r->command) {
    case 1:
      if(r->length) return 2;
      m->last_heartbeat=m->now; m->heartbeat_seen=true; return 0;
    case 2:
      if(r->length!=1 || r->payload[0]>1) return 2;
      if(!r->payload[0]) {
        m->armed=false; m->remaining=0; m->step_high=false; m->stopping=false;
        if(!input_faults(m)) m->faults=0;
        return 0;
      }
      if(input_faults(m) || m->faults) return ((input_faults(m)|m->faults)&TEMP_FAULT) ? 6:5;
      if(!m->limit_ready)return 4;
      if(!m->limit_ma)return 3;
      if(!m->heartbeat_seen || (uint32_t)(m->now-m->last_heartbeat)>=100) return 3;
      if(!m->armed) m->armed_at=m->now;
      m->armed=true; return 0;
    case 0x10: {
      if(r->length!=8) return 2;
      if(!m->armed || m->faults) return 3;
      // DRV8825 tWAKE is 1.7 ms. Three millisecond ticks guarantee at least
      // 2 ms of real wake time even at the SysTick boundary.
      if((uint32_t)(m->now-m->armed_at)<3) return 4;
      if(m->remaining || m->step_high) return 4;
      int32_t steps=(int32_t)read32(r->payload);
      uint32_t rate=read32(r->payload+4);
      if(!steps || rate<1 || rate>20000) return 2;
      m->reverse=steps<0;
      m->remaining=steps<0 ? (uint32_t)(-(int64_t)steps) : (uint32_t)steps;
      m->rate=rate; m->stopping=false; return 0;
    }
    case 0x11:
      if(r->length) return 2;
      if(m->remaining) {m->stopping=true; m->last_ramp=m->now;}
      else m->armed=false;
      return 0;
    case 0x12:
      if(r->length) return 2;
      m->armed=false; m->remaining=0; m->step_high=false; m->stopping=false; return 0;
    case 0x22: {
      if(r->length!=2)return 2;
      uint16_t ma=r->payload[0]|(uint16_t)r->payload[1]<<8;
      if(ma>CURRENT_LIMIT_MAX_MA)return 2;
      if(m->armed || m->remaining || m->step_high)return 4;
      m->limit_ma=ma; m->limit_ready=false; return 0;
    }
    case 0x20:
    case 0x21:
    case 0x23: return r->length ? 2:0;
    default: return 1;
  }
}
void motion_step_tick(Motion *m) {
  if(!m->armed || m->faults) {m->step_high=false; return;}
  if(m->step_high) {m->step_high=false; return;}
  if(m->remaining) {m->step_high=true; --m->remaining;}
}
uint16_t motion_crc16(const uint8_t *p, size_t count) {
  uint16_t crc=0xffff;
  while(count--) {
    crc^=(uint16_t)*p++<<8;
    for(unsigned i=0;i<8;i++) crc=(uint16_t)((crc&0x8000)?(crc<<1)^0x1021:crc<<1);
  }
  return crc;
}