From bdd94d53a1ccc85b015caf68058b1864374e9b98 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andis=20Z=C4=ABle?= Date: Sun, 28 Apr 2024 21:00:42 +0300 Subject: [PATCH] Added utils and ain --- firmware/src/bsp/ain.cpp | 35 ++++ firmware/src/bsp/ain.h | 37 ++++ .../bsp/mcu/mcu_hal.cpp | 0 .../{logic-controller => }/bsp/mcu/mcu_hal.h | 0 .../logic-controller.atsln | 0 .../logic-controller.componentinfo.xml | 0 firmware/src/logic-controller.cppproj | 190 ++++++++++++++++++ .../logic-controller/logic-controller.cppproj | 171 ---------------- firmware/src/{logic-controller => }/main.cpp | 0 firmware/src/utils/interpolate.cpp | 163 +++++++++++++++ firmware/src/utils/interpolate.h | 23 +++ firmware/src/utils/utils.cpp | 164 +++++++++++++++ firmware/src/utils/utils.h | 44 ++++ 13 files changed, 656 insertions(+), 171 deletions(-) create mode 100644 firmware/src/bsp/ain.cpp create mode 100644 firmware/src/bsp/ain.h rename firmware/src/{logic-controller => }/bsp/mcu/mcu_hal.cpp (100%) rename firmware/src/{logic-controller => }/bsp/mcu/mcu_hal.h (100%) rename firmware/src/{logic-controller => }/logic-controller.atsln (100%) rename firmware/src/{logic-controller => }/logic-controller.componentinfo.xml (100%) create mode 100644 firmware/src/logic-controller.cppproj delete mode 100644 firmware/src/logic-controller/logic-controller.cppproj rename firmware/src/{logic-controller => }/main.cpp (100%) create mode 100644 firmware/src/utils/interpolate.cpp create mode 100644 firmware/src/utils/interpolate.h create mode 100644 firmware/src/utils/utils.cpp create mode 100644 firmware/src/utils/utils.h diff --git a/firmware/src/bsp/ain.cpp b/firmware/src/bsp/ain.cpp new file mode 100644 index 0000000..1d4cc06 --- /dev/null +++ b/firmware/src/bsp/ain.cpp @@ -0,0 +1,35 @@ +/**** Includes ****/ +#include "../utils/utils.h" +#include "mcu/mcu_hal.h" +#include "ain.h" + +using namespace bsp; + +/**** Private definitions ****/ +/**** Private constants ****/ +/**** Private variables ****/ +/**** Private function declarations ****/ + +/**** Public function definitions ****/ +bsp::AnalogIn::AnalogIn(uint8_t adc_ch) +{ + this->adc_ch = adc_ch; + this->mul = DEF_AIN_MUL; + this->div = DEF_AIN_DIV; + this->offset = DEF_AIN_OFFSET; + this->last_read = 0; +} + +uint16_t bsp::AnalogIn::read(void) +{ + //Read ADC + uint16_t raw = mcu::adc_read(this->adc_ch); + + //Convert to mV + this->last_read = util::convert_muldivoff(raw, this->mul, this->div, this->offset); + + return this->last_read; +} + +/**** Private function definitions ****/ + diff --git a/firmware/src/bsp/ain.h b/firmware/src/bsp/ain.h new file mode 100644 index 0000000..00b90b5 --- /dev/null +++ b/firmware/src/bsp/ain.h @@ -0,0 +1,37 @@ +#ifndef ANALOG_IN_H_ +#define ANALOG_IN_H_ + +/**** Includes ****/ +#include + +namespace bsp { + +/**** Public definitions ****/ +static const uint8_t DEF_AIN_MUL = 215; +static const uint8_t DEF_AIN_DIV = 44; +static const int16_t DEF_AIN_OFFSET = 0; + +class AnalogIn +{ + protected: + uint8_t adc_ch; + + public: + AnalogIn(uint8_t adc_ch); + + uint8_t mul; + uint8_t div; + int16_t offset; + uint16_t last_read; + + uint16_t read(void); +}; + +/**** Public function declarations ****/ + +#ifdef TESTING +#endif + +} //namespace + +#endif /* ANALOG_IN_H_ */ \ No newline at end of file diff --git a/firmware/src/logic-controller/bsp/mcu/mcu_hal.cpp b/firmware/src/bsp/mcu/mcu_hal.cpp similarity index 100% rename from firmware/src/logic-controller/bsp/mcu/mcu_hal.cpp rename to firmware/src/bsp/mcu/mcu_hal.cpp diff --git a/firmware/src/logic-controller/bsp/mcu/mcu_hal.h b/firmware/src/bsp/mcu/mcu_hal.h similarity index 100% rename from firmware/src/logic-controller/bsp/mcu/mcu_hal.h rename to firmware/src/bsp/mcu/mcu_hal.h diff --git a/firmware/src/logic-controller/logic-controller.atsln b/firmware/src/logic-controller.atsln similarity index 100% rename from firmware/src/logic-controller/logic-controller.atsln rename to firmware/src/logic-controller.atsln diff --git a/firmware/src/logic-controller/logic-controller.componentinfo.xml b/firmware/src/logic-controller.componentinfo.xml similarity index 100% rename from firmware/src/logic-controller/logic-controller.componentinfo.xml rename to firmware/src/logic-controller.componentinfo.xml diff --git a/firmware/src/logic-controller.cppproj b/firmware/src/logic-controller.cppproj new file mode 100644 index 0000000..dd54479 --- /dev/null +++ b/firmware/src/logic-controller.cppproj @@ -0,0 +1,190 @@ + + + + 2.0 + 7.0 + com.Atmel.AVRGCC8.CPP + dce6c7e3-ee26-4d79-826b-08594b9ad897 + ATmega328PB + none + Executable + CPP + $(MSBuildProjectName) + .elf + $(MSBuildProjectDirectory)\$(Configuration) + logic-controller + logic-controller + logic-controller + Native + true + false + true + true + + + true + + 2 + 0 + 0 + + + + + + + + + + + + + + + + + + -mmcu=atmega328pb -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb" + True + True + True + True + False + True + True + + + NDEBUG + + + + + %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ + + + Optimize for size (-Os) + True + True + True + True + True + + + NDEBUG + + + + + %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ + + + Optimize for size (-Os) + True + True + True + + + libm + + + + + %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ + + + + + + + + + -mmcu=atmega328pb -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb" + True + True + True + True + False + True + True + + + DEBUG + + + + + %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ + + + Optimize debugging experience (-Og) + True + True + Default (-g2) + True + True + True + + + DEBUG + + + + + %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ + + + Optimize debugging experience (-Og) + True + True + Default (-g2) + True + + + libm + + + + + %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ + + + Default (-Wa,-g) + + + + + + compile + + + compile + + + compile + + + compile + + + compile + + + compile + + + compile + + + compile + + + compile + + + + + + + + + \ No newline at end of file diff --git a/firmware/src/logic-controller/logic-controller.cppproj b/firmware/src/logic-controller/logic-controller.cppproj deleted file mode 100644 index 30eabd3..0000000 --- a/firmware/src/logic-controller/logic-controller.cppproj +++ /dev/null @@ -1,171 +0,0 @@ - - - - 2.0 - 7.0 - com.Atmel.AVRGCC8.CPP - dce6c7e3-ee26-4d79-826b-08594b9ad897 - ATmega328PB - none - Executable - CPP - $(MSBuildProjectName) - .elf - $(MSBuildProjectDirectory)\$(Configuration) - logic-controller - logic-controller - logic-controller - Native - true - false - true - true - - - true - - 2 - 0 - 0 - - - - - - - - - - - - - - - - - - -mmcu=atmega328pb -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb" - True - True - True - True - False - True - True - - - NDEBUG - - - - - %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ - - - Optimize for size (-Os) - True - True - True - True - True - - - NDEBUG - - - - - %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ - - - Optimize for size (-Os) - True - True - True - - - libm - - - - - %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ - - - - - - - - - -mmcu=atmega328pb -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\gcc\dev\atmega328pb" - True - True - True - True - False - True - True - - - DEBUG - - - - - %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ - - - Optimize debugging experience (-Og) - True - True - Default (-g2) - True - True - True - - - DEBUG - - - - - %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ - - - Optimize debugging experience (-Og) - True - True - Default (-g2) - True - - - libm - - - - - %24(PackRepoDir)\atmel\ATmega_DFP\1.7.374\include\ - - - Default (-Wa,-g) - - - - - - compile - - - compile - - - compile - - - - - - - - \ No newline at end of file diff --git a/firmware/src/logic-controller/main.cpp b/firmware/src/main.cpp similarity index 100% rename from firmware/src/logic-controller/main.cpp rename to firmware/src/main.cpp diff --git a/firmware/src/utils/interpolate.cpp b/firmware/src/utils/interpolate.cpp new file mode 100644 index 0000000..a9af4c7 --- /dev/null +++ b/firmware/src/utils/interpolate.cpp @@ -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 util::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 util::find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis) +{ + for(uint8_t i=0; i + +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_ */ \ No newline at end of file diff --git a/firmware/src/utils/utils.cpp b/firmware/src/utils/utils.cpp new file mode 100644 index 0000000..b4801bc --- /dev/null +++ b/firmware/src/utils/utils.cpp @@ -0,0 +1,164 @@ +/**** 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; +} + +uint8_t util::abs_subtract(uint8_t x, uint8_t y) +{ + if(x > y) return x - y; + else return y-x; +} + +uint16_t util::abs_subtract(uint16_t x, uint16_t y) +{ + if(x > y) return x - y; + else return y-x; +} + +uint32_t util::abs_subtract(uint32_t x, uint32_t y) +{ + if(x > y) return x - y; + else return y-x; +} + + +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; +} + +uint16_t util::percent_of(uint8_t percent, uint16_t value) +{ + if(percent == 0) return 0; + else if(percent >= 100) return value; + + uint32_t temp = (uint32_t)value * percent; + return temp/100; +} + +/**** Private function definitions ****/ \ No newline at end of file diff --git a/firmware/src/utils/utils.h b/firmware/src/utils/utils.h new file mode 100644 index 0000000..9f16bb9 --- /dev/null +++ b/firmware/src/utils/utils.h @@ -0,0 +1,44 @@ +#ifndef UTILS_H_ +#define UTILS_H_ + +/**** Includes ****/ +#include + +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); + +uint16_t percent_of(uint8_t percent, uint16_t value); + +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); + +uint8_t abs_subtract(uint8_t x, uint8_t y); +uint16_t abs_subtract(uint16_t x, uint16_t y); +uint32_t abs_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_ */ \ No newline at end of file