MicroRally/uDCCD
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 8 Wiki Activity

board abstraction

Browse Source
This commit is contained in:
Andis Zīle
2024-04-08 19:40:48 +03:00
parent dd4ff43515
commit c50b3d90bf
23 changed files with 1705 additions and 686 deletions
Download Patch File Download Diff File Expand all files Collapse all files

163
firmware/src/utils/interpolate.cpp Normal file
View File

@@ -0,0 +1,163 @@
/**** Includes ****/
#include "utils.h"
#include "interpolate.h"
using namespace util;
/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/
/**** Public function definitions ****/
uint16_t util::interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis)
{
// validate axis length
if(len_axis==0) return 0; // Empty data set
if(len_axis==1) return y_values[0]; // Only one data point
uint16_t y;
uint8_t i = find_interval_end_index(x, x_axis, len_axis);
if(i==0)
{
//Less then start
y = y_values[0];
}
else if(i==len_axis)
{
//More than end
y = y_values[len_axis-1];
}
else
{
// Do interpolate
y = interpolate(x, x_axis[i-1], x_axis[i], y_values[i-1], y_values[i]);
}
return y;
}
uint16_t util::interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values)
{
// validate axis length
if((len_x_axis==0)&&(len_y_axis==0)) return 0; // Empty data set
if((len_x_axis==1)&&(len_y_axis==1)) return z_values[0]; // Only one data point
uint8_t ix = find_interval_end_index(x, x_axis, len_x_axis);
uint8_t iy = find_interval_end_index(y, y_axis, len_y_axis);
// Check corners - easy answers
if((ix==0)&&(iy==0))
{
return z_values[0]; //[0][0] [Y][X]
}
else if((ix==len_x_axis)&&(iy==0))
{
return z_values[len_x_axis-1]; //[0][end]
}
else if((ix==0)&&(iy==len_y_axis))
{
uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
return z_values[i]; //[end][0]
}
else if((ix==len_x_axis)&&(iy==len_y_axis))
{
uint16_t i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
return z_values[i]; //[end][end]
};
// Check boundaries - 1D interpolation
if(ix==0)
{
// On ix=0 line
uint16_t i = 0;
uint16_t z0 = z_values[i];
i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
uint16_t z1 = z_values[i];
return interpolate(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
}
else if(ix==len_x_axis)
{
// On ix=END line
uint16_t i = len_x_axis-1;
uint16_t z0 = z_values[i];
i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
uint16_t z1 = z_values[i];
return interpolate(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
}
else if(iy==0)
{
// On iy=0 line
uint16_t i = 0;
uint16_t z0 = z_values[i];
i = len_x_axis-1;
uint16_t z1 = z_values[i];
return interpolate(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
}
else if(iy==len_y_axis)
{
// On iy=END line
uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
uint16_t z0 = z_values[i];
i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
uint16_t z1 = z_values[i];
return interpolate(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
}
// Do interpolation
// Get axis values
uint16_t x0 = x_axis[ix-1];
uint16_t x1 = x_axis[ix];
uint16_t y0 = y_axis[iy-1];
uint16_t y1 = y_axis[iy];
// Do y0 line calculation
// Get z values at x0 and x1 points on y0 line
uint16_t i = index2d_to_index1d(ix-1, iy-1, len_x_axis);
uint16_t z0 = z_values[i];
uint16_t z1 = z_values[i+1];
// Interpolate z value on y0 line
uint16_t zy0 = interpolate(x, x0, x1, z0, z1);
// Do y1 line calculation
// Get z values at x0 and x1 points on y1 line
i = index2d_to_index1d(ix-1, iy, len_x_axis);
z0 = z_values[i];
z1 = z_values[i+1];
// Interpolate z value on y0 line
uint16_t zy1 = interpolate(x, x0, x1, z0, z1);
// Do calculation in y axis on xz line
return interpolate(y, y0, y1, zy0, zy1);
}
uint16_t interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1)
{
int32_t dy = (int32_t)y1 - (int32_t)y0;
int32_t dx = (int32_t)x1 - (int32_t)x0;
int32_t d = (int32_t)x - (int32_t)x0;
int32_t y = dy * d;
y /= dx;
y += y0;
return util::sat_cast(y);
}
uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis)
{
for(uint8_t i=0; i<len_axis; i++)
{
if(val < axis_values[i]) return i;
continue;
}
return len_axis;
}
uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x)
{
return ((uint16_t)len_x * iy) + ix;
}
/**** Private function definitions ****/

23
firmware/src/utils/interpolate.h Normal file
View File

@@ -0,0 +1,23 @@
#ifndef UTILS_INTERPOLATE_H_
#define UTILS_INTERPOLATE_H_
/**** Includes ****/
#include <stdint.h>
namespace util {
/**** Public definitions ****/
/**** Public function declarations ****/
uint16_t interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis);
uint16_t interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values);
uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis);
uint16_t interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1);
uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x);
#ifdef TESTING
#endif
} //namespace
#endif /* UTILS_INTERPOLATE_H_ */

137
firmware/src/utils/utils.cpp Normal file
View File

@@ -0,0 +1,137 @@
/**** Includes ****/
#include "utils.h"
using namespace util;
/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/
#ifndef TESTING
#endif
/**** Public function definitions ****/
uint8_t util::invert(uint8_t x)
{
if(x!=0) return 0;
else return 1;
}
uint8_t util::sat_add(uint8_t x, uint8_t y)
{
uint8_t z = x + y;
// Check for overflow
if((z < x)||(z < y)) return 0xFF;
else return z;
}
uint16_t util::sat_add(uint16_t x, uint16_t y)
{
uint16_t z = x + y;
// Check for overflow
if((z < x)||(z < y)) return 0xFF;
else return z;
}
uint32_t util::sat_add(uint32_t x, uint32_t y)
{
uint32_t z = x + y;
// Check for overflow
if((z < x)||(z < y)) return 0xFF;
else return z;
}
uint8_t util::sat_subtract(uint8_t x, uint8_t y)
{
uint8_t z = x - y;
// Check for underflow
if(z > x) return 0;
else return z;
}
uint16_t util::sat_subtract(uint16_t x, uint16_t y)
{
uint16_t z = x - y;
// Check for underflow
if(z > x) return 0;
else return z;
}
uint32_t util::sat_subtract(uint32_t x, uint32_t y)
{
uint32_t z = x - y;
// Check for underflow
if(z > x) return 0;
else return z;
}
uint16_t util::sat_cast(uint32_t x)
{
if(x > 0x0000FFFF) return 0xFFFF;
else return (uint16_t)x;
}
uint16_t util::sat_cast(int32_t x)
{
if(x < 0) return 0x0000;
else if(x > 0x0000FFFF) return 0xFFFF;
else return (uint16_t)x;
}
uint16_t util::convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset)
{
int32_t temp = (int32_t)raw;
temp = temp * mul;
if(div>1) temp /= div;
temp += offset;
return sat_cast(temp);
}
uint16_t util::sat_mul_kilo(uint16_t xk, uint16_t yk)
{
uint32_t temp = (uint32_t)xk * (uint32_t)yk;
temp /= 1000;
return sat_cast(temp);
}
uint16_t util::sat_div_kilo(uint16_t top, uint16_t bot)
{
//Sanity check bot
if(bot==0) return 0xFFFF; //aka infinity
uint32_t temp = (uint32_t)top * 1000;
temp /= (uint32_t)bot;
return sat_cast(temp);
}
uint16_t util::sat_ratio(uint16_t top, uint16_t bot)
{
//Sanity check bot
if(bot==0) return 0xFFFF; //aka infinity
//Easy option
if(top>=bot) return 0xFFFF;
uint32_t temp = (uint32_t)top * 0x0000FFFF;
temp /= (uint32_t)bot;
return sat_cast(temp);
}
uint16_t util::percent_to_16b(uint8_t percent)
{
uint32_t temp = (uint32_t)percent * 0x0000FFFF;
temp /= 100;
// Limit to 16 bits
uint16_t pwm = sat_cast(temp);
return pwm;
}
/**** Private function definitions ****/

38
firmware/src/utils/utils.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef UTILS_H_
#define UTILS_H_
/**** Includes ****/
#include <stdint.h>
namespace util {
/**** Public definitions ****/
/**** Public function declarations ****/
uint8_t invert(uint8_t x);
uint16_t sat_cast(uint32_t x);
uint16_t sat_cast(int32_t x);
uint16_t convert_muldivoff(uint16_t raw, uint8_t mul, uint8_t div, int16_t offset);
uint16_t sat_mul_kilo(uint16_t xk, uint16_t yk);
uint16_t sat_div_kilo(uint16_t top, uint16_t bot);
uint16_t sat_ratio(uint16_t top, uint16_t bot);
uint16_t percent_to_16b(uint8_t percent);
uint8_t sat_add(uint8_t x, uint8_t y);
uint16_t sat_add(uint16_t x, uint16_t y);
uint32_t sat_add(uint32_t x, uint32_t y);
uint8_t sat_subtract(uint8_t x, uint8_t y);
uint16_t sat_subtract(uint16_t x, uint16_t y);
uint32_t sat_subtract(uint32_t x, uint32_t y);
uint16_t interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis);
uint16_t interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values);
#ifdef TESTING
#endif
} //namespace
#endif /* UTILS_H_ */