AVR/embedded: быстрая нормализация векторов?

Я потратил впустую несколько дней, борясь со своим 16-мегагерцовым 8-битным AVR (мега 2560).

Цель состоит в том, чтобы нормализовать значение, которое я получаю (акселерометр, магнитометр и т. д.).
Значения имеют 16-битную подпись (int16), и после того, как мне нужно число с плавающей запятой от 0,0f до 1,0f, я использую это для 3D IMU.

Общий подход:

int32_t tmp = (int32_t)a*a+b*b+c*c;
float magnitude = sqrt(tmp);
float a_v = a / magnitude;
float b_v = b / magnitude;
float c_v = c / magnitude;

Более быстрый подход:

int32_t tmp = (int32_t)a*a+b*b+c*c;
float imagnitude = InvSqrt(tmp); // like the 'tricky' one for ID software quake source
float a_v = a * imagnitude;
float b_v = b * imagnitude;
float c_v = c * imagnitude;

У второго есть некоторые преимущества, поскольку он использует аппроксимацию вместо 1/sqrt (но есть также аппроксимированные sqrt) и требует 3 умножения вместо деления, что хорошо, поскольку AVR поддерживает MUL, но не DIV. С другой стороны, в любом случае это очень медленно из-за вычислений с плавающей запятой и 32-битных вычислений.

Такая функция обычно занимает 1-2 миллисекунды, что оказывает огромное влияние на мой код, который пытается решить множество дополнительных задач и нормализаций в цикле, который должен длиться максимум 2,4 мс.

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

Может быть, есть другой подход к нормализации значений моего датчика.

Обновление для людей с моей особой проблемой (величина акселерометра): Без плавающей точки и sqrt я работаю над этим прямо сейчас: (игнорируйте дополнительные приведения:) int16 cal[] содержит откалиброванное значение акселерометра для 3 оси.

int16 average_sq_1g = CONST_1G / 256;
uint32_t work = (int32_t)((int32_t)cal[0]*cal[0] + (int32_t)cal[1]*cal[1] + (int32_t)cal[2]*cal[2])/256;
work = work * 100L / average_sq_1g;
attitude.acc_magnitude = work;

Это довольно специализировано для моего дела, так как я работаю, чтобы получить величину ускорения, и я знаю значение, которое я получаю за 1G (около 16000). Итак, формула (X ^ 2 + Y ^ 2 + Z ^ 2) * 100/1G ^2 возвращает мне величину (100 = без дополнительного ускорения и может быть выполнено без использования чисел с плавающей запятой.
Я не проверял разницу в производительности, но это должно быть намного быстрее.


c performance approximation normalization avr
person John    schedule 23.06.2014    source источник
comment
Зачем вообще использовать плавающую точку? Можете ли вы вместо этого использовать фиксированную точку (например, Q15)?   -  person anatolyg    schedule 24.06.2014
comment
Первая линия нуждается в большем количестве бросков. Первое умножение — 1nt32_t, а два других — только между целыми числами. Может произойти переполнение.   -  person uncleO    schedule 24.06.2014
comment
UncleO: Сомневаюсь, что будет переполнение, компилятор должен корректно обрабатывать всю операцию. Я использую аналогичный код, и он отлично работает без переполнения. @anatolyg: В настоящее время я меняю весь свой код на целочисленный 32-битный, так как не мог заставить его работать быстрее. Я не уверен, насколько поможет фиксированная точка, на AVR-GCC нет поддержки фиксированной точки с плавающей запятой.   -  person John    schedule 24.06.2014
comment
SCNR: +1 за ссылку InvSqrt() на источник землетрясения   -  person Rev    schedule 24.06.2014
comment
@UncleO: все умножения находятся между int, а не int16_t, из-за повышения.   -  person Ben Voigt    schedule 25.06.2014
comment
@BenVoigt Спасибо, Бен. Насколько мне известно, в AVR int равно int16_t. Вот причина моего комментария. Я предполагал, что два множества будут встречаться как целые числа и не будут повышаться до long int или int32_t до добавления. Джон говорит, что его тесты показывают обратное.   -  person uncleO    schedule 25.06.2014
comment
@John, фиксированная точка использует целочисленное оборудование, для него не нужна специальная поддержка.   -  person Ben Voigt    schedule 25.06.2014

Ответы (2)


arrow_upward
2
arrow_downward

использовать арифметику с фиксированной точкой.

масштабируйте ввод на разумное количество бит и используйте операции с целыми числами. (есть также алгоритмы вычисления целого квадратного корня)

например, если у вас есть диапазон значений от -10 м до 10 м и требуется разрешение не менее мм, я бы добавил 11 бит (в масштабе 2048)

#define VEC_SHIFT 11
#define VEC_SCALE (1 << (VEC_SHIFT))
int16_t a =  7 * VEC_SCALE;
int16_t b =  3 * VEC_SCALE;
int16_t c = 10 * VEC_SCALE;

// calculations have to be done in larger data type so they do not overflow
int32_t snorm = (int32_t)a * a + (int32_t)b*b + (int32_t)c*c;  // snorm now is scaled by VEC_SCALE*VEC_SCALE (2*VEC_SHIFT)
int16_t norm = intsqrt(snorm); // norm is scaled with VEC_SCALE

// since norm and a,b,c is in VEC_SCALE, you have to scale up the divident so that one VEC_SCALE is chanceled out by division 
int16_t as =  (((int32_t)a) * VEC_SCALE )/norm;
int16_t bs =  (((int32_t)b) * VEC_SCALE )/norm;
int16_t cs =  (((int32_t)c) * VEC_SCALE )/norm;
person vlad_tepesch    schedule 24.06.2014

arrow_upward
3
arrow_downward

Фиксированная точка, безусловно, путь. Единственная трудность состоит в том, чтобы сохранить точность.

В этом случае, если важна точность, я бы использовал следующий довольно быстрый алгоритм:

  • извлечь показатели каждого числа с плавающей запятой (биты 23..30 числа с плавающей запятой)
  • сохранить знак каждого значения (бит 31) для дальнейшего использования
  • извлеките 15 старших битов мантиссы (биты 8..22), добавьте одну «1» слева

Это может показаться сложным, но это может быть выполнено, например,

sign = b3 & 0x80;
exponent = b3 << 1;
if (b2 & 0x80)
  exponent |= 1;
else
  b2 |= 0x80;
mantissa = join_to_word(b1, b2);

где b0..b3 — это одиночные октеты числа с плавающей запятой (b3 — это октеты со знаком, см. структуру числа с плавающей запятой IEEE754). Ориентация b2 на 0x80 связана со скрытым битом в представлении с плавающей запятой. Функция join_to_word предназначена для объединения двух байтов в слово. Это не должно приводить к созданию одной инструкции в машинном коде, поскольку только компилятору нужно знать, где находятся два октета. (Один из способов добиться этого — использовать союзы.)

Теперь, когда мы знаем показатели:

  • найти наибольший показатель
  • вычислить квадрат мантиссы для этого числа
  • сдвиг вправо на 2
  • for the other two mantissas
    • shift to the right by the exponent difference (i.e. if the largest exponent is 17, a number with exponent 14 needs to be shifted right by 17-14=3)
    • если разница >= 7, забудьте это число (оба немного оптимизируют и обрабатывают нулевые показатели)
    • в противном случае возведите в квадрат мантиссу, сдвиньте вправо на 2 и прибавьте к квадрату, рассчитанному выше
  • извлеките квадратный корень из суммы квадратов

Итак, на данный момент имеем:

  • норма с 15-16 значащими битами
  • абсолютные значения компонентов с 16 значащими битами

Что остается в целочисленной области, так это выполнить деление, сдвинув абсолютные значения на 15 бит влево и разделив их на норму. Полученные векторы затем масштабируются на 2^16.

Затем у нас осталось преобразование в число с плавающей запятой. Основные шаги для каждого числа

  • если число равно нулю, заполните fp нулями, иначе выполните следующие шаги
  • показатель степени 127+16 = 142
  • если самый значащий бит не равен единице, сдвиньте мантиссу влево до тех пор, пока старший бит не станет равным единице, уменьшайте показатель степени при каждом сдвиге
  • сбросить старший бит мантиссы
  • если младший бит экспоненты равен 1 или старшему биту мантиссы
  • сдвинуть показатель степени вправо на единицу
  • ИЛИ знак обратно к октету экспоненты
  • собрать число fp из экспоненты, мантиссы и байта нулей

Весь алгоритм должен выполняться за несколько сотен тактов.

Если вы очень торопитесь, первым делом посмотрите на ассемблерный код. Могут быть ненужные вызовы библиотек, ненужные нулевые байты и т. д., в зависимости от того, как вы написали код и что думает или не думает ваш компилятор C. Второй шаг — написать эту процедуру на ассемблере, но обычно этого можно избежать.

Но есть одна вещь, которую вы должны рассмотреть: действительно ли вообще необходимо использовать плавающие точки? Они медленные.

person DrV    schedule 25.06.2014