shibosoftwaredev/f1c100s-linux-nema17-stepper-controller

firmware/stm32/current.c

#include "current.h"

// INA241B1 gain 10 and inline 0.300 ohm shunts: 3 V/A, zero at VDDA/2.
// This is nominal filtered telemetry, not calibrated protection or true RMS.
static int32_t milliamps(uint16_t raw, uint16_t vdda) {
  int32_t numerator=((int32_t)raw*2-4095)*(int32_t)vdda;
  const int32_t denominator=4095*6;
  return numerator>=0 ? (numerator+denominator/2)/denominator
    : -((-numerator+denominator/2)/denominator);
}
void current_update(CurrentTelemetry *t, const CurrentSample *s) {
  *t=(CurrentTelemetry){.raw_a=s->raw_a,.raw_b=s->raw_b,.vdda_mv=s->vdda_mv,.updated_at=s->now};
  if(!s->adc_ok || s->vdda_mv<2700 || s->vdda_mv>3600 || s->raw_a>4095 || s->raw_b>4095)return;
  t->a_ma=milliamps(s->raw_a,s->vdda_mv);t->b_ma=milliamps(s->raw_b,s->vdda_mv);
  // At least 200 mV of upper-rail headroom is specified. Rail-adjacent
  // readings remain visible but must not be represented as valid current.
  if(s->raw_a<64 || (uint32_t)s->raw_a*s->vdda_mv>(uint32_t)(s->vdda_mv-200)*4095)
    t->flags|=CURRENT_A_CLIPPED;
  else t->flags|=CURRENT_A_VALID;
  if(s->raw_b<64 || (uint32_t)s->raw_b*s->vdda_mv>(uint32_t)(s->vdda_mv-200)*4095)
    t->flags|=CURRENT_B_CLIPPED;
  else t->flags|=CURRENT_B_VALID;
}
CurrentTelemetry current_snapshot(const CurrentTelemetry *t, uint32_t now) {
  CurrentTelemetry result=*t;
  uint32_t age=now-t->updated_at;
  result.age_ms=age>65535 ? 65535:(uint16_t)age;
  if(age>20)result.flags&=~(CURRENT_A_VALID|CURRENT_B_VALID);
  return result;
}