shibosoftwaredev/f1c100s-linux-nema17-stepper-controller

firmware/stm32/test_motion.c

#include "motion.h"
#include <assert.h>
#include <stdio.h>
#include <string.h>

static const Inputs healthy={true,true,true,15000,25000};
static uint8_t command(Motion *m, uint8_t cmd) {
  Request r={cmd,0,0}; return motion_command(m,&r);
}
static void arm(Motion *m) {
  /* Mock the independently verified hardware DAC service. */
  m->limit_io_ok=true;m->limit_ready=true;
  uint8_t one=1; Request r={2,1,&one};
  motion_sample(m,&healthy); assert(command(m,1)==0); assert(motion_command(m,&r)==0);
  motion_tick(m,m->now+3);
}
int main(void) {
  Motion m; motion_init(&m); assert(!m.armed);
  uint8_t one=1; Request a={2,1,&one}; assert(motion_command(&m,&a)==5);
  arm(&m); motion_tick(&m,249); assert(m.armed);
  motion_tick(&m,250); assert(!m.armed && (m.faults&HEARTBEAT_FAULT));
  motion_init(&m); arm(&m);
  uint8_t wake_bytes[8]={1,0,0,0,200,0,0,0}; Request wake_move={0x10,8,wake_bytes};
  m.now=m.armed_at+2; assert(motion_command(&m,&wake_move)==4 && !m.remaining);
  m.now=m.armed_at+3; assert(motion_command(&m,&wake_move)==0);
  motion_init(&m); arm(&m);
  Inputs broken=healthy; broken.pd_good=false; motion_sample(&m,&broken);
  assert(!m.armed && (m.faults&POWER_FAULT));
  motion_init(&m); arm(&m); broken=healthy; broken.driver_ok=false; motion_sample(&m,&broken);
  assert(!m.armed && (m.faults&DRIVER_FAULT));
  motion_init(&m); arm(&m); broken=healthy; broken.temperature_mc=71000; motion_sample(&m,&broken);
  assert(!m.armed && (m.faults&TEMP_FAULT));
  broken.temperature_mc=65000; motion_sample(&m,&broken);
  uint8_t zero=0; Request disarm={2,1,&zero}; motion_command(&m,&disarm); assert(m.faults&TEMP_FAULT);
  motion_sample(&m,&healthy); motion_command(&m,&disarm); assert(!m.faults);
  motion_init(&m); arm(&m); broken=healthy; broken.sensor_ok=false; motion_sample(&m,&broken);
  assert(!m.armed && (m.faults&SENSOR_FAULT));
  motion_init(&m); arm(&m); motion_fault(&m,PROTOCOL_FAULT); assert(!m.armed);
  for(unsigned count=1;count<=1000;count++) {
    motion_init(&m); arm(&m);
    uint8_t bytes[8]={(uint8_t)count,(uint8_t)(count>>8),0,0,200,0,0,0};
    Request move={0x10,8,bytes}; assert(!motion_command(&m,&move));
    unsigned rising=0;
    for(unsigned tick=0;tick<count*2+20;tick++) {
      bool old=m.step_high; motion_step_tick(&m); if(!old && m.step_high)rising++;
    }
    assert(rising==count && !m.remaining && !m.step_high);
    assert(command(&m,0x12)==0 && !m.armed);
  }
  motion_init(&m); arm(&m);m.remaining=100000;m.rate=1000;m.stopping=true;
  for(unsigned now=0;now<1000 && m.armed;now++) {
    if(now%50==0)command(&m,1);
    motion_tick(&m,now);
  }
  assert(!m.armed && !m.remaining);
  motion_init(&m);arm(&m);
  uint8_t reverse_bytes[8]={0xfd,0xff,0xff,0xff,200,0,0,0};
  Request reverse_move={0x10,8,reverse_bytes};
  assert(!motion_command(&m,&reverse_move) && m.reverse && m.remaining==3);
  assert(motion_command(&m,&reverse_move)==4 && m.remaining==3);
  for(unsigned i=0;i<6;i++)motion_step_tick(&m);
  assert(!m.remaining && !m.step_high);
  reverse_bytes[4]=0x21;reverse_bytes[5]=0x4e;
  assert(motion_command(&m,&reverse_move)==2 && !m.remaining);
  motion_init(&m);m.now=0xfffffff0;arm(&m);motion_tick(&m,0x80);assert(m.armed);
  motion_tick(&m,0x100);assert(!m.armed);
  motion_init(&m);arm(&m);m.remaining=37;m.rate=200;m.reverse=true;
  unsigned char before[sizeof m];memcpy(before,&m,sizeof m);
  assert(command(&m,0x21)==0 && memcmp(before,&m,sizeof m)==0);
  Request bad_current={0x21,1,&one};
  assert(motion_command(&m,&bad_current)==2 && memcmp(before,&m,sizeof m)==0);
  motion_tick(&m,249);assert(command(&m,0x21)==0 && m.armed);
  motion_tick(&m,250);assert(!m.armed && (m.faults&HEARTBEAT_FAULT));
  assert(motion_crc16((const uint8_t*)"123456789",9)==0x29b1);
  motion_init(&m);motion_sample(&m,&healthy);m.limit_io_ok=true;
  assert(motion_command(&m,&a)==4 && !m.armed);
  uint8_t ma[2]={200,0};Request limit={0x22,2,ma};
  uint32_t heartbeat=m.last_heartbeat;
  assert(motion_command(&m,&limit)==0 && m.limit_ma==200 && !m.limit_ready);
  assert(m.last_heartbeat==heartbeat);
  arm(&m);assert(motion_command(&m,&limit)==4 && m.limit_ma==200);
  motion_fault(&m,CURRENT_LIMIT_FAULT);assert(!m.armed && !m.limit_ready);
  ma[0]=0x8d;ma[1]=1;assert(motion_command(&m,&limit)==2 && m.limit_ma==200);
  ma[0]=0;ma[1]=0;assert(motion_command(&m,&limit)==0 && m.limit_ma==0);
  m.faults=0;m.limit_ready=true;assert(motion_command(&m,&a)==3 && !m.armed);
  puts("PASS: startup/arm/wake, heartbeat/wraparound, PD/driver/temperature/sensor faults, controlled stop, reverse/busy/rate validation, read-only current requests cannot refresh heartbeat, CRC vector, 1000 exact pulse counts");
}