• Habr: ARM Cortex M* — сколько «весит» примитив?

В микроконтроллере STM32F103 нет блока FPU для аппаратной поддержки работы в числами в формате float. Программная реализация занимает больше сотни тактов, поэтому обычно работают с FixedPoint арифметикой, которая работает значительно быстрее.

Такты на операции Habr: ARM Cortex M* — сколько «весит» примитив?

Но FixedPoint накладывает ограничение на диапазон значений и точность. Если знак запятой (точки) ставить "левее", то повышается точность дробной части, но уменьшается целая часть. Если точку ставить правее, то больше диапазон целых чисел, но меньшая точность у дробной части. Нужно заранее знать, что в вычислениях не будет чисел больше какого-то предела, вмещающегося в выбранный формат FixedPoint. Но это не всегда возможно.

Решением будет конвертировать float в ближайший FixedPoint формат для вычислений, а не закладываться сразу на какой-то определенный формат, например FP8.24 (8 бит на целую часть и 24 на дробную).

Например, для реализации разгона мотора по s-кривой, в контроллер передается величина ACC_A в формате float:

const uint32_t RateStart = 100;
const uint32_t RateMax = 50000;

const uint32_t dV_ACC = RateMax - RateStart;
const uint32_t ACC_PNT = 2000;

const float ACC_A = 2 * (float)dV_ACC / (ACC_PNT * ACC_PNT);

Высчитывать текущую скорость с использованием float арифметики слишком накладно. В зависимости от параметров разгона, значение ACC_A может быть либо маленьким дробным без целой части, либо каким-то целым с дробной частью. Чтобы не искать какой формат FixedPoint будет покрывать весь диапазон, проще использовать "положение точки", которое уже заложено в формате float.

По существу, мантисса в float хранится в формате FP1.23, а экспонента показывает куда "точку" надо дополнительно сдвинуть. Вытащим отдельно мантиссу и экспоненту. Получаем вот такую функцию:

bool floatToFP(float value, int32_t *mnt, int8_t *exp)
{
  int32_t valI = *(uint32_t*)&value;
  uint32_t rawsign = (valI >> 31) & 0x01;         /* 1 bit */
  uint32_t rawexp  = (valI >> 23) & 0xff;         /* 8 bits */
  uint32_t rawfrac =  valI        & 0x7fffff;     // 23 bits */
  
  uint32_t expValue;
  
  // Mantissa is FixedPoint 1.23
  // Extponenta is additional shift for radix point
  if(rawexp > 0 && rawexp < 255) {
      expValue = 23 - (rawexp - 127);
      rawfrac = (1UL << 23) | rawfrac;
  } else if(rawexp == 0) {
      expValue = 23 - (rawexp - 126);
  } else {
      // rawexp = 255 NAN
      return false;
  }
  
  // Sign Restore
  if (rawsign != 0) {
      rawfrac = -(uint32_t)rawfrac;
  }  
  // FP0.32 max
  if (expValue > 32) {
      uint8_t offs = expValue - 32;
      rawfrac = rawfrac >> offs;
      expValue = 32;
  }
  
  *mnt = rawfrac;
  *exp = expValue;  
  return true;
}

Теперь, чтобы посчитать значение скорости на i-м шаге ускорения используем умножение в формате целое на FixedPoint:

  uint32_t ii = acc_i * acc_i;
  unt64_t mul = (uint64_t)mnt * (uint64_t)ii; // FP_32.0 * FP_y.x = FP_32+y.x
  mul = mul >> exp;                           // сдвигаем на х бит чтобы убрать дробную часть
  dRate = (uint32_t)mul;

В dRate получаем целое значение скорости.

www.h-schmidt.net: IEEE-754 Floating Point Converter

Продолжая тему разгона моторов, есть необходимость отслеживать время с начала движения, чтобы высчитывать скорость. Проще всего для этого использовать таймер.

Если рассматривать значение таймера как знаковое число, то

• если время старта < текущего времени, то берем разницу этих значений.
• если время старта > текущего времени, то надо взять разницу этих значений как беззнаковых чисел.

void moverUpdateAccRate(Mover_t *mvr)
{   
  int16_t actTime = BRD_ACC_TIM->CNT;  
  int16_t start_time = mvr->accdecStartTime;
  
  uint16_t dTime = 0;
  if (actTime > start_time) {
      dTime = (uint16_t)(actTime - start_time);
  } else {
      dTime = (uint16_t)actTime - (uint16_t)start_time;
  }
  ...
}

Пример того, как можно реализовать разгон и торможение мотора по S кривой. Торможение обычно можно сделать покороче и остановиться на большей скорости, чем стартовая, с которой начиналось движение.

Код на Python, под Stm32 реализуется аналогично:

import serial
import matplotlib.pyplot as plt
import numpy as np
import math

dt = 1   # time of i
pnt_acc = 100
pnt_dec = 50
Vmax = 100
Vstart = 20
Vfin = 40

dV_acc = Vmax - Vstart
dV_dec = Vmax - Vfin

scaleA_acc = 2 * dV_acc  / (pnt_acc **2)
scaleA_dec = 2 * dV_dec  / (pnt_dec **2)


def accRate(i, accPnts, dV, Vmax, scA):
  if i < accPnts/2:
      return Vmax - dV + scA * i**2
  else:
      return Vmax - scA * (accPnts - i)**2


def testTrackCurve(pointCount):
   y = [0 for i in range(pointCount)]

   for i in range(pointCount):      
      dec_i = pointCount - i
      if (dec_i < pnt_dec):
          y[i] = accRate(dec_i, pnt_dec, dV_dec, Vmax, scaleA_dec)
      elif (i < pnt_acc):
          y[i] = accRate(i, pnt_acc, dV_acc, Vmax, scaleA_acc)
      else:   
          y[i] = Vmax

   plt.figure()
   plt.plot(range(pointCount), y)
   plt.show()

   for i in range(pointCount-1):
      y[i] = y[i + 1] - y[i]
   plt.plot(range(pointCount), y)   
   plt.show()


testTrackCurve((pnt_acc + pnt_dec) * 2)