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

Could be working version

Browse Source
This commit is contained in:
Andis Zīle
2024-04-12 15:18:31 +03:00
parent 5bb3ebe1bf
commit f8b62d4b00
19 changed files with 632 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
firmware/src/bsp/board.cpp Normal file
View File

@@ -0,0 +1,62 @@
/**** Includes ****/
#include "../utils/utils.h"
#include "board.h"
/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/
/**** Public function definitions ****/
void board_init(void)
{
// MCU setup
mcu::startupCfg_t mcu_cfg;
mcu_cfg.adc_clk = mcu::ADC_DIV2;
mcu_cfg.pwm_clk = mcu::TIM_DIV1;
mcu_cfg.pwm_top = 200;
mcu_cfg.pwm_ch1_en = 1;
mcu::startup(&mcu_cfg);
// Board setup
dccd_i.mul = 215;
dccd_i.div = 22;
dccd_i.offset = 0;
dccd_i.last_read = 0;
dccd_u.mul = 20;
dccd_u.div = 1;
dccd_u.offset = 0;
dccd_u.last_read = 0;
bat_u.mul = 20;
bat_u.div = 1;
bat_u.offset = 0;
bat_u.last_read = 12000;
bat_i.mul = 235;
bat_i.div = 6;
bat_i.offset = 0;
bat_i.last_read = 0;
}
void board_read(void)
{
dccd_i.read();
dccd_u.read();
bat_u.read();
bat_i.read();
ain1.read();
ain2.read();
din1.read();
din2.read();
din3.read();
din4.read();
hvdin1.read();
hvdin2.read();
hvdin3.read();
}
/**** Private function definitions ****/

41
firmware/src/bsp/board.h Normal file
View File

@@ -0,0 +1,41 @@
#ifndef BSP_BOARD_H_
#define BSP_BOARD_H_
/**** Includes ****/
#include <stdint.h>
#include "mcu/mcu_hal.h"
#include "ain.h"
#include "din.h"
#include "dout.h"
#include "dio.h"
#include "halfbridge.h"
#include "pwm.h"
static bsp::AnalogIn dccd_i = bsp::AnalogIn(mcu::ADC0);
static bsp::AnalogIn dccd_u = bsp::AnalogIn(mcu::ADC1);
static bsp::AnalogIn bat_u = bsp::AnalogIn(mcu::ADC2);
static bsp::AnalogIn bat_i = bsp::AnalogIn(mcu::ADC3);
static bsp::Hafbridge hbridge = bsp::Hafbridge(mcu::PWM0, mcu::GPIO15, 95);
static bsp::AnalogIn ain1 = bsp::AnalogIn(mcu::ADC5); // mode
static bsp::AnalogIn ain2 = bsp::AnalogIn(mcu::ADC4); // pot
static bsp::DigitalIn din1 = bsp::DigitalIn(mcu::GPIO0, 0, bsp::DIN_HIGH); //mode
static bsp::DigitalIn din2 = bsp::DigitalIn(mcu::GPIO1, 0, bsp::DIN_HIGH); //pot
static bsp::DigitalIn din3 = bsp::DigitalIn(mcu::GPIO2, 0, bsp::DIN_HIGH); //down
static bsp::DigitalIn din4 = bsp::DigitalIn(mcu::GPIO3, 0, bsp::DIN_HIGH); //up
static bsp::DigitalIn hvdin1 = bsp::DigitalIn(mcu::GPIO4, 1, bsp::DIN_LOW); //dimm
static bsp::DigitalIn hvdin2 = bsp::DigitalIn(mcu::GPIO5, 1, bsp::DIN_LOW); //brakes
static bsp::DigitalIn hvdin3 = bsp::DigitalIn(mcu::GPIO6, 1, bsp::DIN_LOW); //hbrake
static bsp::DigitalIO hvdin3_pull = bsp::DigitalIO(mcu::GPIO7, bsp::DIN_HIGH); //hbrake pull
static bsp::DigitalOut odout1 = bsp::DigitalOut(mcu::GPIO9, 1);
static bsp::DigitalOut odout2 = bsp::DigitalOut(mcu::GPIO10, 1);
static bsp::DigitalOut odout3 = bsp::DigitalOut(mcu::GPIO11, 1);
static bsp::DigitalOut odout4 = bsp::DigitalOut(mcu::GPIO12, 1);
static bsp::DigitalOut odout5 = bsp::DigitalOut(mcu::GPIO13, 1);
static bsp::DigitalOut odout6 = bsp::DigitalOut(mcu::GPIO14, 1);
static bsp::PWMout od_pwm = bsp::PWMout(mcu::PWM1);
void board_init(void);
void board_read(void);
#endif /* BSP_BOARD_H_ */

9
firmware/src/hw/button.cpp
View File

@@ -25,6 +25,8 @@ hw::Button::Button(bsp::DigitalIn* din_ch, uint8_t act_lvl, uint8_t dbnc_lim, ui
this->time = 0;
this->is_new = 0;
this->hold_time = 0;
}
hw::Button::~Button(void)
@@ -40,6 +42,13 @@ uint8_t hw::Button::update(void)
// Increase state counter
this->time = util::sat_add(this->time, 1);
// Repeat new flag after hold time
if((this->state == BUTTON_ON)&&(this->time > this->hold_time)&&(this->hold_time > 0))
{
this->time = 0;
this->is_new = 1;
};
// Determine next state
uint8_t next_state = BUTTON_OFF;
if(lvl==this->act_lvl) next_state = BUTTON_ON;

1
firmware/src/hw/button.h
View File

@@ -26,6 +26,7 @@ class Button
uint16_t time;
uint8_t dbnc_lim;
uint8_t is_new;
uint16_t hold_time;
uint8_t update(void);
uint8_t force_update(void);

41
firmware/src/hw/cc_output.cpp Normal file
View File

@@ -0,0 +1,41 @@
/**** Includes ****/
#include "../utils/utils.h"
#include "cc_output.h"
using namespace hw;
/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/
/**** Public function definitions ****/
hw::CCoutput::CCoutput(bsp::Hafbridge* hbridge, bsp::AnalogIn* supply_u, bsp::AnalogIn* out_u, bsp::AnalogIn* out_i) : CVoutput(hbridge, supply_u)
{
this->out_voltage = out_u;
this->out_currnet = out_i;
this->out_impedance = 0xFFFF;
this->target_voltage = 0;
}
void hw::CCoutput::update(void)
{
// Calculate output impedance
if((this->out_currnet == 0)||(this->out_voltage->last_read == 0)) this->out_impedance = 0xFFFF;
else
{
this->out_impedance = util::sat_div_kilo(this->out_voltage->last_read, this->out_currnet->last_read);
}
// Check target
if((this->target < this->min_out)&&(this->target > 0)) this->target = this->min_out;
// Convert target current to voltage
this->target_voltage = util::sat_mul_kilo(this->target, this->out_impedance);
// Set output
this->hbridge->write(util::sat_ratio(this->target_voltage, this->supply->last_read));
}
/**** Private function definitions ****/

35
firmware/src/hw/cc_output.h Normal file
View File

@@ -0,0 +1,35 @@
#ifndef CONST_CURRENT_OUTPUT_H_
#define CONST_CURRENT_OUTPUT_H_
/**** Includes ****/
#include <stdint.h>
#include "../bsp/ain.h"
#include "cv_output.h"
namespace hw {
/**** Public definitions ****/
class CCoutput : public CVoutput
{
protected:
bsp::AnalogIn* out_voltage;
bsp::AnalogIn* out_currnet;
public:
CCoutput(bsp::Hafbridge* hbridge, bsp::AnalogIn* supply_u, bsp::AnalogIn* out_u, bsp::AnalogIn* out_i);
void update(void);
uint16_t out_impedance;
uint16_t target_voltage;
};
/**** Public function declarations ****/
#ifdef TESTING
#endif
} //namespace
#endif /* CONST_VOLTAGE_OUTPUT_H_ */

0
firmware/src/hw/cv_otput.cpp → firmware/src/hw/cv_output.cpp
View File

59
firmware/src/hw/devices.cpp Normal file
View File

@@ -0,0 +1,59 @@
/**** Includes ****/
#include "../utils/utils.h"
#include "devices.h"
/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/
/**** Public function definitions ****/
void devices_init(void)
{
board_init();
btn_up.hold_time = 1000;
btn_down.hold_time = 1000;
ccout.target = 0;
ccout.min_out = 100;
sup_fuse.hold_current = 6000;
sup_fuse.trip_cycles = 50;
sup_fuse.cooldown_cycles = 1000;
out_fuse.hold_current = 6000;
out_fuse.trip_cycles = 100;
out_fuse.cooldown_cycles = 1000;
hvdin3_pull.write(bsp::DOUT_HIGH);
devices_update_inputs();
display.write(0x00);
display.set_brigthness(100);
ccout.target = 0;
ccout.update();
ccout.enable();
}
void devices_update_inputs(void)
{
board_read();
pot.update();
btn_mode.update();
btn_up.update();
btn_down.update();
sw_dimm.update();
sw_brakes.update();
sw_hbrake.update();
sup_fuse.update();
out_fuse.update();
}
/**** Private function definitions ****/

35
firmware/src/hw/devices.h Normal file
View File

@@ -0,0 +1,35 @@
#ifndef HW_DEVICES_H_
#define HW_DEVICES_H_
/**** Includes ****/
#include <stdint.h>
#include "../bsp/board.h"
#include "button.h"
#include "potentiometer.h"
#include "display_led.h"
#include "cv_output.h"
#include "cc_output.h"
#include "fuse.h"
static hw::Button btn_mode = hw::Button(&din1, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
static hw::Button btn_up = hw::Button(&din4, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
static hw::Button btn_down = hw::Button(&din3, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
static hw::Button sw_dimm = hw::Button(&hvdin1, bsp::DIN_HIGH, 10, hw::BUTTON_OFF);
static hw::Button sw_brakes = hw::Button(&hvdin2, bsp::DIN_HIGH, 10, hw::BUTTON_OFF);
static hw::Button sw_hbrake = hw::Button(&hvdin3, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
static hw::Potentiometer pot = hw::Potentiometer(&ain2, 500, 4500);
static hw::DisplayLed display = hw::DisplayLed(&odout1, &odout2, &odout3, &odout4, &odout5, &odout6, &od_pwm);
static hw::CCoutput ccout = hw::CCoutput(&hbridge, &bat_u, &dccd_u, &dccd_i);
static hw::Fuse sup_fuse = hw::Fuse(&bat_i);
static hw::Fuse out_fuse = hw::Fuse(&dccd_i);
void devices_init(void);
void devices_update_inputs(void);
#endif /* BSP_BOARD_H_ */

1
firmware/src/hw/fuse.cpp
View File

@@ -1,6 +1,5 @@
/**** Includes ****/
#include "../utils/utils.h"
#include "../utils/interpolate.h"
#include "fuse.h"
using namespace hw;

51
firmware/src/logic/button_force.cpp Normal file
View File

@@ -0,0 +1,51 @@
/**** Includes ****/
#include "../utils/utils.h"
#include "button_force.h"
using namespace logic;
/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/
/**** Public function definitions ****/
logic::ButtonForce::ButtonForce(hw::Button* btn_up, hw::Button* btn_down)
{
this->btn_up = btn_up;
this->btn_down = btn_down;
this->force = 0;
this->step = 10;
this->is_new = 0;
}
logic::ButtonForce::~ButtonForce(void)
{
return;
}
uint8_t logic::ButtonForce::update(void)
{
uint8_t next_force = 0;
if((this->btn_up->is_new)&&(this->btn_up->state == hw::BUTTON_ON))
{
next_force = util::sat_add(this->force, this->step);
if(next_force > 100) next_force = 100;
this->btn_up->is_new = 0;
};
if((this->btn_down->is_new)&&(this->btn_down->state == hw::BUTTON_ON))
{
next_force = util::sat_subtract(this->force, this->step);
this->btn_down->is_new = 0;
};
if(next_force != this->force) this->is_new = 1;
this->force = next_force;
return this->force;
}
/**** Private function definitions ****/

36
firmware/src/logic/button_force.h Normal file
View File

@@ -0,0 +1,36 @@
#ifndef BUTTON_FORCE_H_
#define BUTTON_FORCE_H_
/**** Includes ****/
#include <stdint.h>
#include "../hw/button.h"
namespace logic {
/**** Public definitions ****/
class ButtonForce
{
protected:
hw::Button* btn_up;
hw::Button* btn_down;
public:
ButtonForce(hw::Button* btn_up, hw::Button* btn_down);
~ButtonForce(void);
uint8_t force;
uint8_t step;
uint8_t is_new;
uint8_t update(void);
};
/**** Public function declarations ****/
#ifdef TESTING
#endif
} //namespace
#endif /* BUTTON_FORCE_H_ */

79
firmware/src/logic/dccd_force.cpp Normal file
View File

@@ -0,0 +1,79 @@
/**** Includes ****/
#include "../utils/utils.h"
#include "dccd_force.h"
using namespace logic;
/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
/**** Private function declarations ****/
/**** Public function definitions ****/
logic::DccdForce::DccdForce(hw::Button* btn_mode, hw::Button* sw_hbrake, hw::Button* sw_brakes)
{
this->mode = btn_mode;
this->handbrake = sw_hbrake;
this->brakes = sw_brakes;
this->is_new = 0;
this->force = 0;
this->brake_mode = 0;
}
logic::DccdForce::~DccdForce(void)
{
return;
}
uint8_t logic::DccdForce::update(uint8_t user_force)
{
// Process mode button
if((this->mode->is_new)&&(this->mode->state == hw::BUTTON_ON))
{
// Cycle brake mode
switch(this->brake_mode)
{
case 0:
this->brake_mode = 1;
case 1:
this->brake_mode = 2;
default:
this->brake_mode = 0;
}
this->mode->is_new = 0;
this->is_new_bmode = 1;
};
// Determine target force source
uint8_t next_force = user_force;
if(this->handbrake->state == hw::BUTTON_ON)
{
next_force = 0;
}
else if(this->brakes->state == hw::BUTTON_ON)
{
switch(this->brake_mode)
{
case 0:
next_force = 0;
case 1:
next_force = user_force;
default:
next_force = 100;
}
};
if(next_force != this->force) this->is_new = 1;
this->force = next_force;
return this->force;
}
/**** Private function definitions ****/

39
firmware/src/logic/dccd_force.h Normal file
View File

@@ -0,0 +1,39 @@
#ifndef DCCD_FORCE_H_
#define DCCD_FORCE_H_
/**** Includes ****/
#include <stdint.h>
#include "../hw/button.h"
namespace logic {
/**** Public definitions ****/
class DccdForce
{
protected:
hw::Button* mode;
hw::Button* handbrake;
hw::Button* brakes;
public:
DccdForce(hw::Button* btn_mode, hw::Button* sw_hbrake, hw::Button* sw_brakes);
~DccdForce(void);
uint8_t force;
uint8_t is_new;
uint8_t brake_mode;
uint8_t is_new_bmode;
uint8_t update(uint8_t user_force);
};
/**** Public function declarations ****/
#ifdef TESTING
#endif
} //namespace
#endif /* DCCD_FORCE_H_ */

146
firmware/src/main.cpp
View File

@@ -1,66 +1,116 @@
/**** Includes ****/
#include "utils/utils.h"
#include "utils/interpolate.h"
#include "bsp/mcu/mcu_hal.h"
#include "bsp/ain.h"
#include "bsp/din.h"
#include "bsp/dout.h"
#include "bsp/dio.h"
#include "bsp/halfbridge.h"
#include "bsp/pwm.h"
#include "hw/devices.h"
#include "hw/button.h"
#include "hw/potentiometer.h"
#include "hw/display_led.h"
#include "hw/cv_output.h"
#include "logic/button_force.h"
#include "logic/dccd_force.h"
/**** Private definitions ****/
/**** Private constants ****/
/**** Private variables ****/
static bsp::AnalogIn dccd_i(mcu::ADC0);
static bsp::AnalogIn dccd_u(mcu::ADC1);
static bsp::AnalogIn bat_u(mcu::ADC2);
static bsp::AnalogIn bat_i(mcu::ADC3);
static bsp::Hafbridge hbridge(mcu::PWM0, mcu::GPIO15, 95);
static bsp::AnalogIn ain1(mcu::ADC5); // mode
static bsp::AnalogIn ain2(mcu::ADC4); // pot
static bsp::DigitalIn din1(mcu::GPIO0, 0, bsp::DIN_HIGH); //mode
static bsp::DigitalIn din2(mcu::GPIO1, 0, bsp::DIN_HIGH); //pot
static bsp::DigitalIn din3(mcu::GPIO2, 0, bsp::DIN_HIGH); //down
static bsp::DigitalIn din4(mcu::GPIO3, 0, bsp::DIN_HIGH); //up
static bsp::DigitalIn hvdin1(mcu::GPIO4, 1, bsp::DIN_LOW); //dimm
static bsp::DigitalIn hvdin2(mcu::GPIO5, 1, bsp::DIN_LOW); //brakes
static bsp::DigitalIn hvdin3(mcu::GPIO6, 1, bsp::DIN_LOW); //hbrake
static bsp::DigitalIO hvdin3_pull(mcu::GPIO7, bsp::DIN_HIGH); //hbrake pull
static bsp::DigitalOut odout1(mcu::GPIO9, 1);
static bsp::DigitalOut odout2(mcu::GPIO10, 1);
static bsp::DigitalOut odout3(mcu::GPIO11, 1);
static bsp::DigitalOut odout4(mcu::GPIO12, 1);
static bsp::DigitalOut odout5(mcu::GPIO13, 1);
static bsp::DigitalOut odout6(mcu::GPIO14, 1);
static bsp::PWMout od_pwm(mcu::PWM1);
static hw::Button btn_mode(&din1, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
static hw::Button btn_up(&din4, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
static hw::Button btn_down(&din3, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
static hw::Button sw_dimm(&hvdin1, bsp::DIN_HIGH, 10, hw::BUTTON_OFF);
static hw::Button sw_brakes(&hvdin2, bsp::DIN_HIGH, 10, hw::BUTTON_OFF);
static hw::Button sw_hbrake(&hvdin3, bsp::DIN_LOW, 10, hw::BUTTON_OFF);
static hw::Potentiometer pot(&ain2, 500, 4500);
static hw::DisplayLed display(&odout1, &odout2, &odout3, &odout4, &odout5, &odout6, &od_pwm);
static hw::CVoutput cvout(&hbridge, &bat_u);
static logic::ButtonForce button_force = logic::ButtonForce(&btn_up, &btn_down);
static logic::DccdForce dccd_force = logic::DccdForce(&btn_mode, &sw_hbrake, &sw_brakes);
/**** Private function declarations ****/
/**** Public function definitions ****/
int main(void)
{
// HW setup
devices_init();
uint8_t user_force = 0;
button_force.force = mcu::eeprom_read8b(0x0000);
dccd_force.brake_mode = mcu::eeprom_read8b(0x0001);
uint8_t pot_mode = mcu::eeprom_read8b(0x0002);
uint8_t dsp_brigth = mcu::eeprom_read8b(0x0003);
uint8_t dsp_dimm = mcu::eeprom_read8b(0x0004);
uint16_t lock_current = mcu::eeprom_read16b(0x0005);
// Safeguard eeprom values
if(button_force.force > 100)
{
button_force.force = 0;
mcu::eeprom_write8b(0x0000, button_force.force);
};
if(dccd_force.brake_mode > 2)
{
button_force.force = 0;
mcu::eeprom_write8b(0x0001, dccd_force.brake_mode);
};
if(pot_mode > 1)
{
pot_mode = 0;
mcu::eeprom_write8b(0x0002, pot_mode);
};
if(dsp_brigth > 100)
{
dsp_brigth = 100;
mcu::eeprom_write8b(0x0003, dsp_brigth);
};
if(dsp_dimm > 100)
{
dsp_dimm = 50;
mcu::eeprom_write8b(0x0004, dsp_dimm);
};
if(lock_current > 6000)
{
lock_current = 4500;
mcu::eeprom_write16b(0x0005, lock_current);
};
// Super loop
while(1)
{
// Update inputs
devices_update_inputs();
// Update user setting
button_force.update();
// Select user force input
if(pot_mode) user_force = pot.percent;
else user_force = button_force.force;
// Calculate next target force
dccd_force.update(user_force);
// Override force in case of fault
if((sup_fuse.fault)||(out_fuse.fault)) dccd_force.force = 0;
// Convert force to current
ccout.target = util::percent_of(dccd_force.force, lock_current);
// Execute outputs
ccout.update();
// Set display
display.show_percent(dccd_force.force, hw::DisplayLed::LED_DSP_DOT10);
// Process dimm
if(sw_dimm.state == hw::BUTTON_ON) display.set_brigthness(dsp_dimm);
else display.set_brigthness(dsp_brigth);
// Save user setting changes
if(button_force.is_new)
{
mcu::eeprom_write8b(0x0000, button_force.force);
button_force.is_new = 0;
};
if(dccd_force.is_new_bmode)
{
mcu::eeprom_write8b(0x0001, dccd_force.brake_mode);
dccd_force.is_new_bmode = 0;
};
continue; // End of super loop
}
@@ -68,4 +118,4 @@ int main(void)
return 0;
}
/**** Private function definitions ***/
/**** Private function definitions ***/

33
firmware/src/uDCCD.cppproj
View File

@@ -159,6 +159,12 @@
<Compile Include="bsp\ain.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="bsp\board.cpp">
<SubType>compile</SubType>
</Compile>
<Compile Include="bsp\board.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="bsp\din.cpp">
<SubType>compile</SubType>
</Compile>
@@ -201,7 +207,13 @@
<Compile Include="hw\button.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="hw\cv_otput.cpp">
<Compile Include="hw\cc_output.cpp">
<SubType>compile</SubType>
</Compile>
<Compile Include="hw\cc_output.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="hw\cv_output.cpp">
<SubType>compile</SubType>
</Compile>
<Compile Include="hw\cv_output.h">
@@ -219,12 +231,30 @@
<Compile Include="hw\fuse.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="hw\devices.cpp">
<SubType>compile</SubType>
</Compile>
<Compile Include="hw\devices.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="hw\potentiometer.cpp">
<SubType>compile</SubType>
</Compile>
<Compile Include="hw\potentiometer.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="logic\button_force.cpp">
<SubType>compile</SubType>
</Compile>
<Compile Include="logic\button_force.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="logic\dccd_force.cpp">
<SubType>compile</SubType>
</Compile>
<Compile Include="logic\dccd_force.h">
<SubType>compile</SubType>
</Compile>
<Compile Include="main.cpp">
<SubType>compile</SubType>
</Compile>
@@ -245,6 +275,7 @@
<Folder Include="bsp" />
<Folder Include="bsp\mcu" />
<Folder Include="hw" />
<Folder Include="logic" />
<Folder Include="utils" />
</ItemGroup>
<Import Project="$(AVRSTUDIO_EXE_PATH)\\Vs\\Compiler.targets" />

6
firmware/src/utils/interpolate.cpp
View File

@@ -131,7 +131,7 @@ uint16_t util::interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t
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)
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;
@@ -144,7 +144,7 @@ uint16_t interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t
return util::sat_cast(y);
}
uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis)
uint8_t util::find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis)
{
for(uint8_t i=0; i<len_axis; i++)
{
@@ -155,7 +155,7 @@ uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len
return len_axis;
}
uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x)
uint16_t util::index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x)
{
return ((uint16_t)len_x * iy) + ix;
}

9
firmware/src/utils/utils.cpp
View File

@@ -152,4 +152,13 @@ uint16_t util::percent_to_16b(uint8_t percent)
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 ****/

2
firmware/src/utils/utils.h
View File

@@ -19,6 +19,8 @@ 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);